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元素存储地址和初始化构造函数中参数的顺序是不一致的。
-
Eigen初始化Quaterniond(w,x,y,z),其中w是实部,x,y,z对应三个轴的虚部
但是这和Eigen内部的Quaterniond的数据存储顺序不一样,存储顺序x,y,z,w。
-
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; }// Δ 所在的切线空间的维度
};
#
评论(0)
您还未登录,请登录后发表或查看评论