Fail to use CERES Analytic Derivation for CT-ICP

[Zikang Yuan]

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;
}

[Kaziy Botashev]

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!

[chengwei]

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;