Material about function ImuInitializer::EstimateExtrinsicRotation

[icameling]

Thank yo so much to update your code. I am confused about this function. Why is 'svd.matrixV().col(3)' equal to the extrinsic rotation between Lidar and IMU? Is there some supplementary material about this? Thank you so much.

bool ImuInitializer::EstimateExtrinsicRotation(CircularBuffer<PairTimeLaserTransform> &all_laser_transforms,
                                               Transform &transform_lb) {

  Transform transform_bl = transform_lb.inverse();
  Eigen::Quaterniond rot_bl = transform_bl.rot.template cast<double>();

  size_t window_size = all_laser_transforms.size() - 1;

  Eigen::MatrixXd A(window_size * 4, 4);

  for (size_t i = 0; i < window_size; ++i) {
    PairTimeLaserTransform &laser_trans_i = all_laser_transforms[i];
    PairTimeLaserTransform &laser_trans_j = all_laser_transforms[i + 1];

    Eigen::Quaterniond delta_qij_imu = laser_trans_j.second.pre_integration->delta_q_;

    Eigen::Quaterniond delta_qij_laser
        = (laser_trans_i.second.transform.rot.conjugate() * laser_trans_j.second.transform.rot).template cast<double>();

    Eigen::Quaterniond delta_qij_laser_from_imu = rot_bl.conjugate() * delta_qij_imu * rot_bl;

    double angular_distance = 180 / M_PI * delta_qij_laser.angularDistance(delta_qij_laser_from_imu);

//    DLOG(INFO) << i << ", " << angular_distance;

    double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;

    Eigen::Matrix4d lq_mat = LeftQuatMatrix(delta_qij_laser);
    Eigen::Matrix4d rq_mat = RightQuatMatrix(delta_qij_imu);

    A.block<4, 4>(i * 4, 0) = huber * (lq_mat - rq_mat);
  }

//  DLOG(INFO) << ">>>>>>> A <<<<<<<" << endl << A;

  Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<double, 4, 1> x = svd.matrixV().col(3);
  Quaterniond estimated_qlb(x);

  transform_lb.rot = estimated_qlb.cast<float>().toRotationMatrix();

  Vector3d cov = svd.singularValues().tail<3>();

  // NOTE: covariance 0.25
  if (cov(1) > 0.25) {
    return true;
  } else {
    return false;
  }
}

[hyye]

Hi @icameling, you may find equation (6) in this paper helpful, and similarly the extrinsic parameter initialization in VINS-mono. The SVD is used for solving the final Ax = 0 problem.

[icameling]

This paper is really helpful. Thanks a lot !

[narutojxl]

Hi @hyye, thanks for your paper first. I found in the reference paper, the quaternion convention is JPL .

• The sign of skew-symmetric in left q_L and q_R is different from in this paper, equ 9. and equ 10., is it a typo in code?
• The solution of Ax=0 is JPL , do we need to put real part first and then imaginary part when initializing the Eigen quaternion from a vector4d in code? Thanks a lot!