ROS-3DSLAM(16):视觉部分visual estimator第九节 factor4

2021@SDUSC
2021年12月24日星期五——2021年12月27日星期一

一、背景简介：

这一周继续分析factor文件夹。

这一周我分析的代码部分视觉的残差，特征点在相机的投影下产生的残差:projection_factor.cpp和projection_td_factor.cpp和pose_local_parameterization.cpp以及对应的头文件。

首先是原理分析：

回到我们之前分析过的边缘化的总框架图上去，可以看出，这部分的代码是同ceres息息相关的的，是利用ceres Solver来处理问题的。

---

经过阅读分析，我发现这部分的核心代码就是projection_factor
projetcion_td_factor是它的扩展，pose_local_parameterization是ceres里的一个小函数。

二、代码分析：

1projection_factor.h

对于projection_factor而言，它继承了Ceres库中的SizedCostFunction。

它表示的是第i帧中单位相机平面上的点pts_i重投影到第j帧单位相机平面上的点与匹配点pts_j的投影误差。需要实现构造函数以及重载Evaluate函数。

#pragma once

#include <ros/assert.h>
#include <ceres/ceres.h>
#include <Eigen/Dense>
#include "../utility/utility.h"
#include "../utility/tic_toc.h"
#include "../parameters.h"

class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>
{
   
  public:
    ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j);
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
    void check(double **parameters);

    Eigen::Vector3d pts_i, pts_j;
    Eigen::Matrix<double, 2, 3> tangent_base;
    static Eigen::Matrix2d sqrt_info;
    static double sum_t;
};

2 projection_factor.cpp

构造函数传入的是常数，是一对匹配的特征点，这里传入的数据是匹配点在相机平面上的坐标。
但是重投影误差计算的是单位球面切平面上的误差。（为什么是球平面?）
因此需要利用施密特正交化求出切平面的正交基。

#include "projection_factor.h"

Eigen::Matrix2d ProjectionFactor::sqrt_info;
double ProjectionFactor::sum_t;

ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j)
{
   
#ifdef UNIT_SPHERE_ERROR
    Eigen::Vector3d b1, b2;
    Eigen::Vector3d a = pts_j.normalized();
    Eigen::Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    b1 = (tmp - a * (a.transpose() * tmp)).normalized();
    b2 = a.cross(b1);
    tangent_base.block<1, 3>(0, 0) = b1.transpose();
    tangent_base.block<1, 3>(1, 0) = b2.transpose();
#endif
};

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
   
    TicToc tic_toc;
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    //pts_i 是i时刻归一化相机坐标系下的3D坐标
    //第i帧相机坐标系下的的逆深度
    double inv_dep_i = parameters[3][0];
    //第i帧相机坐标系下的3D坐标
    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    //第i帧IMU坐标系下的3D坐标
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    //世界坐标系下的3D坐标
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    //第j帧imu坐标系下的3D坐标
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    //第j帧相机坐标系下的3D坐标
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
    Eigen::Map<Eigen::Vector2d> residual(residuals);

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

    residual = sqrt_info * residual;

    //reduce 表示残差residual对fci（pts_camera_j）的导数，同样根据不同的相机模型
    if (jacobians)
    {
   
        Eigen::Matrix3d Ri = Qi.toRotationMatrix();
        Eigen::Matrix3d Rj = Qj.toRotationMatrix();
        Eigen::Matrix3d ric