0. 前言

在许多优化问题中,尤其是传感器融合问题,必须对存在于称为流形的空间中的数量进行建模,例如由四元数表示的传感器的旋转/方向。其中流型中的加法用⊞表示。以旋转矩阵更新为例:

在这里插入图片描述
LocalParameterization 接口允许用户定义参数块并与它们所属的流形相关联。它通过定义 Plus (⊞) 运算及其在 Δ=0 处相对于 Δ 的导数来实现。

class LocalParameterization {
 public:
  virtual ~LocalParameterization() {}
  // 流型空间中的加法
  virtual bool Plus(const double* x,   
                    const double* delta,
                    double* x_plus_delta) const = 0;
  // 计算雅克比矩阵
  virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
  // local_matrix = global_matrix * jacobian
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;
  virtual int GlobalSize() const = 0; // 参数块 x 所在的环境空间的维度。
  virtual int LocalSize() const = 0; // Δ 所在的切线空间的维度
};

我们常常可以发现在VINS,ORB系列中含有这类的操作,通过局部参数化能够有效地实现流型的相加(其实是扰动计算)。这里作者提供一个基于Sophus的Ceres优化方案。

1. Sophus

这个问题和数据存储顺序及update时使用的SE3::exp(Eigen::Matrix<double,6,1> &a)操作有关

Eigen内部Quaterniond元素存储地址和初始化构造函数中参数的顺序是不一致的。

  1. Eigen初始化Quaterniond(w,x,y,z),其中w是实部,x,y,z对应三个轴的虚部

    但是这和Eigen内部的Quaterniond的数据存储顺序不一样,存储顺序x,y,z,w

  2. Sophus内部SO3部分由Eigen::Quaternion表示,translation用Eigen::Vector3d表示。通过SE3::data()可以获得SE3存储的数据的double*指针,存储地址顺序如下:

    data()[0]对应Quaterniond x

    data()[1]对应Quaterniond y

    data()[2]对应Quaterniond z

    data()[3]对应Quaterniond w

    data()[4]对应SE3的tx

    data()[5]对应SE3的ty

    data()[6]对应SE3的tz

    但是Sophus::SE3d::exp(const Eigen::Vector6d &a)函数中a的前三维对应着SE3中translation的部分,而后三维才对应SO3的部分。和Sophus内部存储的顺序是不一致的。因此在SLAM中对位姿进行优化的时候,利用Sophus和ceres需要注意这个顺序

实践了一下Sophus + ceres对SE3进行解析Jacobian的优化时,继承ceres::CostFunction自定义自己的CostFunction,在其中中编码Jacobian解析形式。然后再自定义一个相应的LocalParameterization

#pragma once

#include "Thirdparty/sophus/se3.hpp"
#include <ceres/local_parameterization.h>


class LocalParameterizationSE3 : public ceres::LocalParameterization
{
  public:
    virtual ~LocalParameterizationSE3() {}

    // SE3 plus operation for Ceres
    //
    //  exp(delta)*T
    // 通过局部参数化实现plus的功能,流型空间中的加法
    virtual bool Plus(double const *T_raw, double const *delta_raw, double *T_plus_delta_raw) const
    {
        Eigen::Map<Sophus::SE3d const> const T(T_raw);

        Eigen::Map<Sophus::SE3d> T_plus_delta(T_plus_delta_raw);

        Eigen::Map<Eigen::Vector3d const> delta_phi(delta_raw);
        Eigen::Map<Eigen::Vector3d const> delta_rho(delta_raw+3);
        Eigen::Matrix<double,6,1> delta;

        delta.block<3,1>(0,0) = delta_rho;//translation
        delta.block<3,1>(3,0) = delta_phi;//rotation

        T_plus_delta = Sophus::SE3d::exp(delta) * T;//左扰动,相对于T的绝对坐标轴
        return true;
    }

    // 计算雅克比矩阵
    virtual bool ComputeJacobian(double const *T_raw, double *jacobian_raw) const
    {
        Eigen::Map<Sophus::SE3d const> T(T_raw);
        Eigen::Map<Eigen::Matrix<double, 7, 6,Eigen::RowMajor>> jacobian(jacobian_raw);
        jacobian.setZero();
        jacobian.block<3,3>(4,0) = Eigen::Matrix3d::Identity();
        jacobian.block<3,3>(0,3) = Eigen::Matrix3d::Identity();//注意需要一一对应
        return true;
    }

    virtual int GlobalSize() const { return Sophus::SE3d::num_parameters; }// 参数块 x 所在的环境空间的维度。

    virtual int LocalSize() const { return Sophus::SE3d::DoF; }// Δ 所在的切线空间的维度
};

#