some questions about calEdgeReliabilityInTCFS()

[narutojxl]

Hello author,
First of all, please forgive my nagging. I hope i won't make you feel bored.

• IdM_1's rotation is identity, its translation is - rotation_element.rot_origin, calculated in rot_element.rot_origin = vec_first_t;, namely -P_j. IdM_2 is R_z. In paper, p_zk = Rz * p_k, q_k' = transform * q_k; q_zk = Rz * q_k'. There is no IdM_1 variable in the paper. Q1: What's the meaning of IdM_1?

IdM_1.block<3, 1>(0, 3) = -1.0 * origin;
IdM_2.block<3, 3>(0, 0) = twoVectorsAlign(axis, z_axis);


Eigen::Matrix4f local_tm_t = IdM_2*IdM_1;
Eigen::Matrix4f local_tm_s = local_tm_t*transform.matrix();


pcl::transformPointCloud(*key_source_, *source_local, local_tm_s);
pcl::transformPointCloud(*key_target_, *target_local, local_tm_t);

• Q2: I do not totally understand the func vl_fast_atan2_f(), why not directly use atan2() to calculate the coordinate azimuth, is there something we need to consider?
• Q3: The relationship of R(theta, epsilon_PQ_ij), R(theta, Z) and Rz is R(theta, epsilon_PQ_ij) = R(theta, Z) * Rz ?
• Q4: Related to Q1, the final translation t's calculation seems is different from eq(18) of paper, also confused this.

Eigen::Matrix4f IdM_1 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f IdM_2 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f IdM_3 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot = Eigen::AngleAxisf(best_angle, two_point_rot_ele.rot_axis).toRotationMatrix();
IdM_1.block<3, 1>(0, 3) = -1.0*two_point_rot_ele.rot_origin;
IdM_2.block<3, 3>(0, 0) = rot;
IdM_3.block<3, 1>(0, 3) = two_point_rot_ele.rot_origin;
Eigen::Matrix4f gr_tran_mat = IdM_3*IdM_2*IdM_1*two_point_tran_mat;

• BTW, in code best_count_ = der_in_tcfs;, in paper pseudo code line20 of "Algorithm 1" is best_count_ = der_in_tcfs + 2; I don't understand why plus 2?

Thanks for author's patience and time in advance sincerely!