Ceres中CostFunctionToFunctor的优化使用讲解

0. 讲述如何在Ceres优化中使用CostFunctionToFunctor

在Ceres中计算代价函数的导数有3种方式: 解析导数, 自动求导, 数值导数
0.1. 解析导数: 速度最快, 更加稳定和精确;
0.2. 自动求导: 编写简单, 只需要告诉Ceres如何计算残差即可, 要求: 需要写出关于计算残差的模版函数functor, 这个functor有两个用处, 一个是用来计算残差, 另一个是根据链式法则计算雅克比矩阵.(这也就是为什么functor要写成模版函数的原因: 一个函数由于类型的不同, 被使用了两次)
0.3. 数值导数: 编写简单, 根据微分的定义来计算导数, 面临的问题: 鲁棒性较低, 耗时长

1. CostFunctionToFunctor使用场合

1.1. 具体的使用场合和出发点可以参考参考资料这一节中给出的超链接;
1.2. 当我们面临一个大的优化问题的时候, 经常都是使用Ceres中单一的求导方式来计算雅克比矩阵, 比如我们在使用自动求导求解雅克比矩阵的时候, 当模版functor函数中使用了第三方库函数的时候, 不出意外的话, 如果你进行编译make的话, 模版functor函数会报各种error, 主要是因为这个函数是两用的, 第三方库函数并不支持ceres::Jet类型的形参类型, 如果我们这个时候要在模版functor中强行使用第三方库函数, 我们就需要进行强制转换(方法如下代码), 这个时候编译没有问题了:

double tmp_theta;
tmp_theta = ((ceres::Jet<double, 12>)(theta)).a;

但是你运行这个优化问题以后发现, 优化并没有进行, 因为上面的强制转换, 只是将编译系统这个层面通过了, 但是却给不了计算导数的具体实现, 通过调试可以发现这个操作将ceres::Jet中含有导数信息的双数dual number给抹除了, 都变成了0, 这个时候雅克比矩阵是0矩阵, 当然无法为优化提供梯度方向了, 也就导致优化没有进行, 这个时候可以在模版functor函数中嵌入数值求导部分, 也就是跟第三方库有关联的优化变量, 这个也就是CostFunctionToFunctor的用处.

也可以嵌入解析导数部分, 道理一样, 这里就不展开讲解了.

2. 实例分析

2.1. 该demo是一个大型的BA优化问题, 优化n个相机位姿(旋转$R$, 平移$t$), m个路标点($P$), 以及相机的畸变参数和焦距.
$$\begin{gathered} \mathbf{X}=[{\mathbf{X}}_{cam}^1,{\mathbf{X}}_{cam}^2,\cdots ,{\mathbf{X}}_{cam}^n,{\mathbf{P}}_1,{\mathbf{P}}_2,\cdots ,{\mathbf{P}}_m,] \\ 其中{\mathbf{X}}_{cam}^i=[\mathbf{R},\mathbf{t},f_x,d_1,d_2] \end{gathered}$$
其中, m远大于n (m>>n), 相机camera变量维度为9(3+3+1+1+1), 路标点P的维度为3.
2.2. 如果我们这个时候用于计算相机位姿和路标点的是来自于库函数的, 该如何编写CostFunctionToFunctor?
2.3. 这里我们使用了Eigen中关于AngleAxisd中重载的运算符operator*当做这个第三方的库函数, 该库函数跟待优化变量中的相机位姿中的旋转$R$, 路标点$P$关联了起来. 它的输出值为3维, 待优化变量camera为3维, 路标点point_in为3维, 所以CostFunctionToFunctor中的模版参数为: ceres::CostFunctionToFunctor<3, 3, 3>

3. Factor的实现如下

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

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

#include "common/tools/rotation.h"
#include "common/projection.h"

#include <cxxabi.h>

struct RotatedPointFunctor {
    bool operator()(const double* camera, const double* point_in, double* value) const {
//        Eigen::AngleAxisd rotation_vector(theta[0], Eigen::Vector3d(axis[0], axis[1], axis[2]));
        const double theta2 = DotProduct(camera, camera);
        if (theta2 > (std::numeric_limits<double>::epsilon())) {
            const double theta = sqrt(theta2);
            const double costheta = cos(theta);
            const double sintheta = sin(theta);
            const double theta_inverse = 1.0 / theta;

            const double w[3] = { camera[0] * theta_inverse,
                                  camera[1] * theta_inverse,
                                  camera[2] * theta_inverse };

            Eigen::AngleAxisd rotation_vector(theta, Eigen::Vector3d(w[0], w[1], w[2]));
            Eigen::Vector3d point(point_in[0], point_in[1], point_in[2]);
            Eigen::Vector3d point_rotated = rotation_vector * point;

            value[0] = point_rotated.x();
            value[1] = point_rotated.y();
            value[2] = point_rotated.z();
            } else {
                double w_cross_pt[3];
                CrossProduct(camera, point_in, w_cross_pt);

                value[0] = point_in[0] + w_cross_pt[0];
                value[1] = point_in[1] + w_cross_pt[1];
                value[2] = point_in[2] + w_cross_pt[2];
            }

        return true;
    }
};

class SnavelyReprojectionError
{
public:
    SnavelyReprojectionError(double observation_x, double observation_y) {
        observed_x = observation_x;
        observed_y = observation_y;

        rotated_point.reset(new ceres::CostFunctionToFunctor<3, 3, 3>(
            new ceres::NumericDiffCostFunction<RotatedPointFunctor,
                ceres::CENTRAL, 3, 3, 3>(new RotatedPointFunctor)));
    }


template<typename T>
    bool operator()(const T* const camera,
                    const T* const point,
                    T* residuals) const {
        // camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        T p[3];

        (*rotated_point)( camera, point, p );

        p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];

        // Compute the center fo distortion
        T xp = -p[0]/p[2];
        T yp = -p[1]/p[2];

        // Apply second and fourth order radial distortion
        const T& l1 = camera[7];
        const T& l2 = camera[8];

        T r2 = xp*xp + yp*yp;
        T distortion = T(1.0) + r2 * (l1 + l2 * r2);

        const T& focal = camera[6];
        predictions[0] = focal * distortion * xp;
        predictions[1] = focal * distortion * yp;

        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    static ceres::CostFunction* Create(const double observed_x, const double observed_y){
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>(
            new SnavelyReprojectionError(observed_x,observed_y)));
//        return (new ceres::NumericDiffCostFunction<SnavelyReprojectionError, ceres::CENTRAL, 2,9,3>(
//            new SnavelyReprojectionError(observed_x,observed_y)));
    }


private:
    double observed_x;
    double observed_y;
    std::unique_ptr<ceres::CostFunctionToFunctor<3, 3, 3> > rotated_point;
};

#endif // SnavelyReprojection.h

4. 说明

4.1. 当然我们可以通过Eigen库的模版函数实现这个功能, 方法有很多;
4.2. 使用Eigen模版函数库计算旋转后的3D坐标点代码如下:

// 直接使用Eigen中的模版库函数operator*, 而不用自己去实现罗德里格斯公式
template<typename T>
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
    const T theta2 = DotProduct(angle_axis, angle_axis);

    const T theta = sqrt(theta2);
    const T costheta = cos(theta);
    const T sintheta = sin(theta);
    const T theta_inverse = 1.0 / theta;

    const T w[3] = { angle_axis[0] * theta_inverse,
                     angle_axis[1] * theta_inverse,
                     angle_axis[2] * theta_inverse };

    Eigen::AngleAxis<T> rotation_vector(theta, Eigen::Matrix<T, 3, 1>(w[0], w[1], w[2]));  // add this by lyf
    Eigen::Matrix<T, 3, 1> tmp_pt(pt[0], pt[1], pt[2]);
    Eigen::Matrix<T, 3, 1> point_rotated = rotation_vector * tmp_pt;
    result[0] = point_rotated.x();
    result[1] = point_rotated.y();
    result[2] = point_rotated.z();
}

只需将这一部分取代掉原程序中的rodriguez formula部分就行, 我们不用自己实现rodriguez formula, 只需要使用Eigen模版库中的operator*即可.

5. 参考资料

5.1. Ceres官网对CostFunctionToFunctor类的解释与举例
5.2. Interfacing with Automatic Differentiation