ceres学习笔记5——机器人2D/3D位姿图优化pose_graph_2d/3d
1. 2D位姿图优化pose_graph_2d.cc
1.1.问题描述
注意：
- 上面的图片中官网教程写的没有问题！因为官网的教程写的确实是非常细心！注意公式中是Ra^T，Ra确实是节点坐标系到世界坐标系的旋转矩阵，但是由于pb-pa是世界坐标系下面的位置表示，而hat_pab是在A的坐标系下表示的b相对于a的位姿，所以最后做减法的时候需要全部转换到a的坐标系下表示。由于旋转矩阵是正交矩阵，即 R的逆等于R的转置。所以上面的公式中使用的是R的转置，也就是R的逆，也就是世界坐标系到机器人坐标系之间的变换关系，再乘以pb-pa（世界坐标系下表示），得到的就是在a的坐标系下表示的结果！
- 最下面那一行，使用局部参数化，然后后面还有一个Normalize，后来结合程序发现意思是这样的：由于旋转的角度是周期值，所以要把它定义在一个单调的区间内，所以这里就选了[-pi, pi)这个区间，然后这个局部参数化的操作相当于给旋转角度加了约束，让更新后的结果（也就是图中的ψ+δψ）保持在这个区间范围内。而官网教程中的那个函数，就是他们自己定义的一个类，并不是ceres库中定义的。但是ceres库中定义了一个
LocalParameterization类，用途应该和这里的一样，就是为了在变量优化的时候对变量做一些约束。
1.2.程序详解
1.2.1.pose_graph_2d_error_term.h
这个文件中主要是实现了ceres计算残差需要的类，也就是之前自己定义的包含模板()运算符的结构体或类。并且在这个类的定义中，官方注释非常详细的解释了残差的计算公式，结合官网的教程很容易明白。
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement.
//
// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
//
// where r_a is the rotation matrix that rotates a vector represented in frame A
// into the global frame, and Normalize(*) ensures the angles are in the range
// [-pi, pi).
class PoseGraph2dErrorTerm {
public:
PoseGraph2dErrorTerm(double x_ab, // 这些都是实际观测值，也就是位姿图中的边
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information)
: p_ab_(x_ab, y_ab),
yaw_ab_radians_(yaw_ab_radians),
sqrt_information_(sqrt_information) {}
template <typename T>
// 括号运算符中传入的是节点的值，然后利用这些值计算估计的观测值，
// 和实际传入的观测值做差，得到残差
bool operator()(const T* const x_a,
const T* const y_a,
const T* const yaw_a,
const T* const x_b,
const T* const y_b,
const T* const yaw_b,
T* residuals_ptr) const {
const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
// 为什么要声明为Map呢？经常操作，并且给它赋值的数据是Eigen的数据类型？
// map就像相当于C++中的数组，主要是为了和原生的C++数组交互
Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
residuals_map.template head<2>() =
RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
residuals_map(2) = ceres::examples::NormalizeAngle(
(*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
// Scale the residuals by the square root information matrix to account for
// the measurement uncertainty.
residuals_map = sqrt_information_.template cast<T>() * residuals_map;
return true;
}
// 再次看到static，只是为了提高程序紧凑型，因为下面这个函数只给当前这个类用，类似一种全局函数
static ceres::CostFunction* Create(double x_ab,
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information) {
return (new ceres::
AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
new PoseGraph2dErrorTerm(
x_ab, y_ab, yaw_ab_radians, sqrt_information)));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The position of B relative to A in the A frame.
const Eigen::Vector2d p_ab_;
// The orientation of frame B relative to frame A.
const double yaw_ab_radians_;
// The inverse square root of the measurement covariance matrix.
const Eigen::Matrix3d sqrt_information_;
};
注意上面程序中的几个细节：
-
还是原来的套路，类的形参传入的是实际观测的数据点，也就是g2o中的边，传入的这些数据值会赋值给类的成员变量，在
()运算中计算残差的时候会用到这些成员变量， 也就是观测的数据值。而对于()运算符，传入的是顶点的值，就是g2o中的顶点，或者说是要被优化的值。然后在()运算中，先利用这些顶点值得到计算的观测值，然后和实际的观测值相减，就得到了残差。 -
这个类中再次出现了static函数，这个函数就是返回new出来的一个自动求导对象指针。这样写在类里的好处是让程序更加紧凑，因为自动求导对象里的构造参数是和我们定义的这个类数据紧密相关的。
-
在
()运算中定义残差的时候，使用到了eigen中的map类。实际上map类是为了兼容eigen的矩阵操作与C++的数组操作。
（1）看程序中有一句EIGEN_MAKE_ALIGNED_OPERATOR_NEW声明内存对齐，这在自定义的类中使用Eigen的数据类型时是必要的，还有的时候能看到在std::vector中插入Eigen的变量后面要加一个Eigen::Aligend...，也是为了声明Eigen的内存对齐，这样才能使用STL容器。所以这里要清楚，Eigen为了实现矩阵运算，其内存和原生的C++内存分配出现了差别，很多时候都需要声明一下。
（2）再回到map类，这个类就是为了实现和C++中数组一样的内存分配方式，比如使用指针可以快速访问；但是同时他又可以支持Eigen的矩阵运算，所以在需要对矩阵元素频繁操作或者访问的时候，声明为map类是一个很好的选择。再来看这里为什么要声明为map类？首先频繁操作，要把算出来的残差存到这个矩阵中；其次计算的结果（=右边）都是eigen的数据类型，所以也要和Eigen的数据类型兼容。所以满足使用map的两个情况，因此使用map类很合适。
（3）详细资料参见：Eigen库中的Map类到底是做什么的
(4) 问题：赋值的时候，map.template中template是什么用法？
residuals_map.template head<2>() =
RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
residuals_map(2) = ceres::examples::NormalizeAngle(
(*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
1.2.2.pose_graph_2d.h
这个文件就是主文件，主要就是实现添加残差块。对于每一个观测数据，都需要添加残差块。然后其中还有官网说的那个Normalize限制旋转角度的范围的问题。
其次还有一个比较重要的问题，就是程序中最后把最开始的那个位姿设置成了常量，不优化它，也就是那段长注释解释的，具体我没有看的很懂。
// The pose graph optimization problem has three 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 mitigate 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.
位姿图优化问题具有三个未完全约束的自由度。这通常称为规范自由度。您可以对所有节点应用刚体变换，优化问题仍然具有完全相同的成本。 Levenberg-Marquardt 算法具有内部阻尼来缓解这个问题，但最好适当地约束规范自由度。这可以通过将其中一个姿势设置为常量来完成，这样优化器就无法更改它。
// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
// Constraint2d 2D的约束，是一个自定义的结构体类，描述了顶点之间的位姿约束，也就是g2o中的边
void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints,
std::map<int, Pose2d>* poses,
ceres::Problem* problem) {
CHECK(poses != NULL);
CHECK(problem != NULL);
if (constraints.empty()) {
LOG(INFO) << "No constraints, no problem to optimize.";
return;
}
// LossFunction就是和函数，这里不使用核函数
ceres::LossFunction* loss_function = NULL;
// 局部参数化：
// Lastly, we use a local parameterization to normalize the orientation in the range which is normalized between【-pi,pi)
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
// 使用常量迭代器访问容器，得到容器中的所有元素
for (std::vector<Constraint2d>::const_iterator constraints_iter =
constraints.begin();
constraints_iter != constraints.end();
++constraints_iter) {
const Constraint2d& constraint = *constraints_iter; // 得到当前访问的约束元素
std::map<int, Pose2d>::iterator pose_begin_iter =
poses->find(constraint.id_begin); // 根据约束的顶点Id号得到指向顶点1的迭代器
CHECK(pose_begin_iter != poses->end()) // 找到了
<< "Pose with ID: " << constraint.id_begin << " not found.";
std::map<int, Pose2d>::iterator pose_end_iter = // 根据约束的顶点Id号得到指向顶点2的迭代器
poses->find(constraint.id_end);
CHECK(pose_end_iter != poses->end())
<< "Pose with ID: " << constraint.id_end << " not found.";
const Eigen::Matrix3d sqrt_information =
// llt = LL^T，cholesky分解，然后matrixL是cholesky因子组成的矩阵
constraint.information.llt().matrixL(); // 信息矩阵，即协方差矩阵的逆
// Ceres will take ownership of the pointer.
// PoseGraph2dErrorTerm是专门定义的一个用于2D位姿图优化的类，相当于这个类中已经定义好了残差块的()运算符
// 和之前自己写计算残差的结构体或者类是一样的。costfunction是添加参数块的时候必须使用的
ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create(
constraint.x, constraint.y, constraint.yaw_radians, sqrt_information);
// 对每一个观测值，都添加残差块
problem->AddResidualBlock(cost_function,
loss_function,
&pose_begin_iter->second.x, // 因为这是一个map的迭代器，map中first就是key值，second就是value值
&pose_begin_iter->second.y, // 因为key存的是pose的id号，这里用的是顶点的pose，所以是value的值
&pose_begin_iter->second.yaw_radians,
&pose_end_iter->second.x,
&pose_end_iter->second.y,
&pose_end_iter->second.yaw_radians);
// 设置参数化，这里设置边的两个顶点的pose的旋转角度为 local_parameterization
problem->SetParameterization(&pose_begin_iter->second.yaw_radians,
angle_local_parameterization);
problem->SetParameterization(&pose_end_iter->second.yaw_radians,
angle_local_parameterization);
}
// The pose graph optimization problem has three 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 mitigate 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.
std::map<int, Pose2d>::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(&pose_start_iter->second.x);
problem->SetParameterBlockConstant(&pose_start_iter->second.y);
problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}