0. はじめに
SLAMer として、SLAM プロセス全体は、前処理 (カメラ、レーザー歪み除去、および IMU の事前統合を含むすべての操作のこの部分に属します)、フロントエンドの最適化 (この部分は主にフレーム間処理を行います) に分けることができます。マッチング [スキャン対スキャン]、フレームとマップのマッチング [スキャン対サブマップ]、ウィンドウ最適化、キーフレーム抽出)、バックエンド最適化 (ループ検出、バックエンド マップ最適化) これらのステップ。上記についてはすでに多くのことを説明しましたが、たとえば、ウィンドウ最適化に対応するコードが示されていますが、ここでは ceres、g2o、gtsam の 3 つのレベルからバックエンド最適化機能を分析します。
1. ceres はバックエンドの最適化を完了します
1.1 ポーズグラフ_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 ポーズグラフ_3d_error_term.h
このコードは、PoseGraph3dErrorTerm と PoseGraph3dErrorTerm_SE3 という 2 つの異なるエラー項クラスを実装します。これら 2 つのクラスは、それぞれユークリッド空間とリー代数に基づいてポーズ表現を処理するために使用されます。
PoseGraph3dErrorTerm クラスは、2 つのポーズ間の相対姿勢測定の誤差を計算します。このクラスは、測定された姿勢 t_ab_measured と測定情報行列 sqrt_information の平方根を入力として受け入れます。現在の推定姿勢と測定値の間の誤差を計算し、測定の不確実性に応じて誤差を重み付けすることにより、残差を計算します。
PoseGraph3dErrorTerm_SE3 クラスは PoseGraph3dErrorTerm クラスに似ていますが、ポーズを表現するためにリー代数を使用します。測定された姿勢 t_ab_measured と測定情報行列 sqrt_information の平方根を入力として受け入れます。現在の推定姿勢と測定値の間の誤差を計算し、測定の不確実性に応じて誤差を重み付けすることにより、残差を計算します。
エラー項クラスに加えて、コードは最適化問題を構築および解決するための関数を提供します。BuildOptimizationShould 関数は、制約とポーズ情報に基づいて最適化問題を構築するために使用されます。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 ポーズグラフ.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オブジェクトを作成し、それをグラフオプティマイザーのアルゴリズムに設定します。作成に失敗した場合はエラーメッセージが出力されます。
- 最適化メソッド: グラフ最適化のメイン関数。まず、グラフ内のエッジの数が 1 未満かどうかを確認し、1 未満の場合は false を返します。次に、最適化の初期化、初期推定値の計算、起動誤差の計算、詳細情報を出力するかどうかの設定を行います。次に、最適化の前に chi2 値を計算し、最大反復回数を最適化し、最適化結果に関する情報を出力します。最後に true を返します。
- GetOptimizedPose メソッド: 最適化されたポーズを取得します。まず入力両端キュー コンテナーをクリアし、次にグラフ内の頂点を走査し、各頂点の推定ポーズを 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