find corresponding features about lili-om

[narutojxl]

Hi @kailaili and all,

• I'm very sorry for the Q1 question. I mistake the vector<int> keyframe_idx;, its content begins from 1, not 0. Sorry again !
• For Q2 question, i give some my reasons about the guess. If there are some mistakes, please tell me, look forward to disscuss with you.
  

  lili-om/LiLi-OM-ROT/include/factors/LidarKeyframeFactor.h

  Lines 28 to 54 in 97b6352

  	bool operator()(const T *t, const T *q, T *residual) const
  	{
  	Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
  	Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
  	Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
  	Eigen::Quaternion<T> q_last_curr{q[0], q[1], q[2], q[3]};
  	Eigen::Matrix<T, 3, 1> t_last_curr{t[0], t[1], t[2]};
  	Eigen::Quaternion<T> q_l_b{T(qlb.w()), T(qlb.x()), T(qlb.y()), T(qlb.z())};
  	Eigen::Matrix<T, 3, 1> t_l_b{T(tlb.x()), T(tlb.y()), T(tlb.z())};
  	Eigen::Matrix<T, 3, 1> lp;
  	Eigen::Matrix<T, 3, 1> p_w;
  	p_w = q_l_b.inverse() * (cp - t_l_b);
  	lp = q_last_curr * cp + t_last_curr;
  	Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
  	Eigen::Matrix<T, 3, 1> de = lpa - lpb;
  	residual[0] = nu.norm() / de.norm();
  	residual[0] *= T(s);
  	return true;
  	}

  • cp point is laser's point in current laser's frame. q and t is the corresponding time the imu's pose in the global frame map, namely the imu0 frame. we use the extrinsic qlb and tlb between laser and imu, transform cp into imu's frame, got p_w. then we use q and t transform p_w into map frame. lpa and lpb are the corresponding edge features in map. Is it a typo?
  • I notice that calculate surf points's residual, we use the point_w, not the cp point.
    

    lili-om/LiLi-OM-ROT/include/factors/LidarKeyframeFactor.h

    Lines 89 to 104 in 97b6352

    	bool operator()(const T *t, const T *q, T *residual) const
    	{
    	Eigen::Quaternion<T> q_w_curr{q[0], q[1], q[2], q[3]};
    	Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
    	Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
    	Eigen::Matrix<T, 3, 1> point_w;
    	Eigen::Quaternion<T> q_l_b{T(qlb.w()), T(qlb.x()), T(qlb.y()), T(qlb.z())};
    	Eigen::Matrix<T, 3, 1> t_l_b{T(tlb.x()), T(tlb.y()), T(tlb.z())};
    	point_w = q_l_b.inverse() * (cp - t_l_b);
    	point_w = q_w_curr * point_w + t_w_curr;
    	Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
    	residual[0] = T(score) * (norm.dot(point_w) + T(negative_OA_dot_norm));
    	return true;
    	}

[limeng1523]

Hi @narutojxl, thanks for your interest in this work. I'm sorry to reply to you so late. I checked the code, and you are right about Q2 question. This is a typo. It should be p_w instead of cp. These large residuals are suppressed by the Cauchy kernel function, so I didn't find this typo. I'm really sorry to make this typo, and thanks for pointing it out. I have fixed it, and we will update the code soon. If you have other questions, welcome to discuss with us.

[narutojxl]

@limeng1523, thanks very much ! My problen is solved.