GTSAM曲线拟合与自定义因子
在《视觉SLAM十四讲》中，第六章讲了最小二乘与曲线拟合，采用了三种方法：
- 手动推导高斯牛顿方法，即自己构造正规方程 J T J Δ x = − J T e J^TJ \Delta x = -J^T e JTJΔx=−JTe中的每一项，残差 e e e、雅可比 J J J、海塞矩阵 H = J T J H=J^T J H=JTJ，矩阵分块写入对应的系数即可迭代求解。
- Ceres优化库，这个只需要预先设定好残差（在代码中称为残差块）和待优化变量（在代码中称为参数块），雅可比可采用自动求导、数值求导、解析求导（该方式需要用户自己设置雅可比矩阵，需要手动推导，但是求解效率比前两种高）三种方式，在迭代优化求解的时候可采用高斯牛顿、LM、DogLeg的方法。
- G2O优化库，这个和Ceres几乎一致，只是把一个优化问题看成是图优化问题，需要定义好顶点和边，也有自动求导、数值求导、解析求导三种方式，在迭代优化求解的时候也采用高斯牛顿、LM、DogLeg的方法。
详细内容的请阅读高翔的《视觉SLAM十四讲》，代码ch6。以上的代码没有采用GTSAM优化库进行曲线拟合的过程，本文详细介绍。
GTSAM介绍
先介绍一下GTSAM，GTSAM的因子图优化框架与G2O类似，都把优化问题转化成图论的概念，只不过G2O中只有顶点（Vertex）和边（Edge）的概念，而GTSAM中只有因子节点（factor）和变量节点（value）的概念。出自GTSAM官网介绍的一张图:
其中黑色空心大圆圈的变量节点 x 1 , x 2 , ⋯ x_1,x_2,\cdots x1,x2,⋯为变量节点，而黑色实心小圆代表的是因子节点 f 0 ( x 1 ) , f 1 ( x 1 , x 2 ) , ⋯ f_0(x_1),f_1(x_1,x_2),\cdots f0(x1),f1(x1,x2),⋯，红色的圆圈也是因子节点，代表的是回环因子。
- 变量节点（value）：变量节点代表则为待优化的变量，可以是6维位姿（在GTSAM中已经定义好Pose3的数据类型），可以是3维路标点（在GTSAM中已经定义好Point3的数据类型）。
- 因子节点（factor）：因子节点代表G2O的边（Edge），即残差。在因子图中，每一条边上必定有一个因子节点。在GTSAM中有非常多预定义好因子供用户使用，比如先验因子（PriorFactor），上图中的 f 0 ( x 1 ) f_0(x1) f0(x1); 二元因子（BetweenFactor），上图中的 f 1 ( x 1 , x 2 ) , f 2 ( x 2 , x 3 ) , ⋯ f_1(x_1,x_2),f2(x_2,x_3),\cdots f1(x1,x2),f2(x2,x3),⋯; SFM因子图（GeneralSFMFactor）; 投影因子（GenericProjectionFactor）; 方向距离因子（BearingRangeFactor，用于位姿对路标点的观测）等。由于存在许多GTSAM开发者预定义好的因子，用户可直接调用，这个比Ceres和G2O方便的多。当然也可以自定义因子类型，但需要自己给出雅可比矩阵（自动求导AdaptAutoDiff.h是调用ceres的自动求导功能）。
在这里列出一些关于GTSAM类的继承关系图：
以上还没写完整，只列出了最基本的因子，还有许多定义好的因子。其中NoiseModelFactor1为一元因子（连接一个变量节点），NoiseModelFactor2为二元因子（连接两个变量节点）。
GTSAM进行曲线拟合
与《视觉SLAM十四讲》第6章问题一样，给定一系列的数据点
(
x
,
y
)
(x,y)
(x,y)，然后拟合出直线：
y
=
e
a
x
2
+
b
x
+
c
y = e^{ax^2 + bx + c}
y=eax2+bx+c
参考G2O的框架，我们把待优化变量设置成
a
,
b
,
c
a,b,c
a,b,c，因子节点为残差，曲线拟合的因子图模型如下所示：
关于自定义一元因子，我们把一元因子对应一个高斯噪声模型的测量似然， L ( q ; m ) = e − 1 2 ∥ h ( q ) − m ∥ Σ 2 = f ( q ) L(q ; m)=e^ {-\frac{1}{2}\|h(q)-m\|_{\Sigma}^{2}}=f(q) L(q;m)=e−21∥h(q)−m∥Σ2=f(q)，其中 q q q为待优化变量， m m m为观测， h ( q ) h(q) h(q)为测量函数， E ( q ) = h ( q ) − m E(q) = h(q)-m E(q)=h(q)−m为残差，函数 L ( q , m ) L(q,m) L(q,m)是 q q q的函数（因为测量 m m m已知），这也就能解释为什么这是个一元因子 f ( q ) f(q) f(q)。
对应于代码，我们把待优化变量设置成
q
=
[
a
b
c
]
q = \begin{bmatrix} a \\ b \\ c \end{bmatrix}
q=
abc
, 观测
m
=
[
x
y
]
m = \begin{bmatrix} x \\ y \\ \end{bmatrix}
m=[xy]，观测函数
h
(
q
)
=
e
a
x
2
+
b
x
+
c
h(q)=e^{ax^2 + bx + c}
h(q)=eax2+bx+c（注意这里观测函数只与
q
q
q有关，与
x
x
x无关），所以残差
E
(
q
)
=
h
(
q
)
−
m
=
e
a
x
2
+
b
x
+
c
−
y
∈
R
1
E(q)=h(q)-m=e^{ax^2 + bx + c} - y \in \mathbb{R}^1
E(q)=h(q)−m=eax2+bx+c−y∈R1，残差属于1维变量，而
q
∈
R
3
q \in \mathbb{R}^3
q∈R3，所以雅可比矩阵
H
=
∂
[
h
(
q
)
−
m
]
∂
q
=
[
∂
(
e
a
x
2
+
b
x
+
c
−
y
)
∂
a
∂
(
e
a
x
2
+
b
x
+
c
−
y
)
∂
b
∂
(
e
a
x
2
+
b
x
+
c
−
y
)
∂
c
]
=
[
x
2
e
a
x
2
+
b
x
+
c
x
e
a
x
2
+
b
x
+
c
e
a
x
2
+
b
x
+
c
]
\begin{align} H & = \frac{\partial [h(q) - m]}{\partial q} \\ & = \begin{bmatrix} \frac{\partial (e^{ax^2 + bx + c} - y )}{\partial a} & \frac{\partial (e^{ax^2 + bx + c} - y )}{\partial b} & \frac{\partial(e^{ax^2 + bx + c} - y )}{\partial c} \end{bmatrix} \\ & = \begin{bmatrix} x^2 e^{ax^2 + bx + c} & x e^{ax^2 + bx + c} & e^{ax^2 + bx + c} \end{bmatrix} \end{align}
H=∂q∂[h(q)−m]=[∂a∂(eax2+bx+c−y)∂b∂(eax2+bx+c−y)∂c∂(eax2+bx+c−y)]=[x2eax2+bx+cxeax2+bx+ceax2+bx+c]
在程序代码中主要有以下的步骤：
- 添加GTSAM的库文件
- 自定义因子factor类
- 声明类中观测值
- 需要重写eveluateError函数，其中需要给定雅可比矩阵
- 定义因子图模型
- 加入自定义的一元因子
- 利用高斯牛顿或LM算法优化
- 输出结果
下面是代码，写了核心部分代码注释，比较容易理解
#include <gtsam/base/Vector.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/Values.h>
#include <opencv2/core/core.hpp>
#include <random>
#include "matplotlibcpp.h"
#include <cmath>
using namespace std;
using namespace gtsam;
namespace plt = matplotlibcpp;
// y = exp(a x^2 + b x + c)
// 利用x和参数a,b,c计算y
double funct(const gtsam::Vector3 &p, const double x)
{
return exp(p(0) * x * x + p(1) * x + p(2));
}
// 自定义类名 : 继承于一元因子类<优化变量的数据类型>
class curvfitFactor : public gtsam::NoiseModelFactor1<gtsam::Vector3>
{
double mx, my; // 观测值
public:
curvfitFactor(gtsam::Key key, const gtsam::SharedNoiseModel &noise, double x, double y)
: gtsam::NoiseModelFactor1<gtsam::Vector3>(noise, key), mx(x), my(y)
{
}
virtual ~curvfitFactor()
{
}
// 自定义因子一定要重写evaluateError函数(优化变量, 雅可比矩阵)
gtsam::Vector evaluateError(const gtsam::Vector3 &p, boost::optional<gtsam::Matrix &> H = boost::none) const
{
auto val = funct(p, mx);
if (H) // 残差为1维，优化变量为3维，雅可比矩阵为1*3
{
gtsam::Matrix Jac = gtsam::Matrix::Zero(1, 3);
Jac << mx * mx * val, mx * val, val;
(*H) = Jac;
}
return gtsam::Vector1(val - my); // 返回值为残差
}
};
int main()
{
using gtsam::symbol_shorthand::X;
// const double sig = 1;
// std::random_device rd;
// std::default_random_engine generator_(rd());
// std::normal_distribution<double> noise(0, sig);
gtsam::NonlinearFactorGraph graph;
const gtsam::Vector3 para(1.0, 2.0, 1.0); // a,b,c的真实值
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data;
for (int i = 0; i < 100; ++i)
{
double mx = i / 100.0;
auto my = funct(para, mx) + rng.gaussian(w_sigma * w_sigma); // 加入了噪声数据
auto noiseM = gtsam::noiseModel::Isotropic::Sigma(1, w_sigma); // 噪声的维度需要与观测值维度保持一致
graph.emplace_shared<curvfitFactor>(X(0), noiseM, mx, my); // 加入一元因子
x_data.push_back(mx);
y_data.push_back(my);
}
gtsam::Values intial;
intial.insert<gtsam::Vector3>(X(0), gtsam::Vector3(2.0, -1.0, 5.0));
// DoglegOptimizer opt(graph, intial); // 使用Dogleg优化
// GaussNewtonOptimizer opt(graph, intial); // 使用高斯牛顿优化
LevenbergMarquardtOptimizer opt(graph, intial); // 使用LM优化
std::cout << "initial error=" << graph.error(intial) << std::endl;
auto res = opt.optimize();
res.print("final res:");
std::cout << "final error=" << graph.error(res) << std::endl;
gtsam::Vector3 matX0 = res.at<gtsam::Vector3>(X(0));
std::cout << "a b c: " << matX0 << "\n";
/************* 绘制曲线************************/
int n = 5000;
std::vector<double> x(n), y(n), w(n, 2);
for (int i = 0; i < n; ++i)
{
x.at(i) = i * 1.0 / n;
y.at(i) = exp(matX0(0) * x[i] * x[i] + matX0(1) * x[i] + matX0(2));
}
plt::figure_size(640, 480);
plt::plot(x_data, y_data, "ro");
plt::plot(x, y, {{"color", "blue"}, {"label", "$y = e^{ax^2+bx+c}$"}});
plt::show();
return 0;
}
其中绘制曲线部分采用了matplotlib-cpp，也就是在C++直接调python3的matpoltib。结果如下，红色圆点为数据点观测值 m = [ x y ] m = \begin{bmatrix} x \\ y \\ \end{bmatrix} m=[xy]，蓝色曲线为拟合后的曲线 y = e a x 2 + b x + c y = e^{ax^2 + bx + c} y=eax2+bx+c
对应CMakeLists.txt为
cmake_minimum_required(VERSION 3.5.1)
project(test)
find_package(Boost REQUIRED COMPONENTS thread)
set(CMAKE_BUILD_TYPE "Release")
set( CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
set(GTSAM_USE_SYSTEM_EIGEN ON)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
message_filters
nav_msgs
sensor_msgs
message_generation
message_runtime
message_filters
roscpp
rospy
rosbag
std_msgs
image_transport
cv_bridge
tf
)
#find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Ceres REQUIRED)
find_package(OpenMP REQUIRED)
find_package(GTSAM REQUIRED QUIET)
# Python3,numpy
find_package(Python3 COMPONENTS Development NumPy)
include_directories(
include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${GTSAM_INCLUDE_DIR})
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
DEPENDS EIGEN3 PCL
INCLUDE_DIRS include
)
add_executable(custom_factor src/custom_factor.cpp) # 自定义因子（类似于GPS的一元因子）
target_link_libraries(custom_factor ${catkin_LIBRARIES} ${PCL_LIBRARIES} gtsam)
add_executable(curveFitting src/curveFitting.cpp) # 曲线拟合
target_link_libraries(curveFitting ${OpenCV_LIBS} ${catkin_LIBRARIES} ${PCL_LIBRARIES} gtsam Python3::Python Python3::NumPy)
target_include_directories(curveFitting PRIVATE ${Python3_INCLUDE_DIRS} ${Python3_NumPy_INCLUDE_DIRS})
GTSAM的自定义一元因子（类似于GPS）
以上CMakeLists.txt还写了一个关于类似于GPS的一元因子。也就是最开始介绍的
f
0
(
x
1
)
f_0(x_1)
f0(x1)。先来看看推导过程，以一个二维平面为例子，在平面中机器人有三个自由度
(
x
,
y
,
θ
)
(x,y,\theta)
(x,y,θ)，所以优化变量$q = \begin{bmatrix} q_x \ q_y \ q_{\theta} \end{bmatrix}
，如果采用
2
D
刚体变换则表示成
，如果采用2D刚体变换则表示成
，如果采用2D刚体变换则表示成q = \begin{bmatrix} cos(q_{\theta}) & -sin(q_{\theta}) & q_x \ sin(q_{\theta}) & cos(q_{\theta}) & q_y \ 0 & 0 & 1 \end{bmatrix} \in SE(2)
，观测函数
，观测函数
，观测函数h(q) = \begin{bmatrix} q_x \ q_y \end{bmatrix}$。 我们进行一阶泰勒展开进行近似：
h
(
q
e
ξ
^
)
≈
h
(
q
)
+
H
ξ
h\left(q e^{\hat{\xi}}\right) \approx h(q)+H \xi
h(qeξ^)≈h(q)+Hξ
其中
q
e
ξ
^
q e^{\hat{\xi}}
qeξ^是在
q
q
q上加入微小扰动$\xi $，由于李群中的旋转不满足加法封闭性，需要转化为李代数形式:
ξ
=
[
δ
x
δ
y
δ
θ
]
⟹
指数映射
e
ξ
^
=
[
c
o
s
(
δ
θ
)
−
s
i
n
(
δ
θ
)
δ
x
s
i
n
(
δ
θ
)
c
o
s
(
δ
θ
)
δ
y
0
0
1
]
≈
[
1
−
δ
θ
δ
x
δ
θ
1
δ
y
0
0
1
]
\xi = \begin{bmatrix} \delta x \\ \delta y \\ \delta \theta \end{bmatrix} \overset{指数映射}{\Longrightarrow} e^{\hat{\xi}} = \begin{bmatrix} cos(\delta \theta) & -sin(\delta \theta) & \delta x \\ sin(\delta \theta) & cos(\delta \theta) & \delta y \\ 0 & 0 & 1 \end{bmatrix} \approx \begin{bmatrix} 1 & - \delta \theta & \delta x \\ \delta \theta & 1 & \delta y \\ 0 & 0 & 1 \end{bmatrix}
ξ=
δxδyδθ
⟹指数映射eξ^=
cos(δθ)sin(δθ)0−sin(δθ)cos(δθ)0δxδy1
≈
1δθ0−δθ10δxδy1
其中需要利用等价无穷小来替换：
lim
狗
→
0
s
i
n
(
狗
)
狗
=
1
lim
狗
→
0
c
o
s
(
狗
)
=
1
\begin{align} \lim_{狗 \to 0} \frac{sin(狗)}{狗} & = 1 \\ \lim_{狗 \to 0} cos(狗) & = 1 \end{align}
狗→0lim狗sin(狗)狗→0limcos(狗)=1=1
所以
h
(
q
e
ξ
^
)
=
h
(
[
c
o
s
(
q
θ
)
−
s
i
n
(
q
θ
)
q
x
s
i
n
(
q
θ
)
c
o
s
(
q
θ
)
q
y
0
0
1
]
⋅
[
1
−
δ
θ
δ
x
δ
θ
1
δ
y
0
0
1
]
)
h\left(q e^{\hat{\xi}}\right) = h\left ( \begin{bmatrix} cos(q_{\theta}) & -sin(q_{\theta}) & q_x \\ sin(q_{\theta}) & cos(q_{\theta}) & q_y \\ 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 1 & - \delta \theta & \delta x \\ \delta \theta & 1 & \delta y \\ 0 & 0 & 1 \end{bmatrix} \right )
h(qeξ^)=h
cos(qθ)sin(qθ)0−sin(qθ)cos(qθ)0qxqy1
⋅
1δθ0−δθ10δxδy1
由于我们只需要求出残差
E
(
q
)
=
h
(
q
e
ξ
^
)
−
h
(
q
)
E(q) = h\left(q e^{\hat{\xi}}\right) - h(q)
E(q)=h(qeξ^)−h(q)即可，无需把
q
e
ξ
^
q e^{\hat{\xi}}
qeξ^的完整矩阵形式写出来，
h
(
)
h()
h()观测函数只取矩阵中最右边一列前两行坐标，即平移的分量，只用最后一列即可，
q
q
q矩阵的
q
x
,
q
y
q_x,q_y
qx,qy都与
e
ξ
^
e^{\hat{\xi}}
eξ^矩阵中
1
1
1相乘，所以之后能直接消除掉，残差：
E
(
q
)
=
h
(
q
e
ξ
^
)
−
h
(
q
)
=
[
c
o
s
(
q
θ
)
δ
x
−
s
i
n
(
q
θ
)
δ
y
s
i
n
(
q
θ
)
δ
x
+
c
o
s
(
q
θ
)
δ
y
]
=
[
E
1
E
2
]
∈
R
2
×
1
E(q) = h\left(q e^{\hat{\xi}}\right) - h(q)=\begin{bmatrix} cos(q_{\theta}) \delta _ x - sin(q_{\theta}) \delta _ y \\ sin(q_{\theta}) \delta _ x + cos(q_{\theta}) \delta _ y \end{bmatrix} = \begin{bmatrix} E_1 \\ E_2 \end{bmatrix} \in \mathbb{R}^{2 \times 1}
E(q)=h(qeξ^)−h(q)=[cos(qθ)δx−sin(qθ)δysin(qθ)δx+cos(qθ)δy]=[E1E2]∈R2×1
雅可比矩阵：
H
=
∂
E
(
q
)
∂
ξ
=
[
∂
E
1
∂
δ
x
∂
E
1
∂
δ
y
∂
E
1
∂
δ
θ
∂
E
2
∂
δ
x
∂
E
2
∂
δ
y
∂
E
2
∂
δ
θ
]
=
[
c
o
s
(
q
θ
)
−
s
i
n
(
q
θ
)
0
s
i
n
(
q
θ
)
c
o
s
(
q
θ
)
0
]
∈
R
2
×
3
H = \frac{\partial E(q)}{\partial \xi} = \begin{bmatrix} \frac{\partial E_1}{\partial \delta_x} & \frac{\partial E_1}{\partial \delta_y} & \frac{\partial E_1}{\partial \delta_{\theta}} \\ \frac{\partial E_2}{\partial \delta_x} & \frac{\partial E_2}{\partial \delta_y} & \frac{\partial E_2}{\partial \delta_{\theta}} \end{bmatrix} = \begin{bmatrix} cos(q_{\theta}) & -sin(q_{\theta}) & 0\\ sin(q_{\theta}) & cos(q_{\theta}) & 0 \end{bmatrix} \in \mathbb{R}^{2 \times 3}
H=∂ξ∂E(q)=[∂δx∂E1∂δx∂E2∂δy∂E1∂δy∂E2∂δθ∂E1∂δθ∂E2]=[cos(qθ)sin(qθ)−sin(qθ)cos(qθ)00]∈R2×3
现在扩展到三维空间情况：
假设待优化变量 q = [ x y z α β γ ] q = \begin{bmatrix} x \\ y \\ z \\ \alpha \\ \beta \\ \gamma \end{bmatrix} q= xyzαβγ ，其中 α , β , γ \alpha, \beta, \gamma α,β,γ为欧拉角（ZYX顺序），分别绕Z轴旋转 γ \gamma γ，绕Y轴旋转 β \beta β，绕X轴旋转 α \alpha α，观测函数 h ( q ) = [ x y z ] h(q) = \begin{bmatrix} x \\ y \\ z \end{bmatrix} h(q)= xyz 。欧拉角 α , β , γ \alpha, \beta, \gamma α,β,γ转成旋转矩阵 R R R:
R
=
R
Z
(
γ
)
R
Y
(
β
)
R
X
(
α
)
=
[
c
o
s
γ
−
s
i
n
γ
0
s
i
n
γ
c
o
s
γ
0
0
0
1
]
[
c
o
s
β
0
s
i
n
β
0
1
0
−
s
i
n
β
0
c
o
s
β
]
[
1
0
0
0
c
o
s
α
−
s
i
n
α
0
s
i
n
α
c
o
s
α
]
=
[
c
o
s
γ
c
o
s
β
c
o
s
γ
s
i
n
β
s
i
n
α
−
s
i
n
γ
c
o
s
α
c
o
s
γ
s
i
n
β
c
o
s
α
+
s
i
n
γ
s
i
n
α
s
i
n
γ
c
o
s
β
s
i
n
γ
s
i
n
β
s
i
n
α
+
c
o
s
γ
c
o
s
α
s
i
n
γ
s
i
n
β
c
o
s
α
−
c
o
s
γ
s
i
n
α
−
s
i
n
β
c
o
s
β
s
i
n
α
c
o
s
β
c
o
s
α
]
\begin{align} R & = R_Z(\gamma) R_Y(\beta) R_X(\alpha)\\ & = \left[\begin{array}{ccc} cos \gamma & -sin \gamma & 0 \\ sin \gamma & cos \gamma & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ccc} cos \beta & 0 & sin \beta \\ 0 & 1 & 0 \\ -sin \beta & 0 & cos \beta \end{array}\right]\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & cos \alpha & -sin \alpha \\ 0 & sin \alpha & cos \alpha \end{array}\right] \\ & = \left[\begin{array}{ccc} cos \gamma cos \beta & cos \gamma sin \beta sin \alpha-sin \gamma cos \alpha & cos \gamma sin \beta cos \alpha+sin \gamma sin \alpha \\ sin \gamma cos \beta & sin \gamma sin \beta sin \alpha + cos \gamma cos \alpha & sin \gamma sin \beta cos \alpha-cos \gamma sin \alpha \\ -sin \beta & cos \beta sin \alpha & cos \beta cos \alpha \end{array}\right] \end{align}
R=RZ(γ)RY(β)RX(α)=
cosγsinγ0−sinγcosγ0001
cosβ0−sinβ010sinβ0cosβ
1000cosαsinα0−sinαcosα
=
cosγcosβsinγcosβ−sinβcosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinαcosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβcosα
优化变量
q
q
q的变换矩阵形式：
q
=
[
c
o
s
γ
c
o
s
β
c
o
s
γ
s
i
n
β
s
i
n
α
−
s
i
n
γ
c
o
s
α
c
o
s
γ
s
i
n
β
c
o
s
α
+
s
i
n
γ
s
i
n
α
x
s
i
n
γ
c
o
s
β
s
i
n
γ
s
i
n
β
s
i
n
α
+
c
o
s
γ
c
o
s
α
s
i
n
γ
s
i
n
β
c
o
s
α
−
c
o
s
γ
s
i
n
α
y
−
s
i
n
β
c
o
s
β
s
i
n
α
c
o
s
β
c
o
s
α
z
0
0
0
1
]
∈
S
E
(
3
)
q = \left[\begin{array}{cccc} cos \gamma cos \beta & cos \gamma sin \beta sin \alpha-sin \gamma cos \alpha & cos \gamma sin \beta cos \alpha+sin \gamma sin \alpha & x\\ sin \gamma cos \beta & sin \gamma sin \beta sin \alpha + cos \gamma cos \alpha & sin \gamma sin \beta cos \alpha-cos \gamma sin \alpha & y\\ -sin \beta & cos \beta sin \alpha & cos \beta cos \alpha & z \\ 0 & 0 & 0 & 1 \end{array}\right] \in SE(3)
q=
cosγcosβsinγcosβ−sinβ0cosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinα0cosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβcosα0xyz1
∈SE(3)
扰动量
ξ
=
[
δ
x
δ
y
δ
z
δ
α
δ
β
δ
γ
]
\xi = \begin{bmatrix} \delta x \\ \delta y \\ \delta z \\ \delta \alpha \\ \delta \beta \\ \delta \gamma \end{bmatrix}
ξ=
δxδyδzδαδβδγ
，则进行指数映射后的
e
ξ
^
≈
[
1
δ
α
δ
β
−
δ
γ
δ
β
+
δ
α
δ
γ
δ
x
δ
γ
1
+
δ
α
δ
β
δ
γ
δ
γ
δ
β
−
δ
α
δ
y
−
δ
β
δ
α
1
δ
z
0
0
0
1
]
e^{\hat{\xi}} \approx \begin{bmatrix} 1 & \delta \alpha \delta \beta - \delta \gamma & \delta \beta + \delta \alpha \delta \gamma & \delta x \\ \delta \gamma & 1 + \delta \alpha \delta \beta \delta \gamma & \delta \gamma \delta \beta - \delta \alpha &\delta y \\ -\delta \beta & \delta \alpha & 1 & \delta z \\ 0 & 0 & 0 & 1 \end{bmatrix}
eξ^≈
1δγ−δβ0δαδβ−δγ1+δαδβδγδα0δβ+δαδγδγδβ−δα10δxδyδz1
残差可以写成：
E
(
q
)
=
h
(
q
e
ξ
^
)
−
h
(
q
)
=
[
c
o
s
γ
c
o
s
β
⋅
δ
x
+
(
c
o
s
γ
s
i
n
β
s
i
n
α
−
s
i
n
γ
c
o
s
α
)
⋅
δ
y
+
(
c
o
s
γ
s
i
n
β
c
o
s
α
+
s
i
n
γ
s
i
n
α
)
⋅
δ
z
s
i
n
γ
c
o
s
β
⋅
δ
x
+
(
s
i
n
γ
s
i
n
β
s
i
n
α
+
c
o
s
γ
c
o
s
α
)
⋅
δ
y
+
(
s
i
n
γ
s
i
n
β
c
o
s
α
−
c
o
s
γ
s
i
n
α
)
⋅
δ
z
−
s
i
n
β
⋅
δ
x
+
c
o
s
β
s
i
n
α
⋅
δ
y
+
c
o
s
β
c
o
s
α
⋅
δ
z
]
=
[
E
1
E
2
E
3
]
∈
R
3
×
1
\begin{align} E(q) & = h\left(q e^{\hat{\xi}}\right) - h(q) \\ & = \begin{bmatrix} cos \gamma cos \beta \cdot \delta _x + (cos \gamma sin \beta sin \alpha-sin \gamma cos \alpha) \cdot \delta y + (cos \gamma sin \beta cos \alpha+sin \gamma sin \alpha) \cdot \delta z \\ sin \gamma cos \beta \cdot \delta _x + (sin \gamma sin \beta sin \alpha + cos \gamma cos \alpha) \cdot \delta y + (sin \gamma sin \beta cos \alpha-cos \gamma sin \alpha) \cdot \delta z \\ -sin \beta \cdot \delta _x + cos \beta sin \alpha \cdot \delta _y + cos \beta cos \alpha \cdot \delta _z \\ \end{bmatrix} \\ & = \begin{bmatrix} E_1 \\ E_2 \\ E_3 \end{bmatrix} \in \mathbb{R}^{3 \times 1} \end{align}
E(q)=h(qeξ^)−h(q)=
cosγcosβ⋅δx+(cosγsinβsinα−sinγcosα)⋅δy+(cosγsinβcosα+sinγsinα)⋅δzsinγcosβ⋅δx+(sinγsinβsinα+cosγcosα)⋅δy+(sinγsinβcosα−cosγsinα)⋅δz−sinβ⋅δx+cosβsinα⋅δy+cosβcosα⋅δz
=
E1E2E3
∈R3×1
雅可比矩阵：
H
=
∂
E
(
q
)
∂
ξ
=
[
∂
E
1
∂
δ
x
∂
E
1
∂
δ
y
∂
E
1
∂
δ
z
∂
E
1
∂
δ
α
∂
E
1
∂
δ
β
∂
E
1
∂
δ
γ
∂
E
2
∂
δ
x
∂
E
2
∂
δ
y
∂
E
2
∂
δ
z
∂
E
2
∂
δ
α
∂
E
2
∂
δ
β
∂
E
2
∂
δ
γ
∂
E
3
∂
δ
x
∂
E
3
∂
δ
y
∂
E
3
∂
δ
z
∂
E
3
∂
δ
α
∂
E
3
∂
δ
β
∂
E
3
∂
δ
γ
]
∈
R
3
×
6
\begin{align} H & = \frac{\partial E(q)}{\partial \xi} \\ & = \begin{bmatrix} \frac{\partial E_1}{\partial \delta_x} & \frac{\partial E_1}{\partial \delta_y} & \frac{\partial E_1}{\partial \delta_z} & \frac{\partial E_1}{\partial \delta_{\alpha}} & \frac{\partial E_1}{\partial \delta_{\beta}} & \frac{\partial E_1}{\partial \delta_{\gamma}} \\ \frac{\partial E_2}{\partial \delta_x} & \frac{\partial E_2}{\partial \delta_y} & \frac{\partial E_2}{\partial \delta_z} & \frac{\partial E_2}{\partial \delta_{\alpha}} & \frac{\partial E_2}{\partial \delta_{\beta}} & \frac{\partial E_2}{\partial \delta_{\gamma}} \\ \frac{\partial E_3}{\partial \delta_x} & \frac{\partial E_3}{\partial \delta_y} & \frac{\partial E_3}{\partial \delta_z} & \frac{\partial E_3}{\partial \delta_{\alpha}} & \frac{\partial E_3}{\partial \delta_{\beta}} & \frac{\partial E_3}{\partial \delta_{\gamma}} \\ \end{bmatrix} \in \mathbb{R}^{3 \times 6} \end{align}
H=∂ξ∂E(q)=
∂δx∂E1∂δx∂E2∂δx∂E3∂δy∂E1∂δy∂E2∂δy∂E3∂δz∂E1∂δz∂E2∂δz∂E3∂δα∂E1∂δα∂E2∂δα∂E3∂δβ∂E1∂δβ∂E2∂δβ∂E3∂δγ∂E1∂δγ∂E2∂δγ∂E3
∈R3×6
由于雅可比矩阵太长过于复杂就不写了，带入公式即可求出，残差分量对于旋转的导数过长。
下面是采用平面的自定义一元因子
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
// 类名 : 继承于NoiseModelFactor1<优化变量类型>
class UnaryFactor : public NoiseModelFactor1<Pose2>
{
// 观测值(x,y),当然也可以采用Point2
double mx_, my_;
public:
// 需要重写这个函数来定义误差和雅可比矩阵
using NoiseModelFactor1<Pose2>::evaluateError;
// 因子的智能指针
typedef std::shared_ptr<UnaryFactor> shared_ptr;
// 一元因子的构造函数， (key , 测量值， 噪声模型)
UnaryFactor(Key j, double x, double y, const SharedNoiseModel &model) : NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
~UnaryFactor() override {}
Vector evaluateError(const Pose2 &q, boost::optional<Matrix &> H = boost::none) const override
{
// 雅可比矩阵，在切空间等于右手定则的旋转矩阵
// H = [ cos(q.theta) -sin(q.theta) 0 ]
// [ sin(q.theta) cos(q.theta) 0 ]
const Rot2 &R = q.rotation();
if (H)
{
(*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished();
}
// 返回误差
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
// The second is a 'clone' function that allows the factor to be copied. Under most
// circumstances, the following code that employs the default copy constructor should
// work fine.
gtsam::NonlinearFactor::shared_ptr clone() const override
{
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this)));
}
}; // UnaryFactor
int main(int argc, char **argv)
{
// 1. 创建因子图融合，后续加入因子
NonlinearFactorGraph graph;
// 2a. 加入里程计因子（二元因子）
// 里程计因子的噪声模型
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 创建里程计因子（即二元边因子）
graph.emplace_shared<BetweenFactor<Pose2>>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2>>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
// 2b. 加入自定义的一元因子测量值
auto unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 其中需要噪声模型维度需要和测量值的维度保持一致，在XY方向标准差为10cm// 10cm std on x,y
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
graph.print("\nFactor Graph:\n"); // print
// 3. 添加变量节点的初始值
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial Estimate:\n"); // print
// LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
// 4. 采用高斯牛顿方法进行优化
GaussNewtonOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final Result:\n");
// 5. 计算并打印所有变量的边际协方差
Marginals marginals(graph, result);
cout << "x1 covariance:\n"
<< marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n"
<< marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n"
<< marginals.marginalCovariance(3) << endl;
return 0;
}
2864
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
