ceres Analytic Derivatives

ceres Analytic Derivatives

ceres是一个非线性最小二乘的库
其中主要求解方式有Analytic Derivatives，AutoDerivatives & Numeric Derivatives这三种。AutoDerivatives最常用也最方便使用，Analytic Derivatives在雅各比矩阵计算上可能会快于AutoDerivatives，所以在某些地方可以使用Analytic Derivatives来加速优化过程。

Analytic Derivatives需要完成ceres::SizedCostFunction中Evaluate的定义
其中，SizedCostFunction声明为

这里<>中的第一个参数为残差的数量（残差可以是一个向量），后面的int表示参数块的大小，可以输入多个参数块（比如多个数组或者vector等），然后在AddResidualBlock（）时，输入的参数的大小要和<>中的一致

Evaluate的函数声明为：

这里parameters为参数块，由于参数块可以输入多个，所以他是一个指向double指针的指针，residulas为残差，最多是一位向量，所以是一个指向double的指针。雅各比矩阵显然是一个二维的（如果有m维残差向量，n维参数向量，那么雅各比是一个mxn的矩阵），这里重点是雅各比是如何存储的

比如现在有两个参数块，分别是vector p1, vector p2，大小分别为3，2
p1[0]在parameters[0][0]中，p1[1]在parameters[0][1]，p1[2]在parameters[0][2]中
p2[0]在parameters[1][0]中，p2[1]在parameters[1][1]中

然后残差是二维的r1 在residuals[0]中，r2在residuals[1]中

重点为雅各比矩阵存储，第i个参数块都放在parameters[i]指向的double数组中，所有残差依次对第i个参数块中所有参数求导的结果都依次放在jacobians[i]中

这里，c为parameters[I]中的第c个参数
延用上面的例子，
jacobians[0][0]存放r1对p1[0]求导
jacobians[0][1]存放r1对p1[1]求导
jacobians[0][2]存放r1对p1[2]求导
jacobians[0][3]存放r2对p1[0]求导
jacobians[0][4]存放r2对p1[1]求导
jacobians[0][5]存放r2对p1[2]求导

这里给出两个SizedCostFunction为多维的情况的代码，一个是slambook2中的位姿估计的改写，一个是曲线拟合

下面是slam十四讲中ch7，用ceres Analytic Derivatives对pose_estimation_3d2d的修改，放了SizedCostFunction

class pnp_optimization : public ceres::SizedCostFunction<2, 6>{
public:
    pnp_optimization(const Eigen::Vector3d &point3d, const Eigen::Vector2d &point2d, const Eigen::Matrix3d &K) :
            _point3d(point3d), _point2d(point2d), Kk(K) {}
            
    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const {
        typedef Eigen::Matrix<double, 6, 1> vector6d;
        vector6d se3;
        se3 << parameters[0][0], parameters[0][1], parameters[0][2], parameters[0][3],parameters[0][4], parameters[0][5];
        Sophus::SE3d T = Sophus::SE3d::exp(se3);
        double fx = Kk(0, 0);
        double fy = Kk(1, 1);
        double cx = Kk(0, 2);
        double cy = Kk(1, 2);
        Eigen::Vector3d pc = T * _point3d;
        Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
        Eigen::Vector2d e = _point2d - proj;

        residuals[0] = e[0];
        residuals[1] = e[1];

        //cout << residuals[0] << " " << residuals[1] << endl;

        if (jacobians != nullptr && jacobians[0] != nullptr){
            double inv_z = 1.0 / pc[2];
            double inv_z2 = inv_z * inv_z;

            jacobians[0][0] = -fx * inv_z;
            jacobians[0][1] = 0;
            jacobians[0][2] = fx * pc[0] * inv_z2;
            jacobians[0][3] = fx * pc[0] * pc[1] * inv_z2;
            jacobians[0][4] = -fx + fx * pc[0] * pc[0] * inv_z2;
            jacobians[0][5] = fx * pc[1] * inv_z;
            jacobians[0][6] = 0;
            jacobians[0][7] = -fy * inv_z;
            jacobians[0][8] = fy * pc[1] * inv_z2;
            jacobians[0][9] = fy + fy * pc[1] * pc[1] * inv_z2;
            jacobians[0][10] = -fy * pc[0] * pc[1] * inv_z2;
            jacobians[0][11] = -fy * pc[0] * inv_z;
        }
        return true;
    }
private:
    const Eigen::Vector3d _point3d;
    const Eigen::Vector2d _point2d;
    const Eigen::Matrix3d Kk;
};

下面是自己写的一个曲线拟合的例子，sizedcostfuction为2x2

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

#define DATA_LEN 100

using namespace std;
float getRand();

class test : public ceres::SizedCostFunction<2,2> {
public:
    test (const double x, const vector<double> y) : x_(x), y_(y) {}
    virtual bool Evaluate (double const* const* parameters,
                           double* residuals,
                           double** jacobians) const {
        double a, b;
        a = parameters[0][0];
        b = parameters[0][1];

        residuals[0] = y_[0] - a*x_*x_ - b*x_;
        residuals[1] = y_[1] - a*x_ - b;

        if (jacobians != nullptr) {
            if (jacobians[0] != nullptr) {
                jacobians[0][0] = -x_*x_;
                jacobians[0][1] = -x_;
                jacobians[0][2] = -x_;
                jacobians[0][3] = 0;
                //printf("%lf, %lf, %lf, %lf\n", jacobians[0][0], jacobians[0][1], jacobians[0][2], jacobians[0][3]);
            }
        }
    }

private:
    const double x_;
    const vector<double> y_;
};

int main() {
    double para[2];
    vector<vector<double>> y_true(DATA_LEN, vector<double>(2, 0));
    vector<vector<double>> y_in(DATA_LEN, vector<double>(2, 0));

    para[0] = 2;
    para[1] = 0.5;
    for (int i = 0; i < DATA_LEN; i++) {
        //x = 0 : 1 : DATA_LEN
        //y1 = ax^2+bx;
        //y2 = ax + b;
        double x = i;
        y_true[i][0] = para[0] * x * x + para[1] * x;
        y_true[i][1] = para[0] * x + para[1];

        y_in[i][0] = y_true[i][0] + getRand();
        y_in[i][1] = y_true[i][1] + getRand();
    }
    double mypara[2] = {1, 2};
    ceres::Problem problem;
    for (int i = 0; i < DATA_LEN; i++) {
        ceres::CostFunction *pCostFunction = new test(i, y_in[i]);
        problem.AddResidualBlock(pCostFunction, NULL, mypara);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    cout << mypara[0] << endl << mypara[1] << endl;

    return 0;
}

float getRand()
{
    return 2.0 * rand() / RAND_MAX - 1.0;
}