MCPcopy Create free account
hub / github.com/PJLab-ADG/Livox-Mapping / fastAlignUsingVkey

Method fastAlignUsingVkey

SC-PGO/include/scancontext/Scancontext.cpp:93–113  ·  view source on GitHub ↗

Source from the content-addressed store, hash-verified

91
92
93int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)
94{
95 int argmin_vkey_shift = 0;
96 double min_veky_diff_norm = 10000000;
97 for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
98 {
99 MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
100
101 MatrixXd vkey_diff = _vkey1 - vkey2_shifted;
102
103 double cur_diff_norm = vkey_diff.norm();
104 if( cur_diff_norm < min_veky_diff_norm )
105 {
106 argmin_vkey_shift = shift_idx;
107 min_veky_diff_norm = cur_diff_norm;
108 }
109 }
110
111 return argmin_vkey_shift;
112
113} // fastAlignUsingVkey
114
115
116std::pair<double, int> SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )

Callers

nothing calls this directly

Calls 1

circshiftFunction · 0.85

Tested by

no test coverage detected