输出Ceres自动求导的雅克比

原创已于 2022-09-04 19:39:20 修改 · 746 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #自动驾驶 #矩阵

于 2022-09-04 19:06:25 首次发布

本文介绍了如何利用Ceres Solver的自动求导功能计算函数的雅克比矩阵，以SLAM问题中曲线拟合为例，展示了如何构建代价函数并计算指定数据点的雅克比。通过比较手动计算和Ceres自动求导的结果，验证了自动求导的正确性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

输出Ceres自动求导的雅克比

ceres的自动求导是一个非常方便的工具，但为了保证实时性，有时需要自己求雅克比，可以用ceres自动求导求出的雅克比来验证自己求的雅克比对比，验证是否正确。ceres提供了相应的接口，对应的函数为：bool CostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const。

下面演示一下怎么用，以slam十四讲中的函数为例，相信对这个函数都很熟
$y=exp(ax^2+bx+c)$
首先构建代价函数的计算模型，代码参考slam十四讲

struct CURVE_FITTING_COST
{
        CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}
        // 残差的计算
        template <typename T>
        bool operator()(
            const T *const abc, // 模型参数，有3维
            T *residual) const  // 残差
        {
                residual[0] = (T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2])); // y-exp(ax^2+bx+c)
                return true;
        }
        const double _x, _y; // x,y数据
};

为了便于演示，对十四讲中的代码做了一些修改，只计算（x_data[10]，y_data[10]）这个点的雅克比

int main(int argc, char **argv)
{
        double a = 1.0, b = 2.0, c = 1.0; // 真实参数值
        int N = 100;                      // 数据点
        double w_sigma = 1.0;             // 噪声Sigma值
        cv::RNG rng;                      // OpenCV随机数产生器
        double abc[3] = {0, 0, 0};        // abc参数的估计值
        vector<double> x_data, y_data; // 数据

        N = 100;
        for (int i = 0; i < N; i++)
        {
                double x = i / 100.0;
                x_data.push_back(x);
                y_data.push_back(
                    exp(a * x * x + b * x + c) + rng.gaussian(w_sigma));
        }
		//这里只传入x_data[10]，y_data[10]
        CURVE_FITTING_COST *errfuc = new CURVE_FITTING_COST(x_data[10], y_data[10]);
        ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> errdiff(errfuc);
        int res_num = 1; //残差的数量
        int param_num = 3; //参数的数量
        double **jac = 0;
        jac = new double *[res_num];
        jac[0] = new double[param_num];
        for (int j = 0; j < param_num; ++j)
        {
                jac[0][j] = 0;
        }
        double res[] = {0};
        const double *const param[] = {abc};
        errdiff.Evaluate((const double *const *)param, res, (double **)jac);
        cout << "err: " << res[0] << endl;
        for (int j = 0; j < param_num; ++j)
        {
                cout << jac[0][j] << "   ";
        }
        cout << endl;
        return 0;
}