-
Notifications
You must be signed in to change notification settings - Fork 212
Open
Description
First of all, I would like to thank the original author for his creative design. I am very interested in the degradation or significance evaluation, and I think this is the key method to imporve the robustness of SLAM.
However, I don't know if anyone has noticed that the eigen decomposition method in the code uses EigenSolver<M3D> es(Hessian_rot); in LI_init.cpp line 517. Will this method lead to disordered eigenvalues? In other words, the decomposed eigenvalues do not seem to be consistent with Hessian_rot, so is the state order he discussed really valid? I hope someone can answer this. Thanks!
Full code slice shows as follow:
M3D Hessian_rot = Jacobian_rot.transpose() * Jacobian_rot;
EigenSolver<M3D> es(Hessian_rot);
V3D EigenValue = es.eigenvalues().real();
M3D EigenVec_mat = es.eigenvectors().real();
M3D EigenMatCwise = EigenVec_mat.cwiseProduct(EigenVec_mat);
std::vector<double> EigenMat_1_col{EigenMatCwise(0, 0), EigenMatCwise(1, 0), EigenMatCwise(2, 0)};
std::vector<double> EigenMat_2_col{EigenMatCwise(0, 1), EigenMatCwise(1, 1), EigenMatCwise(2, 1)};
std::vector<double> EigenMat_3_col{EigenMatCwise(0, 2), EigenMatCwise(1, 2), EigenMatCwise(2, 2)};
int maxPos[3] = {0};
maxPos[0] = max_element(EigenMat_1_col.begin(), EigenMat_1_col.end()) - EigenMat_1_col.begin();
maxPos[1] = max_element(EigenMat_2_col.begin(), EigenMat_2_col.end()) - EigenMat_2_col.begin();
maxPos[2] = max_element(EigenMat_3_col.begin(), EigenMat_3_col.end()) - EigenMat_3_col.begin();
V3D Scaled_Eigen = EigenValue / data_accum_length; //the larger data_accum_length is, the more data is needed
V3D Rot_percent(Scaled_Eigen[1] * Scaled_Eigen[2],
Scaled_Eigen[0] * Scaled_Eigen[2],
Scaled_Eigen[0] * Scaled_Eigen[1]);
V3D Rot_percent_scaled(Rot_percent[0] < 0.99 ? Rot_percent[0] : 1,
Rot_percent[1] < 0.99 ? Rot_percent[1] : 1,
Rot_percent[2] < 0.99 ? Rot_percent[2] : 1);
int axis[3];
axis[2] = max_element(maxPos, maxPos + 3) - maxPos;
axis[0] = min_element(maxPos, maxPos + 3) - maxPos;
axis[1] = 3 - (axis[0] + axis[2]);
clear(); //clear the screen
printf("\033[3A\r");
printProgress(Rot_percent_scaled[axis[0]], 88);
printProgress(Rot_percent_scaled[axis[1]], 89);
printProgress(Rot_percent_scaled[axis[2]], 90);
Reactions are currently unavailable
Metadata
Metadata
Assignees
Labels
No labels