视觉SLAM十四讲学习6 Ceres Solver (2) Motion BA

前言

本篇继续记录Ceres Solver的学习。使用Ceres实现PnP的BA方法。

Ceres Motion BA 重投影误差

基于BA的PnP，就是通过匹配的特征点，构建关于位姿的重投影误差，并只优化位姿。重投影误差为： e = ∣ ∣ p 1 − K ( R P 2 + t ) ∣ ∣ 2 e=||p_1-K(RP_2+t)||^2 e=∣∣p1​−K(RP2​+t)∣∣2 其中， p 1 , P 2 p_1,P_2 p1​,P2​分别表示当前帧匹配点的像素坐标，以及上一帧匹配点的世界坐标。

每一对2d-3d匹配点的重投影误差，就是Ceres::CostFunction。

ceres::CostFunction构建思路

上面已经列出了重投影误差的公式，现在就是要用Ceres把重投影公式表示出来。

struct motionBA{
	public:
	
	private:	
};

首先考虑外部参量。根据重投影误差，外部参量有 p 1 , K , P 2 p_1,K,P_2 p1​,K,P2​。这些参量都不是优化变量，是固定值，因此可以作为ceres::CostFuntion的成员，通过构造函数取值：

struct motionBA{
	public:
		motionBA(cv::Point2d p1, cv::Point3d P2, cv::Mat K)
	private:
		cv::Point2d p1_;
		cv::Point3d P2_;
		cv::Mat K_cam_;
};

然后确定CostFunction待优化变量和残差的维度。ceres/rotation.h提供了轴角旋转（也就是通过李代数旋转）的函数ceres::AngleAxisRotatePoint()，因此待优化的位姿维度r,t=6，残差的维度与重投影像素坐标相同为2：

struct motionBA
{
private:
    cv::Point2d pt2d_;
    cv::Point3d pt3d_;
    cv::Mat K_cam_;

public:
    motionBA(cv::Point2d pt2d, cv::Point3d pt3d, cv::Mat K);

    template
    bool operator()(const T* const pose6d, T* residual) const {
        T proj_pt2d[3];
        T proj_pt3d[3];
        proj_pt3d[0] = T(pt3d_.x);
        proj_pt3d[1] = T(pt3d_.y);
        proj_pt3d[2] = T(pt3d_.z);
        ceres::AngleAxisRotatePoint(pose6d, proj_pt3d, proj_pt2d);
        proj_pt2d[0] += pose6d[3];
        proj_pt2d[1] += pose6d[4];
        proj_pt2d[2] += pose6d[5];
        proj_pt2d[0] /= proj_pt2d[2];
        proj_pt2d[1] /= proj_pt2d[2];
        proj_pt2d[2] = T(1.0);
       
        std::tuple reproj_pt2d = cam2pixel(proj_pt2d[0], proj_pt2d[1], proj_pt2d[2], K_cam_);
        residual[0] = std::get(reproj_pt2d) - T(pt2d_.x);
        residual[1] = std::get(reproj_pt2d) - T(pt2d_.y);

        return true;
    }

    static ceres::CostFunction* CreateCostFunction(const cv::Point2d pt2d, const cv::Point3d pt3d, const cv::Mat K){
        return (new ceres::AutoDiffCostFunction(new pnpCeres(pt2d, pt3d, K)));
    }
};

解释一下上面的代码：

(1)
ceres的默认数据格式是 ceres::Jet
在重载operator()中，也要使用模板T来进行计算，这样才能自动求导。
(2)
C++的 int,double等数值类型可以通过T()强制转换为ceres::Jet类型
但反之不行。需要查看ceres::Jet j的值时，可以通过成员 j.a 查看。
(3)
ceres::AngleAxisRotatePoint(pose6d, proj_pt3d, proj_pt2d)，把空间坐标旋转
然后下面的加法和除法，就是 RP+t ，以及相机坐标归一化。
(4)
cam2pixel把归一化相机坐标量化为像素坐标，然后与匹配的像素坐标做差
这就是重投影误差

cere::Problem构建思路

上面已经列出了重投影误差的公式，下一步就是构建Ceres::Problem。

有n对2d-3d匹配点，每对匹配点都能得到一个残差，把每个残差都加到ceres::Problem中。

由于少部分匹配点可能是错配，误差比较大，可以添加ceres::LossFunction来减少这些点对优化的影响。

最后，设置好ceres::Solver（包括options求解方法，summary结果展示），然后就可以求解了。

Motion BA整体：

#ifndef MOTION_BA_
#define MOTION_BA_

#include 
#include 
#include 
#include 
#include 
#include 

#include "camera_utils.h"

#endif

// pnp class for ba residual cost function
struct pnpCeres
{
private:
    cv::Point2d pt2d_;
    cv::Point3d pt3d_;
    cv::Mat K_cam_;

public:
    pnpCeres(cv::Point2d pt2d, cv::Point3d pt3d, cv::Mat K);

    template
    bool operator()(const T* const pose6d, T* residual) const {
        T proj_pt2d[3];
        T proj_pt3d[3];
        proj_pt3d[0] = T(pt3d_.x);
        proj_pt3d[1] = T(pt3d_.y);
        proj_pt3d[2] = T(pt3d_.z);
        ceres::AngleAxisRotatePoint(pose6d, proj_pt3d, proj_pt2d);
        proj_pt2d[0] += pose6d[3];
        proj_pt2d[1] += pose6d[4];
        proj_pt2d[2] += pose6d[5];
        proj_pt2d[0] /= proj_pt2d[2];
        proj_pt2d[1] /= proj_pt2d[2];
        proj_pt2d[2] = T(1.0);
       
        std::tuple reproj_pt2d = cam2pixel(proj_pt2d[0], proj_pt2d[1], proj_pt2d[2], K_cam_);
        residual[0] = std::get(reproj_pt2d) - T(pt2d_.x);
        residual[1] = std::get(reproj_pt2d) - T(pt2d_.y);

        return true;
    }

    static ceres::CostFunction* CreateCostFunction(const cv::Point2d pt2d, const cv::Point3d pt3d, const cv::Mat K){
        return (new ceres::AutoDiffCostFunction(new pnpCeres(pt2d, pt3d, K)));
    }
};

// create ceres problem with matched 2d-3d points
template
void solveBAProblem(std::vector pt2ds, std::vector pt3ds, cv::Mat K, T* init_pose6d){
    // create ceres problem for motion BA
    ceres::Problem problem;
    for (size_t i = 0; i