gogojjh
commented
2 years ago Hi, thanks for your interest. This function implements the extrinsics initialization based on the planar motion assumption (for the case that you need to calibrate multiple lidars for a vehicle). Please check methodology-initialization section of our IROS2019 paper:``Automatic Calibration of Multiple 3D LiDARs in Urban Environments'' or the slide for the explanation. This paper the the preliminary version of our TRO paper. Please feel free to contact me if you have any questions.
Richardson-Chong
Thx for your great work and the code in function calibExTranslationPlanar is vague that the paper didn't mention. Can you explain the code with formula?
const Eigen::Quaterniond &q_yx = calib_ext_[idx_data].q_; Eigen::MatrixXd G = Eigen::MatrixXd::Zero(v_pose_.size() * 2, 4); Eigen::MatrixXd w = Eigen::MatrixXd::Zero(v_pose_.size() * 2, 1); for (size_t i = 0; i < v_pose_.size(); i++) { const Pose &pose_ref = v_pose_[i][idx_ref]; const Pose &pose_data = v_pose_[i][idx_data]; AngleAxisd ang_axis_ref(pose_ref.q_); AngleAxisd ang_axis_data(pose_data.q_); double t_dis = abs(pose_ref.t_.dot(ang_axis_ref.axis()) - pose_data.t_.dot(ang_axis_data.axis())); double huber = t_dis > 0.04 ? 0.04 / t_dis : 1.0; Eigen::Matrix2d J = pose_ref.q_.toRotationMatrix().block<2,2>(0, 0) - Eigen::Matrix2d::Identity(); Eigen::Vector3d n = q_yx.toRotationMatrix().row(2); Eigen::Vector3d p = q_yx * (pose_data.t_ - pose_data.t_.dot(n) * n); Eigen::Matrix2d K; K << p(0), -p(1), p(1), p(0); G.block<2, 2>(i * 2, 0) = huber * J; G.block<2, 2>(i * 2, 2) = huber * K; w.block<2, 1>(i * 2, 0) = pose_ref.t_.block<2,1>(0,0); } Eigen::Vector4d m = G.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(w); Eigen::Vector3d x(-m(0), -m(1), 0); double yaw = atan2(m(3), m(2)); Eigen::Quaterniond q_z(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); calib_ext_[idx_data] = Pose(q_z*q_yx, x); return true;