使用ceres solver 计算仿函数的雅克比

//本文通过相机投影坐标对三维点和旋转平移的导数计算，展示ceres的自动微分功能

//具体理论推导参考高博博客 视觉SLAM中的数学基础 第四篇 李群与李代数（2） - 半闲居士 - 博客园

#include <ceres/ceres.h>
#include <ceres/rotation.h>


struct _3D_Error_cst {
    
    double mx;
    double my;
    _3D_Error_cst(double _x,double _y)
        : mx(_x),my(_y)
    {
    }

    template <typename T>
    bool operator()(const T* const point, const T *camera, T* residuals) const
    {
        //for (int i = 0; i < 6; ++i)  camera[i] = T(0);
        
        T p[3];
        ceres::AngleAxisRotatePoint(camera, point, p);
        p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
        T xp = p[0] / p[2];
        T yp = p[1] / p[2];

        //T camInt[9];
        //for (int i = 0; i < 9; ++i)  camInt[i] = T(0);
        //camInt[0] = T(1);
        //camInt[1] = T(1);
        //camInt[2] = T(0);
        //camInt[3] = T(0);
        //const T& l1 = camInt[4];
        //const T& l2 = camInt[5];
        //const T& p1 = camInt[6];
        //const T& p2 = camInt[7];
        //const T& l3 = camInt[8];

        //T r2 = xp * xp + yp * yp;
        //T distortionx = xp * (T(1.0) + r2 * (l1 + l2 * r2 + l3 * r2*r2)) + T(2)*p1*xp*yp + p2 * (r2 + T(2)*xp*xp);
        //T distortiony = yp * (T(1.0) + r2 * (l1 + l2 * r2 + l3 * r2*r2)) + T(2)*p2*xp*yp + p1 * (r2 + T(2)*yp*yp);
        // //Compute final projected point position.
        //const T& focalx = camInt[0];
        //const T& focaly = camInt[1];
        //const T& cx = camInt[2];
        //const T& cy = camInt[3];
        //T predicted_x = focalx * distortionx + cx;
        //T predicted_y = focaly * distortiony + cy;

         The error is the difference between the predicted and observed position.
        //residuals[0] = predicted_x - T(mx);
        //residuals[1] = predicted_y - T(my);

        residuals[0] = xp - T(mx);
        residuals[1] = yp - T(my);
        return true;

    }
};


Eigen::Matrix3d Skew(const Eigen::Vector3d& v)
{
    Eigen::Matrix3d ret;
    ret <<
        0, -v(2), v(1),
        v(2), 0, -v(0),
        -v(1), v(0), 0;

    return ret;
}

Eigen::Vector3d rot_vec_by_ceres(const double* v,const double* rArr) {
    double p[3];
    ceres::AngleAxisRotatePoint(rArr, v, p);
    return Eigen::Vector3d(p[0],p[1],p[2]);
}

void jacubfunc(const double * p3d,const  double *rt) 
{
    Eigen::Vector3d W(p3d[0], p3d[1], p3d[2]);
    Eigen::Vector3d rotV(rt[0], rt[1], rt[2]);
    Eigen::Vector3d traV(rt[3], rt[4], rt[5]);
    Eigen::AngleAxisd rotM(rotV.norm(), rotV.normalized());

    //auto rX = rotM * W;
    auto rX = rot_vec_by_ceres(p3d, rt);
    Eigen::Vector3d X = rX + traV;

    Eigen::Matrix<double, 2, 3> Juv_W;
    Eigen::Matrix<double, 2, 3> Juv_X;
    Eigen::Matrix<double, 2, 6> Juv_RT;
    Eigen::Matrix<double, 3, 6> JX_RT;
    double x = X[0];
    double y = X[1];
    double z = X[2];

    Juv_X << 1 / z, 0,    -x / (z*z),
            0,     1 / z, -y / (z*z);

    Juv_W = Juv_X * rotM.toRotationMatrix();

    JX_RT.block<3, 3>(0, 0) = -Skew(rX);
    JX_RT.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity();

    Juv_RT = Juv_X * JX_RT;

    std::cout << "juv/W" << "\n" << Juv_W << "\n";
    std::cout << "juv/rt" << "\n" << Juv_RT << "\n";
    std::cout << "err:" << x/z << "," << y/z << "\n";
}


int main() {

    _3D_Error_cst* errfuc1 = new _3D_Error_cst(0,0);
    
    ceres::AutoDiffCostFunction<_3D_Error_cst, 2, 3,6> errdiff(errfuc1);

    const double cam_r_t[6] = { 0.1,0,0,-0.5,0.6,1 };
    const double  p3d[3] = { 1,1,5 };
    double res[] = { 0,0 };
    
    jacubfunc(p3d,cam_r_t);
    //errfuc1(p3d, res);
    
    constexpr int M =2;
    constexpr int N1 =2 * 3;
    constexpr int N2 = 2 * 6;

    double** jac = 0;
    jac = new double*[M];
    
    jac[0] = new double[N1];
    for (int j = 0; j < N1; ++j) {
        jac[0][j] = 0;
    }
    jac[1] = new double[N2];
    for (int j = 0; j < N2; ++j) {
        jac[1][j] = 0;
    }

    const double* const  ParaBlk[] = { p3d,cam_r_t };
    //double   ParaBlk[1][3];
    //for (int i = 0; i < 3; ++i) ParaBlk[0][i] = p3d[i];
    
    errdiff.Evaluate((const double* const*)ParaBlk, res, (double**)jac);
    printf("err:%lf,%lf\n", res[0], res[1]);
    if (jac) {
        for (int j = 0; j < N1; ++j) {
            if (j == N1 / M)printf("\n");
            printf(" %lf ",jac[0][j]);
        }
        printf("\n");
        for (int j = 0; j < N2; ++j) {
            if (j == N2 / M)printf("\n");
            printf(" %lf ", jac[1][j]);
        }
        printf("\n");
    }

    system("pause");
    return 0;
}

节可以参考高博的slam教程 直接法 - 半闲居士 - 博客园