Open ZikangYuan opened 2 years ago
Dear @ZikangYuan , Did you find out the reason why parameters cannot be updated normally and are always identity matrix? I am trying to implement analytical Jacobians and facing similar thing. Thank you in advance!
Hi,
I found you used auto derivation of CERES when minimizing "CTPointToPlaneFactor" residuals. However, when I try to use analytic derivation instead of auto derivation, I found the variables (e.g., pose_begin and pose_end) cannot be updated normally and are always identity matrix. Have you successfully used analytic derivation for CT-ICP? I doubt the analytic derivation of CERES may not be applicable to your "CTPointToPlaneFactor".
Heres is my code for the analytic derivation of "CTPointToPlaneFactor":
class CTPointToPlaneFactor : public ceres::SizedCostFunction<1, 7, 7> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CTPointToPlaneFactor(const Eigen::Vector3d &reference_point_, const Eigen::Vector3d &raw_keypoint_, const Eigen::Vector3d &reference_normal_, double alpha_time_, double weight_ = 1.0); virtual ~CTPointToPlaneFactor() {} virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; Eigen::Vector3d raw_keypoint; Eigen::Vector3d reference_point; Eigen::Vector3d reference_normal; double alpha_time; double weight = 1.0; }; CTPointToPlaneFactor::CTPointToPlaneFactor(const Eigen::Vector3d &reference_point_, const Eigen::Vector3d &raw_keypoint_, const Eigen::Vector3d &reference_normal_, double alpha_time_, double weight_) { raw_keypoint = raw_keypoint_; reference_point = reference_point_; reference_normal = reference_normal_; alpha_time = alpha_time_; weight = weight_; } bool CTPointToPlaneFactor::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { const Eigen::Quaterniond rot_begin(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); const Eigen::Quaterniond rot_end(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); const Eigen::Vector3d tran_begin(parameters[0][0], parameters[0][1], parameters[0][2]); const Eigen::Vector3d tran_end(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond rot_slerp = rot_begin.slerp(alpha_time, rot_end); rot_slerp.normalize(); Eigen::Vector3d tran_slerp = tran_begin * (1 - alpha_time) + tran_end * alpha_time; Eigen::Vector3d point_world = rot_slerp * raw_keypoint + tran_slerp; double distance = weight * (reference_point - point_world).transpose() * reference_normal; residuals[0] = distance; if (jacobians) { if (jacobians[0]) { Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor>> jacobian_pose_begin(jacobians[0]); jacobian_pose_begin.setZero(); jacobian_pose_begin.block<1, 3>(0, 3) = - reference_normal.transpose() * rot_begin.toRotationMatrix() * numType::skewSymmetric(raw_keypoint) * weight * (1 - alpha_time); jacobian_pose_begin.block<1, 3>(0, 0) = reference_normal.transpose() * weight * (1 - alpha_time); } if (jacobians[1]) { Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor>> jacobian_pose_end(jacobians[1]); jacobian_pose_end.setZero(); jacobian_pose_end.block<1, 3>(0, 3) = - reference_normal.transpose() * rot_end.toRotationMatrix() * numType::skewSymmetric(raw_keypoint) * weight * alpha_time; jacobian_pose_end.block<1, 3>(0, 0) = reference_normal.transpose() * weight * alpha_time; } } return true; }
@ZikangYuan @KaziiBotashev Maybe there's a minus sign missing from the derivation;d(r)/d(point_world) = -reference_normal;
Hi,
I found you used auto derivation of CERES when minimizing "CTPointToPlaneFactor" residuals. However, when I try to use analytic derivation instead of auto derivation, I found the variables (e.g., pose_begin and pose_end) cannot be updated normally and are always identity matrix. Have you successfully used analytic derivation for CT-ICP? I doubt the analytic derivation of CERES may not be applicable to your "CTPointToPlaneFactor".
Heres is my code for the analytic derivation of "CTPointToPlaneFactor":