0. 简介
作为一个SLAMer来说，整个SLAM过程可以分为预处理（包含相机、激光的畸变去除，以及IMU的预积分都属于这部分操作），前端优化（这部分主要做的就是帧间匹配【scan-to-scan】，帧与地图匹配【scan-to-submap】，划窗优化，提取关键帧），后端优化（回环检测，后端地图优化）这些步骤。而上面这些我已经讲了很多了，比如划窗优化这块也是给出了对应的代码，我们这里将从ceres，g2o，gtsam这三个层面来分析后端优化的能力。
1. ceres完成后端优化
1.1 pose_graph_3d.cpp
下面的是一个构建非线性最小二乘优化问题的函数，当中使用了Ceres库来进行非线性最小二乘优化。代码中定义了一些函数和类来构建优化问题、设置参数化方式、求解优化问题以及输出优化结果。
#include <fstream>
#include <iostream>
#include <string>
#include "gflags/gflags.h"
#include "glog/logging.h"
#include <pose_graph_3d_error_term.h>
#include <read_g2o.h>
#include <types.h>
namespace pose_graph {
// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
void BuildOptimizationProblem(const VectorOfConstraints& constraints,
MapOfPoses* poses,
ceres::Problem* problem) {
CHECK(poses != NULL);//检查输入参数poses和problem是否为空，如果为空则返回
CHECK(problem != NULL);
if (constraints.empty()) {
LOG(INFO) << "No constraints, no problem to optimize.";
return;
}
//创建一个空的损失函数loss_function和一个四元数局部参数化对象quaternion_local_parameterization
ceres::LossFunction* loss_function = NULL;
ceres::LocalParameterization* quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
for (VectorOfConstraints::const_iterator constraints_iter =
constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {//遍历输入的约束向量constraints中的每一个约束
const Constraint3d& constraint = *constraints_iter;
// 对于每一个约束，首先在poses中查找起点和终点的姿态
MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
CHECK(pose_begin_iter != poses->end())
<< "Pose with ID: " << constraint.id_begin << " not found.";
MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
CHECK(pose_end_iter != poses->end())
<< "Pose with ID: " << constraint.id_end << " not found.";
const Eigen::Matrix<double, 6, 6> sqrt_information =
constraint.information.llt().matrixL();//构造一个6x6的信息矩阵的平方根
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function =
PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);//创建一个误差项的代价函数cost_function
problem->AddResidualBlock(cost_function, loss_function,
pose_begin_iter->second.p.data(),
pose_begin_iter->second.q.coeffs().data(),
pose_end_iter->second.p.data(),
pose_end_iter->second.q.coeffs().data());//将误差项添加到优化问题中，使用起点和终点的位置和四元数作为参数
problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
quaternion_local_parameterization);//设置起点和终点的四元数参数使用quaternion_local_parameterization进行参数化
problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
quaternion_local_parameterization);
}
// The pose graph optimization problem has six DOFs that are not fully
// constrained. This is typically referred to as gauge freedom. You can apply
// a rigid body transformation to all the nodes and the optimization problem
// will still have the exact same cost. The Levenberg-Marquardt algorithm has
// internal damping which mitigates this issue, but it is better to properly
// constrain the gauge freedom. This can be done by setting one of the poses
// as constant so the optimizer cannot change it.
MapOfPoses::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());//将起点的位置和四元数参数设置为常量，以固定一个初始姿态
}
//LocalParameterizationSE3类中的成员函数包括Plus、ComputeJacobian、GlobalSize和LocalSize
class LocalParameterizationSE3 : public ceres::LocalParameterization {
public:
virtual ~LocalParameterizationSE3() {}
// SE3 plus operation for Ceres
//
// T * exp(x)
// SE3的加法运算，其参数为T_raw和delta_raw，分别表示初始变换矩阵T和增量矩阵delta。函数通过Sophus库中的exp函数将增量矩阵delta转换为SE3变换，并将结果保存在T_plus_delta_raw中。
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::Vector6d const> const delta(delta_raw);
Eigen::Map<Sophus::SE3d> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * Sophus::SE3d::exp(delta);
return true;
}
// Jacobian of SE3 plus operation for Ceres
//
// Dx T * exp(x) with x=0
// ComputeJacobian函数计算SE3加法运算的雅可比矩阵。参数T_raw表示初始变换矩阵T，jacobian_raw用于保存计算结果。函数通过Sophus库中的Dx_this_mul_exp_x_at_0函数计算雅可比矩阵，并将结果保存在jacobian_raw中。
virtual bool ComputeJacobian(double const* T_raw,
double* jacobian_raw) const {
Eigen::Map<Sophus::SE3d const> T(T_raw);
Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_raw);
jacobian = T.Dx_this_mul_exp_x_at_0();
return true;
}
// GlobalSize函数返回全局参数的维度
virtual int GlobalSize() const { return Sophus::SE3d::num_parameters; }
// LocalSize函数返回局部参数的维度
virtual int LocalSize() const { return Sophus::SE3d::DoF; }
}; // class LocalParameterizationSE3
//构建一个优化问题的函数。函数的输入是一组约束（constraints）、一组位姿（poses）和一个ceres::Problem对象（problem）。函数的目标是将约束添加到问题中，以便通过优化来调整位姿，使约束得到满足。
void BuildOptimizationProblem_SE3(const VectorOfConstraints_SE3& constraints,
MapOfPoses_SE3* poses,
ceres::Problem* problem) {
CHECK(poses != NULL);
CHECK(problem != NULL);
if (constraints.empty()) {
LOG(INFO) << "No constraints, no problem to optimize.";
return;
}
ceres::LossFunction* loss_function = NULL;//代码创建了一个ceres::LossFunction对象，用于定义优化问题的损失函数
// ceres::LocalParameterization* quaternion_local_parameterization =
// new EigenQuaternionParameterization;
ceres::LocalParameterization* SE3_parameterization =
new LocalParameterizationSE3;//定义位姿参数的局部参数化方式。在这个例子中，使用的是LocalParameterizationSE3，它将位姿参数表示为李代数形式。
for (VectorOfConstraints_SE3::const_iterator constraints_iter =
constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {// 通过约束的id在位姿列表中查找对应的起始位姿和结束位姿
const Constraint3d_SE3& constraint = *constraints_iter;
MapOfPoses_SE3::iterator pose_begin_iter = poses->find(constraint.id_begin);
CHECK(pose_begin_iter != poses->end())
<< "Pose with ID: " << constraint.id_begin << " not found.";
MapOfPoses_SE3::iterator pose_end_iter = poses->find(constraint.id_end);
CHECK(pose_end_iter != poses->end())
<< "Pose with ID: " << constraint.id_end << " not found.";
const Eigen::Matrix<double, 6, 6> sqrt_information =
constraint.information.llt().matrixL();//将约束的信息矩阵进行Cholesky分解，得到其平方根信息矩阵
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function =
PoseGraph3dErrorTerm_SE3::Create(constraint.t_be, sqrt_information);//创建一个PoseGraph3dErrorTerm_SE3类型的cost_function，用于表示约束的误差项
problem->AddResidualBlock(cost_function, loss_function,
pose_begin_iter->second.pose.data(),
pose_end_iter->second.pose.data());//cost_function添加到问题中作为一个残差块
problem->SetParameterization(pose_begin_iter->second.pose.data(),
SE3_parameterization);
problem->SetParameterization(pose_end_iter->second.pose.data(),
SE3_parameterization);//起始位姿和结束位姿设置参数化方式，即使用之前创建的SE3_parameterization。
}
MapOfPoses_SE3::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.pose.data());
// problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
//用于求解优化问题的函数。函数的输入参数是一个指向ceres::Problem对象的指针。ceres::Problem是一个用于构建优化问题的类，它将待优化的变量、残差函数和约束等信息组织在一起。
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem* problem) {
CHECK(problem != NULL);
ceres::Solver::Options options;
options.max_num_iterations = 200;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;//创建一个ceres::Solver::Summary对象summary，用于存储求解器的结果信息
ceres::Solve(options, problem, &summary);
std::cout << summary.FullReport() << '\n';
return summary.IsSolutionUsable();//std::cout输出summary对象的完整报告，并返回summary对象的IsSolutionUsable()方法的返回值
}
// Output the poses to the file with format: id x y z q_x q_y q_z q_w.
// 用于将一组位姿数据输出到文件的函数
bool OutputPoses(const std::string& filename, const MapOfPoses& poses) {
std::fstream outfile;
outfile.open(filename.c_str(), std::istream::out);
if (!outfile) {
LOG(ERROR) << "Error opening the file: " << filename;
return false;
}
for (std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
const_iterator poses_iter = poses.begin();
poses_iter != poses.end(); ++poses_iter) {
const std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
value_type& pair = *poses_iter;
outfile << pair.first << " " << pair.second.p.transpose() << " "
<< pair.second.q.x() << " " << pair.second.q.y() << " "
<< pair.second.q.z() << " " << pair.second.q.w() << '\n';
}
return true;
}
bool OutputPoses_SE3(const std::string& filename, const MapOfPoses_SE3& poses) {
std::fstream outfile;
outfile.open(filename.c_str(), std::istream::out);
if (!outfile) {
LOG(ERROR) << "Error opening the file: " << filename;
return false;
}
for (std::map<int, Pose3d_SE3, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d_SE3> > >::
const_iterator poses_iter = poses.begin();
poses_iter != poses.end(); ++poses_iter) {
const std::map<int, Pose3d_SE3, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
value_type& pair = *poses_iter;
outfile << pair.first << " "
<< pair.second.pose.translation().transpose() << " "
<< pair.second.pose.unit_quaternion().x() << " "
<< pair.second.pose.unit_quaternion().y() << " "
<< pair.second.pose.unit_quaternion().z() << " "
<< pair.second.pose.unit_quaternion().w() << '\n';
}
return true;
}
} // namespace pose_graph
1.2 pose_graph_3d_error_term.h
该代码实现了两个不同的误差项类：PoseGraph3dErrorTerm和PoseGraph3dErrorTerm_SE3。这两个类分别用于处理基于欧氏空间和李代数的位姿表示。
PoseGraph3dErrorTerm类计算了两个位姿之间的相对姿态测量的误差。该类接受一个测量的位姿t_ab_measured和测量信息矩阵的平方根sqrt_information作为输入。它通过计算当前估计的位姿和测量之间的误差，并根据测量的不确定性对误差进行加权，从而计算出残差。
PoseGraph3dErrorTerm_SE3类与PoseGraph3dErrorTerm类类似，但是它使用李代数表示位姿。它接受一个测量的位姿t_ab_measured和测量信息矩阵的平方根sqrt_information作为输入。它通过计算当前估计的位姿和测量之间的误差，并根据测量的不确定性对误差进行加权，从而计算出残差。
除了误差项类之外，代码还提供了用于构建优化问题和求解优化问题的函数。BuildOptimizationProblem函数用于根据约束和位姿信息构建优化问题。SolveOptimizationProblem函数用于求解优化问题。OutputPoses函数用于将优化结果保存到文件中。
#ifndef INCLUDE_POSE_GRAPH_3D_ERROR_TERM_H_
#define INCLUDE_POSE_GRAPH_3D_ERROR_TERM_H_
#include "ceres/ceres.h"
#include "Eigen/Core"
#include <sophus/se3.hpp>
#include <types.h>
namespace pose_graph {
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement. We have two poses x_a
// and x_b. Through sensor measurements we can measure the transformation of
// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
// between the current estimate of the poses and the measurement.
//
// In this formulation, we have chosen to represent the rigid transformation as
// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
// [x, y, z, w].
// The estimated measurement is:
// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
// [ q_ab ] [ q_a^{-1] * q_b ]
//
// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
// quaternion. Now we can compute an error metric between the estimated and
// measurement transformation. For the orientation error, we will use the
// standard multiplicative error resulting in:
//
// error = [ p_ab - \hat{p}_ab ]
// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
//
// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
// the measurement has an uncertainty associated with how accurate it is, we
// will weight the errors by the square root of the measurement information
// matrix:
//
// residuals = I^{1/2) * error
// where I is the information matrix which is the inverse of the covariance.
// PoseGraph3dErrorTerm
class PoseGraph3dErrorTerm {
public:
PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information)
: t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}//在构造函数中，传入了测量的相对位姿t_ab_measured和测量的信息矩阵的平方根sqrt_information。
template <typename T>
bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
const T* const p_b_ptr, const T* const q_b_ptr,
T* residuals_ptr) const {
// 传入了优化变量p_a_ptr、q_a_ptr、p_b_ptr和q_b_ptr的指针，以及残差residuals_ptr的指针。这些指针指向了实际的优化变量和残差。
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_a(p_a_ptr);//Eigen库的Map函数将指针映射为Eigen类型的变量
Eigen::Map<const Eigen::Quaternion<T> > q_a(q_a_ptr);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_b(p_b_ptr);
Eigen::Map<const Eigen::Quaternion<T> > q_b(q_b_ptr);
// Compute the relative transformation between the two frames.
Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();//计算了两个帧之间的相对变换
Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
// Represent the displacement between the two frames in the A frame.
Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);//其中q_a_inverse是q_a的共轭，*表示四元数乘法
// Compute the error between the two orientation estimates.
Eigen::Quaternion<T> delta_q =
t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();//两个姿态估计之间的误差
// Compute the residuals.
// [ position ] [ delta_p ]
// [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
residuals.template block<3, 1>(0, 0) =
p_ab_estimated - t_ab_measured_.p.template cast<T>();
residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();//计算残差,其中delta_q是前面计算的误差四元数的向量部分。
// Scale the residuals by the measurement uncertainty.
residuals.applyOnTheLeft(sqrt_information_.template cast<T>());//残差乘以测量不确定性的平方根
return true;
}
static ceres::CostFunction* Create(
const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information) {
return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //保证对齐内存，使用了EIGEN_MAKE_ALIGNED_OPERATOR_NEW宏。
private:
// The measurement for the position of B relative to A in the A frame.
const Pose3d t_ab_measured_;
// The square root of the measurement information matrix.
const Eigen::Matrix<double, 6, 6> sqrt_information_;
};
// PoseGraph3dErrorTerm_SE3
class PoseGraph3dErrorTerm_SE3 {//用于实现在3D位姿图优化中使用的误差项
public:
PoseGraph3dErrorTerm_SE3(const Pose3d_SE3& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information)
: t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}//t_ab_measured表示测量得到的相对位姿（Pose3d_SE3类型），sqrt_information表示测量信息的平方根矩阵
template <typename T>
bool operator()(const T* const pose_a_ptr, const T* const pose_b_ptr,
T* residuals_ptr) const {
Eigen::Map<Sophus::SE3<T> const> const pose_a(pose_a_ptr);
Eigen::Map<Sophus::SE3<T> const> const pose_b(pose_b_ptr);
// Compute the relative transformation between the two frames.
Eigen::Quaternion<T> q_a_inverse = pose_a.unit_quaternion().conjugate();
Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * pose_b.unit_quaternion();
// Represent the displacement between the two frames in the A frame.
Eigen::Matrix<T, 3, 1> p_ab_estimated
= q_a_inverse * (pose_b.translation() - pose_a.translation());
// Compute the error between the two orientation estimates.
Eigen::Quaternion<T> delta_q =
t_ab_measured_.pose.unit_quaternion().template cast<T>() * q_ab_estimated.conjugate();
// Compute the residuals.
// [ position ] [ delta_p ]
// [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
residuals.template block<3, 1>(0, 0) =
p_ab_estimated - t_ab_measured_.pose.translation().template cast<T>();
residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
// Scale the residuals by the measurement uncertainty.
residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
return true;
}
static ceres::CostFunction* Create(
const Pose3d_SE3& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information) {
return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm_SE3,
6,
SE3::num_parameters,
SE3::num_parameters>(
new PoseGraph3dErrorTerm_SE3(t_ab_measured, sqrt_information));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The measurement for the position of B relative to A in the A frame.
const Pose3d_SE3 t_ab_measured_;
// The square root of the measurement information matrix.
const Eigen::Matrix<double, 6, 6> sqrt_information_;
};
void BuildOptimizationProblem(const VectorOfConstraints& constraints,
MapOfPoses* poses,
ceres::Problem* problem);
void BuildOptimizationProblem_SE3(const VectorOfConstraints_SE3& constraints,
MapOfPoses_SE3* poses,
ceres::Problem* problem);
bool SolveOptimizationProblem(ceres::Problem* problem);
bool OutputPoses(const std::string& filename, const MapOfPoses& poses);
bool OutputPoses_SE3(const std::string& filename, const MapOfPoses_SE3& poses);
} // namespace pose_graph
#endif // INCLUDE_POSE_GRAPH_3D_ERROR_TERM_H_
1.3 pose_graph.cpp
段代码是一个使用Ceres库进行图优化的示例。代码的功能是读取一个g2o格式的位姿图定义文件，然后使用Ceres库对该位姿图进行优化
//#include <types.h>
#include <pose_graph_3d_error_term.h>
#include <read_g2o.h>
#include "gflags/gflags.h"
#include "glog/logging.h"
DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);//初始化了日志模块
// CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);//解析命令行参数，其中FLAGS_input是一个命令行参数，用于指定位姿图定义文件的路径。
gflags::ParseCommandLineFlags(&argc, &argv, true);
CHECK(FLAGS_input != "") << "Need to specify the filename to read.";
pose_graph::MapOfPoses poses;
pose_graph::VectorOfConstraints constraints;
pose_graph::MapOfPoses_SE3 poses_SE3;
pose_graph::VectorOfConstraints_SE3 constraints_SE3;
CHECK(pose_graph::ReadG2oFile(FLAGS_input, &poses, &constraints))//读取位姿图数据，并检查是否读取成功。
<< "Error reading the file: " << FLAGS_input;
CHECK(pose_graph::ReadG2oFile(FLAGS_input, &poses_SE3, &constraints_SE3))
<< "Error reading the file: " << FLAGS_input;
std::cout << "Number of poses: " << poses.size() << '\n';
std::cout << "Number of constraints: " << constraints.size() << '\n';
CHECK(pose_graph::OutputPoses("poses_original.txt", poses))
<< "Error outputting to poses_original.txt";
ceres::Problem problem_1, problem_2;//定义了两个Ceres的Problem对象problem_1和problem_2，用于存储优化问题。
// Use Eigen Vector and Quarternion
pose_graph::BuildOptimizationProblem(constraints, &poses, &problem_1);//将位姿图转换为优化问题，并传入problem_1中。
CHECK(pose_graph::SolveOptimizationProblem(&problem_1))//解决优化问题
<< "The solve was not successful, exiting.";
// Use sophus SE3
pose_graph::BuildOptimizationProblem_SE3(constraints_SE3,
&poses_SE3,
&problem_2);//位姿图转换为Sophus库中的SE3优化问题，并传入problem_2中。
CHECK(pose_graph::SolveOptimizationProblem(&problem_2))
<< "The solve was not successful, exiting.";
CHECK(pose_graph::OutputPoses_SE3("poses_optimized.txt", poses_SE3))
<< "Error outputting to poses_original.txt";//将优化后的位姿输出到文件
return 0;
}
2. G2O
该代码文件实现了一个基于G2O的图优化器类G2oGraphOptimizer。它包含以下方法
- 构造函数：根据输入的solver_type创建一个SparseOptimizer对象，并设置为图优化器的算法。如果创建失败，则输出错误信息。
- Optimize方法：进行图优化的主要函数。首先检查图中边的数量是否小于1，如果是则返回false。然后初始化优化、计算初始估计、计算激活错误、设置是否输出详细信息。接着计算优化前的chi2值，进行最大迭代次数的优化，并输出优化结果的相关信息。最后返回true。
- GetOptimizedPose方法：获取优化后的位姿。首先清空输入的deque容器，然后遍历图中的顶点，将每个顶点的估计位姿转换为Eigen::Matrix4f类型，并添加到输入容器中。最后返回true。
- GetNodeNum方法：获取图中顶点的数量。
- AddSe3Node方法：添加一个SE3顶点到图中。根据输入的位姿创建一个VertexSE3对象，并设置顶点的id和估计值。如果need_fix为true，则将顶点设置为固定。最后将顶点添加到图中。
- SetEdgeRobustKernel方法：设置边的鲁棒核函数。将输入的鲁棒核类型和大小保存下来，并将need_robust_kernel_设置为true。
- AddSe3Edge方法：添加一个SE3边到图中。根据输入的顶点索引和相对位姿创建一个EdgeSE3对象，并设置测量值和信息矩阵，将对应的顶点设置为边的顶点，并将边添加到图中。如果need_robust_kernel_为true，则添加鲁棒核函数。
- CalculateSe3EdgeInformationMatrix方法：计算SE3边的信息矩阵。根据输入的噪声向量创建一个单位矩阵，并将对角线上的元素除以对应的噪声值。
- AddRobustKernel方法：添加鲁棒核函数到边上。根据输入的核函数类型和大小创建一个RobustKernel对象，并设置边的鲁棒核函数。
- CalculateDiagMatrix方法：计算对角矩阵。根据输入的噪声向量创建一个单位矩阵，并将对角线上的元素除以对应的噪声值。
- AddSe3PriorXYZEdge方法：添加一个SE3先验位置边到图中。根据输入的顶点索引和位置向量创建一个EdgeSE3PriorXYZ对象，并设置测量值和信息矩阵，将对应的顶点设置为边的顶点，并将边添加到图中。
- AddSe3PriorQuaternionEdge方法：添加一个SE3先验姿态边到图中。根据输入的顶点索引和四元数创建一个EdgeSE3PriorQuat对象，并设置测量值和信息矩阵，将对应的顶点设置为边的顶点，并将边添加到图中。
#include "lidar_localization/models/graph_optimizer/g2o/g2o_graph_optimizer.hpp"
#include "glog/logging.h"
#include "lidar_localization/tools/tic_toc.hpp"
namespace lidar_localization {
G2oGraphOptimizer::G2oGraphOptimizer(const std::string &solver_type) {
graph_ptr_.reset(new g2o::SparseOptimizer());
g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();
g2o::OptimizationAlgorithmProperty solver_property;
g2o::OptimizationAlgorithm *solver = solver_factory->construct(solver_type, solver_property);
graph_ptr_->setAlgorithm(solver);
if (!graph_ptr_->solver()) {
LOG(ERROR) << "G2O 优化器创建失败！";
}
robust_kernel_factory_ = g2o::RobustKernelFactory::instance();
}
bool G2oGraphOptimizer::Optimize() {
static int optimize_cnt = 0;
if(graph_ptr_->edges().size() < 1) {
return false;
}
TicToc optimize_time;
graph_ptr_->initializeOptimization();
graph_ptr_->computeInitialGuess();
graph_ptr_->computeActiveErrors();
graph_ptr_->setVerbose(false);
double chi2 = graph_ptr_->chi2();
int iterations = graph_ptr_->optimize(max_iterations_num_);
LOG(INFO) << std::endl << "------ 完成第 " << ++optimize_cnt << " 次后端优化 -------" << std::endl
<< "顶点数：" << graph_ptr_->vertices().size() << ", 边数： " << graph_ptr_->edges().size() << std::endl
<< "迭代次数： " << iterations << "/" << max_iterations_num_ << std::endl
<< "用时：" << optimize_time.toc() << std::endl
<< "优化前后误差变化：" << chi2 << "--->" << graph_ptr_->chi2()
<< std::endl << std::endl;
return true;
}
bool G2oGraphOptimizer::GetOptimizedPose(std::deque<Eigen::Matrix4f>& optimized_pose) {
optimized_pose.clear();
int vertex_num = graph_ptr_->vertices().size();
for (int i = 0; i < vertex_num; i++) {
g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(graph_ptr_->vertex(i));
Eigen::Isometry3d pose = v->estimate();
optimized_pose.push_back(pose.matrix().cast<float>());
}
return true;
}
int G2oGraphOptimizer::GetNodeNum() {
return graph_ptr_->vertices().size();
}
void G2oGraphOptimizer::AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix) {
g2o::VertexSE3 *vertex(new g2o::VertexSE3());
vertex->setId(graph_ptr_->vertices().size());
vertex->setEstimate(pose);
if (need_fix) {
vertex->setFixed(true);
}
graph_ptr_->addVertex(vertex);
}
void G2oGraphOptimizer::SetEdgeRobustKernel(std::string robust_kernel_name,
double robust_kernel_size) {
robust_kernel_name_ = robust_kernel_name;
robust_kernel_size_ = robust_kernel_size;
need_robust_kernel_ = true;
}
void G2oGraphOptimizer::AddSe3Edge(int vertex_index1,
int vertex_index2,
const Eigen::Isometry3d &relative_pose,
const Eigen::VectorXd noise) {
Eigen::MatrixXd information_matrix = CalculateSe3EdgeInformationMatrix(noise);
g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(graph_ptr_->vertex(vertex_index1));
g2o::VertexSE3* v2 = dynamic_cast<g2o::VertexSE3*>(graph_ptr_->vertex(vertex_index2));
g2o::EdgeSE3 *edge(new g2o::EdgeSE3());
edge->setMeasurement(relative_pose);
edge->setInformation(information_matrix);
edge->vertices()[0] = v1;
edge->vertices()[1] = v2;
graph_ptr_->addEdge(edge);
if (need_robust_kernel_) {
AddRobustKernel(edge, robust_kernel_name_, robust_kernel_size_);
}
}
Eigen::MatrixXd G2oGraphOptimizer::CalculateSe3EdgeInformationMatrix(Eigen::VectorXd noise) {
Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(6, 6);
information_matrix = CalculateDiagMatrix(noise);
return information_matrix;
}
void G2oGraphOptimizer::AddRobustKernel(g2o::OptimizableGraph::Edge *edge, const std::string &kernel_type, double kernel_size) {
if (kernel_type == "NONE") {
return;
}
g2o::RobustKernel *kernel = robust_kernel_factory_->construct(kernel_type);
if (kernel == nullptr) {
std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl;
return;
}
kernel->setDelta(kernel_size);
edge->setRobustKernel(kernel);
}
Eigen::MatrixXd G2oGraphOptimizer::CalculateDiagMatrix(Eigen::VectorXd noise) {
Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(noise.rows(), noise.rows());
for (int i = 0; i < noise.rows(); i++) {
information_matrix(i, i) /= noise(i);
}
return information_matrix;
}
void G2oGraphOptimizer::AddSe3PriorXYZEdge(int se3_vertex_index,
const Eigen::Vector3d &xyz,
Eigen::VectorXd noise) {
Eigen::MatrixXd information_matrix = CalculateDiagMatrix(noise);
g2o::VertexSE3 *v_se3 = dynamic_cast<g2o::VertexSE3 *>(graph_ptr_->vertex(se3_vertex_index));
g2o::EdgeSE3PriorXYZ *edge(new g2o::EdgeSE3PriorXYZ());
edge->setMeasurement(xyz);
edge->setInformation(information_matrix);
edge->vertices()[0] = v_se3;
graph_ptr_->addEdge(edge);
}
void G2oGraphOptimizer::AddSe3PriorQuaternionEdge(int se3_vertex_index,
const Eigen::Quaterniond &quat,
Eigen::VectorXd noise) {
Eigen::MatrixXd information_matrix = CalculateSe3PriorQuaternionEdgeInformationMatrix(noise);
g2o::VertexSE3 *v_se3 = dynamic_cast<g2o::VertexSE3 *>(graph_ptr_->vertex(se3_vertex_index));
g2o::EdgeSE3PriorQuat *edge(new g2o::EdgeSE3PriorQuat());
edge->setMeasurement(quat);
edge->setInformation(information_matrix);
edge->vertices()[0] = v_se3;
graph_ptr_->addEdge(edge);
}
//TODO: 姿态观测的信息矩阵尚未添加
Eigen::MatrixXd G2oGraphOptimizer::CalculateSe3PriorQuaternionEdgeInformationMatrix(Eigen::VectorXd noise) {
Eigen::MatrixXd information_matrix;
return information_matrix;
}
} // namespace graph_ptr_optimization
3. GTSAM
下面这段代码实现了一个基于GTSAM（Generalized Trajectory and Sparse Inference for Mapping）的图优化器。GTSAM是一个用于SLAM（Simultaneous Localization and Mapping）和视觉/惯性导航等问题的开源库，提供了一些优化算法和数据结构。
在代码中，首先定义了一个GTSamGraphOptimizer类，构造函数接受一个配置节点作为参数，并从配置中加载一些参数。然后，代码实现了优化函数Optimize()，根据配置中的优化策略选择不同的优化器进行优化。目前支持的优化策略有Levenberg-Marquardt、Gaussian-Newton和Dogleg。优化器会根据配置中的参数设置优化的参数，并对图进行优化。
接下来，代码实现了获取优化后的位姿的函数GetOptimizedPose()，将优化后的位姿转化为Eigen::Matrix4f格式，并存储到给定的双端队列中。
代码还实现了一些其他的函数，包括添加节点和边的函数AddSe3Node()和AddSe3Edge()，以及添加先验信息的函数AddSe3PriorXYZEdge()。这些函数会将节点、边和先验信息添加到图中。
最后，代码还实现了设置鲁棒核的函数SetEdgeRobustKernel()，该函数可以设置边的鲁棒核，用于处理异常值。
总之，这段代码实现了一个基于GTSAM的图优化器，可以根据配置中的参数选择不同的优化策略，并对图进行优化。同时，还提供了一些函数用于添加节点、边和先验信息，以及设置鲁棒核。
#include "mapping_localization/models/graph_optimizer/gtsam/gtsam_graph_optimizer.hpp"
namespace mapping_localization {
GTSamGraphOptimizer::GTSamGraphOptimizer(const YAML::Node &config_node) {
try_load_param(config_node, "verbose", verbose_, verbose_set_[0], verbose_set_);//调用try_load_param函数尝试加载名为"verbose"的参数。try_load_param函数会尝试从config_node中获取名为"verbose"的值
try_load_param(config_node,
"optimization_strategy",
optimization_strategy_,
optimization_strategy_set_[0],
optimization_strategy_set_);//调用try_load_param函数尝试加载名为"optimization_strategy"的参数
try_load_param(config_node, "linear_solver_type", linear_solver_type_, linear_solver_set_[0], linear_solver_set_);
}
bool GTSamGraphOptimizer::Optimize() {
if (optimization_strategy_ == "LevenbergMarquardt") {//代码检查优化策略的值
gtsam::LevenbergMarquardtParams params;
params.setVerbosity(verbose_);
params.setLinearSolverType(linear_solver_type_);
estimates_ = gtsam::LevenbergMarquardtOptimizer(graph_, estimates_, params).optimize();
} else if (optimization_strategy_ == "GaussianNewton") {
gtsam::GaussNewtonParams params;
params.setVerbosity(verbose_);
params.setLinearSolverType(linear_solver_type_);
estimates_ = gtsam::GaussNewtonOptimizer(graph_, estimates_, params).optimize();
} else if (optimization_strategy_ == "GaussianNewton") {
gtsam::DoglegParams params;
params.setVerbosity(verbose_);
params.setLinearSolverType(linear_solver_type_);
estimates_ = gtsam::DoglegOptimizer(graph_, estimates_, params).optimize();
}
return true;
}
// optimized_pose，表示优化后的位姿序列
bool GTSamGraphOptimizer::GetOptimizedPose(std::deque<Eigen::Matrix4f> &optimized_pose) {
optimized_pose.resize(estimates_.size());
for (size_t i = 0; i < estimates_.size(); ++i) {
const gtsam::Pose3 &p = estimates_.at<gtsam::Pose3>(i);
optimized_pose[i] = p.matrix().matrix().template cast<float>();//获取第i个位姿，并将其转换为float类型的矩阵，然后将该矩阵赋值给optimized_pose[i]
}
return true;
}
int GTSamGraphOptimizer::GetNodeNum() { return estimates_.size(); }//用于获取节点数量
// 添加节点、边、鲁棒核
void GTSamGraphOptimizer::SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size) {
need_robust_kernel_ = true;
robust_kernel_name_ = robust_kernel_name;
robust_kernel_size_ = robust_kernel_size;
}
// 函数的作用是向一个图优化器中添加一个SE(3)节点。
void GTSamGraphOptimizer::AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix) {
gtsam::Key key = estimates_.size();
gtsam::Pose3 p(pose.matrix());
estimates_.insert(key, p);
// for fixed vertex, add a prior with pretty small noise
if (need_fix) {//则表示需要将该节点设置为固定节点，即固定其位置。
gtsam::Vector6 fixed_noise;
fixed_noise << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6;
gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Sigmas(fixed_noise);
graph_.addPrior(key, p, prior_noise);// noise表示固定节点的位置的噪声模型。
}
}
void GTSamGraphOptimizer::AddSe3Edge(int vertex_index1,
int vertex_index2,
const Eigen::Isometry3d &relative_pose,
const Eigen::Vector3d &translation_noise,
const Eigen::Vector3d &rotation_noise) {// 该函数的作用是向图优化器中添加一个SE(3)边
gtsam::Pose3 measurement(relative_pose.matrix());
Eigen::Matrix<double, 6, 1> noise;
noise << rotation_noise, translation_noise;
gtsam::noiseModel::Diagonal::shared_ptr noise_model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6(noise));
//判断是否需要使用鲁棒核函数（robust kernel），如果需要，则根据robust_kernel_name的值创建一个对应的鲁棒核函数，并将noise_model作为参数传入，得到一个gtsam库中的noiseModel::Robust::shared_ptr类型的robust_loss。如果不需要使用鲁棒核函数，则直接将noise_model作为参数传入
if (need_robust_kernel_) {
gtsam::noiseModel::Robust::shared_ptr robust_loss;
if (robust_kernel_name_ == "Huber") {
robust_loss = gtsam::noiseModel::Robust::Create(
gtsam::noiseModel::mEstimator::Huber::Create(robust_kernel_size_), noise_model);
}
graph_.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
vertex_index1, vertex_index2, measurement, robust_loss);
} else {
graph_.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
vertex_index1, vertex_index2, measurement, noise_model);
}
}
//用于向一个GTSAM图中添加一个SE3的先验约束
void GTSamGraphOptimizer::AddSe3PriorXYZEdge(int se3_vertex_index,
const Eigen::Vector3d &xyz,
const Eigen::Vector3d &translation_noise) {
Eigen::MatrixXd information_matrix = CalculateDiagMatrix(translation_noise);//函数调用了一个名为CalculateDiagMatrix的函数，该函数接受一个三维向量作为输入，返回一个对角线元素由输入向量确定的矩阵
gtsam::Matrix3 information(information_matrix.matrix());//使用xyz向量的值创建了一个GTSAM的Point3类型的测量值
gtsam::Point3 measurement(xyz.x(), xyz.y(), xyz.z());
graph_.emplace_shared<gtsam::PoseTranslationPrior<gtsam::Pose3>>(
se3_vertex_index, measurement, gtsam::noiseModel::Gaussian::Information(information));//使用之前创建的信息矩阵、SE3顶点索引和测量值创建了一个PoseTranslationPrior对象，并将其添加到图中
}
} // namespace mapping_localization参考链接
https://github.com/tom13133/pose_graph_3d
https://github.com/Little-Potato-1990/localization_in_auto_driving
https://github.com/XiaotaoGuo/modular_mapping_and_localization_framework
评论（0）
您还未登录，请登录后发表或查看评论