0. 简介
我们知道，在SLAM中，最主要也最关键的就是如何对我们设计出来的非线性问题进行求解，并计算出最优解。而在这个过程中我们用的最多的就是Ceres，之前作者在《SLAM本质剖析-Ceres》。而我们这一篇文章就来讨论一下如何使用Ceres来完成常见的优化。
1. Ceres中点到点的距离残差
下面的部分就是我们最常用的两个点到点的计算公式，其中Eigen_MAKE_ALIGNED_OPERATOR_NEW是一个Eigen库中的宏，用于启用对齐操作，以提高程序性能。
PointToPointError函数用于构造函数，它初始化了m_point_lidar（点云中的点），m_point_cam（相机中的点）和m_w（权重）变量。接下来是重载运算符，它接收一个旋转和平移向量（R_t）作为参数，并计算出旋转矩阵（R）。它通过旋转矩阵和平移向量来计算投影点，并通过计算相机中的点和点云中的点的投影点的距离来计算残差，最后将残差乘以权重。
Create函数用于创建costfunction。它接收点云中的点，相机中的点和权重作为参数，并创建一个自动微分代价函数，返回一个costfunction。最后是m_point_lidar，m_point_cam和m_w变量的定义，它们代表了点云中的点，相机中的点和权重。
// 点到点的距离损失
struct PointToPointError
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PointToPointError(const Eigen::Vector3d& point_lidar,
const Eigen::Vector3d& point_cam,
const double& w):
m_point_lidar(point_lidar),
m_point_cam(point_cam),
m_w(w){}//m_point_lidar代表了点云中的点，m_point_cam代表了相机中的点，m_w代表了权重
template<typename T>
bool operator()(const T* const R_t,T* residual) const{
T rot[3*3];
ceres::AngleAxisToRotationMatrix(R_t, rot);//将旋转向量转换为旋转矩阵
Eigen::Matrix<T, 3, 3> R = Eigen::Matrix<T, 3, 3>::Identity();//初始化旋转矩阵
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
R(i, j) = rot[i+j*3];//将旋转矩阵赋值给R
}
}
Eigen::Map<const Eigen::Matrix<T,3,1>> trans(R_t+3);//将平移向量赋值给trans
residual[0] = (m_point_cam.template cast<T>() - (R * m_point_lidar.template cast<T>() + trans)).norm();//通过获取相机中的点和点云中的点的投影点，计算残差
residual[0] *= m_w;//乘以权重
return true;
}
static ceres::CostFunction* Create(const Eigen::Vector3d& point_lidar,
const Eigen::Vector3d& point_cam,
const double& w){
return (new ceres::AutoDiffCostFunction<PointToPointError, 1, 6>(
new PointToPointError(point_lidar, point_cam, w)));//创建costfunction
}
const Eigen::Vector3d m_point_lidar;
const Eigen::Vector3d m_point_cam;
const double m_w;
};
// 点到点的距离损失，方法2，主要是通过
struct LidarDistanceFactor {
LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_)
: curr_point(curr_point_), closed_point(closed_point_) {}// curr_point代表当前点，closed_point代表前一帧平面上的一个点
template <typename T>
bool operator()(const T* q, const T* t, T* residual) const {
Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};// 当前帧到世界坐标系的旋转
Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};// 当前帧到世界坐标系的平移
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
Eigen::Matrix<T, 3, 1> point_w;// 当前点在世界坐标系下的位置
point_w = q_w_curr * cp + t_w_curr;// 当前点在世界坐标系下的位置
residual[0] = point_w.x() - T(closed_point.x());// 误差为当前点在世界坐标系下的位置的x坐标减去前一帧平面上的一个点的x坐标
residual[1] = point_w.y() - T(closed_point.y());
residual[2] = point_w.z() - T(closed_point.z());
return true;
}
static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d closed_point_) {
return (new ceres::AutoDiffCostFunction<LidarDistanceFactor, 3, 4, 3>(
new LidarDistanceFactor(curr_point_, closed_point_)));
}
Eigen::Vector3d curr_point;
Eigen::Vector3d closed_point;
};
// ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
// problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
2. Ceres中点到线的距离残差
struct LidarEdgeFactor是一个Ceres的代价函数类，用于计算激光雷达数据的优化问题中的边界残差。它计算了当前点到前一帧的边的投影点的距离作为误差残差。该类包含了一个构造函数，一个重载的运算符”()”，以及一个静态的Create函数。
构造函数定义了四个变量：curr_point表示当前点；last_point_a代表前一帧的边上的第一个点；last_point_b代表前一帧的边上的第二个点；s代表当前点在前一帧边上的投影的位置。重载的运算符”()”定义了代价函数的计算过程，其中：
- q和t分别代表优化变量中的旋转和平移量
- residual数组存储了残差
该函数通过插值的方式计算前一帧到当前帧的旋转和平移，并通过矩阵运算将当前点变换到前一帧空间，最后计算当前点到前一帧边的投影点的距离。静态的Create函数用于创建LidarEdgeFactor类的实例，并返回一个指向ceres::AutoDiffCostFunction的指针，它接受curr_point、last_point_a、last_point_b和s作为参数。
struct LidarEdgeFactor {
//计算3D点到直线的距离
LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
Eigen::Vector3d last_point_b_, double s_)
: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {// curr_point代表当前点，last_point_a代表前一帧的边上的第一个点，last_point_b代表前一帧的边上的第二个点，s代表当前点在前一帧边上的投影的位置
}
template <typename T>
bool operator()(const T* q, const T* t, T* residual) const {
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};// 前一帧边上的第一个点
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};// 前一帧边上的第二个点
// Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};// 前一帧到当前帧的旋转
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};// 单位四元数
q_last_curr = q_identity.slerp(T(s), q_last_curr);// slerp插值
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};// 前一帧到当前帧的平移
Eigen::Matrix<T, 3, 1> lp;// 前一帧边上的投影点
lp = q_last_curr * cp + t_last_curr;// 前一帧边上的投影点
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);// 当前点到前一帧边上的投影点的向量与前一帧边上的向量的叉乘
Eigen::Matrix<T, 3, 1> de = lpa - lpb;// 前一帧边上的向量
residual[0] = nu.x() / de.norm();// 误差为当前点到前一帧边上的投影点的向量与前一帧边上的向量的叉乘与前一帧边上的向量的模的比值
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();
return true;
}
static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_, const double s_) {
return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>( //残差维数，第一个变量维数，第二个变量维数
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
// ceres::CostFunction* cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
// problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
3. Ceres中点到面的残差
下面两个为点到面的方程，这部分主要的是第二种方法用的比较多，我们下面来主要梳理一下第二种方法。这里主要命名了一个名为LidarPlaneNormFactor函数，。它的作用是在BA（Bundle Adjustment）中对激光雷达平面的法向量进行误差优化。
该类构造函数接受三个参数：curr_point_代表当前点，plane_unit_norm_代表前一帧平面的法向量，negative_OA_dot_norm_代表前一帧平面的法向量与前一帧平面上的一个点的点乘。
在该类中有一个模板函数operator()，它定义了误差函数：对于当前帧的位姿（q为旋转四元数，t为平移向量），误差为前一帧平面的法向量与当前点在世界坐标系下的位置的点乘。
最后，该类还提供了一个静态函数Create，用于创建残差块。
//计算激光点到平面的残差，以三个点为平面
struct LidarPlaneFactor {
LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
last_point_m(last_point_m_), s(s_) {// curr_point代表当前点，last_point_j代表前一帧的平面上的第一个点，last_point_l代表前一帧的平面上的第二个点，last_point_m代表前一帧的平面上的第三个点，s代表当前点在前一帧平面上的投影的位置
ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);// 前一帧平面上的向量
ljm_norm.normalize();// 前一帧平面上的向量的模
}
template <typename T>
bool operator()(const T* q, const T* t, T* residual) const {
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};// 前一帧平面上的第一个点
// Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()),
// T(last_point_l.z())};
// Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()),
// T(last_point_m.z())};
Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};// 前一帧平面上的向量
// Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};// 前一帧到当前帧的旋转
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};// 单位四元数
q_last_curr = q_identity.slerp(T(s), q_last_curr);// 前一帧到当前帧的旋转
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};// 前一帧到当前帧的平移
Eigen::Matrix<T, 3, 1> lp;// 前一帧边上的投影点
lp = q_last_curr * cp + t_last_curr;// 当前点在前一帧的位置
residual[0] = (lp - lpj).dot(ljm);// 误差为当前点到前一帧平面上的投影点的向量与前一帧平面上的向量的点乘
return true;
}
static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_,
const Eigen::Vector3d last_point_m_, const double s_) {
return (new ceres::AutoDiffCostFunction<LidarPlaneFactor, 1, 4, 3>(
new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
}
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
};
// 计算激光点到平面的残差，以平面的法向量为参数
struct LidarPlaneNormFactor {
LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,
double negative_OA_dot_norm_)
: curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),
negative_OA_dot_norm(negative_OA_dot_norm_) {}// curr_point代表当前点，plane_unit_norm代表前一帧平面的法向量，negative_OA_dot_norm代表前一帧平面的法向量与前一帧平面上的一个点的点乘
template <typename T>
bool operator()(const T* q, const T* t, T* residual) const {
Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};// 当前帧到世界坐标系的旋转
Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};// 当前帧到世界坐标系的平移
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
Eigen::Matrix<T, 3, 1> point_w;
point_w = q_w_curr * cp + t_w_curr;// 当前点在世界坐标系下的位置
Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()),
T(plane_unit_norm.z()));// 前一帧平面的法向量
residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);// 误差为前一帧平面的法向量与当前点在世界坐标系下的位置的点乘
return true;
}
static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d plane_unit_norm_,
const double negative_OA_dot_norm_) {
return (new ceres::AutoDiffCostFunction<LidarPlaneNormFactor, 1, 4, 3>(
new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));// 创建残差块
}
Eigen::Vector3d curr_point;
Eigen::Vector3d plane_unit_norm;
double negative_OA_dot_norm;
};
// ceres::CostFunction* cost_function =
// LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);//点到平面的距离： 点与平面单位法向量的点乘
// problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
4. Ceres中线特征误差
这里定义线特征的误差为一个结构体LineFeatureProjFactor。构造函数LineFeatureProjFactor用于初始化sp_，ep_，norm_vec_，direct_vec_，K_。在构造函数内部，通过计算获得KK_的值。
代码的最终目的是使用Ceres-Solver对每一个线特征点的投影残差进行优化，并计算误差并返回。具体做法为：
- 计算当前帧到世界坐标系的旋转矩阵Rwc。
- 计算当前帧到世界坐标系的平移向量twc。
- 计算世界坐标系到当前帧的旋转矩阵Rcw。
- 计算世界坐标系到当前帧的平移向量的反对称矩阵twc_x。
- 计算当前帧到世界坐标系的法向量norm_vec_c。
- 计算当前帧到世界坐标系的方向向量direct_vec_c。
- 计算相机内参的逆乘以当前帧到世界坐标系的法向量im_line。
- 计算相机内参的逆乘
struct LineFeatureProjFactor {//计算线特征点
Eigen::Vector3d sp_, ep_;//线特征点的起点和终点
Eigen::Vector3d norm_vec_, direct_vec_;//线特征点的法向量和方向向量
Eigen::Matrix3d K_;//相机内参
Eigen::Matrix3d KK_;//相机内参的逆
LineFeatureProjFactor(const Eigen::Vector3d sp, const Eigen::Vector3d ep,
const Eigen::Vector3d norm_vec, const Eigen::Vector3d direct_vec, const Eigen::Matrix3d K)
: sp_(sp), ep_(ep), norm_vec_(norm_vec), direct_vec_(direct_vec), K_(K) {//sp_为线特征点的起点，ep_为线特征点的终点，norm_vec_为线特征点的法向量，direct_vec_为线特征点的方向向量，K_为相机内参
const double fx = K_(0, 0), cx = K_(0, 2);//相机内参的fx和cx
const double fy = K_(1, 1), cy = K_(1, 2);//相机内参的fy和cy
KK_ << fy, 0, 0, 0, fx, 0, -fy*cx, -fx*cy, fx*fy;//相机内参的逆
}
template <typename T>
bool operator()(const T* const rotate, const T* const trans, T* residuals) const {
Eigen::Matrix<T, 3, 3> Rwc = Eigen::Quaternion<T>(rotate[3], rotate[0], rotate[1], rotate[2]).toRotationMatrix();//当前帧到世界坐标系的旋转矩阵
Eigen::Matrix<T, 3, 1> twc{trans[0], trans[1], trans[2]};//当前帧到世界坐标系的平移向量
Eigen::Matrix<T, 3, 3> Rcw = Rwc.transpose();//世界坐标系到当前帧的旋转矩阵
Eigen::Matrix<T, 3, 3> twc_x = MATH_UTILS::skewMatrix<T>(twc);//世界坐标系到当前帧的平移向量的反对称矩阵
Eigen::Matrix<T, 3, 1> norm_vec_c = Rcw * norm_vec_.cast<T>() + Rcw * twc_x * direct_vec_.cast<T>();//当前帧到世界坐标系的法向量
Eigen::Matrix<T, 3, 1> direct_vec_c = Rcw * direct_vec_.cast<T>();//当前帧到世界坐标系的方向向量
Eigen::Matrix<T, 3, 1> im_line = KK_ * norm_vec_c;//相机内参的逆乘以当前帧到世界坐标系的法向量
// std::cout << "line: " << im_line.transpose() << std::endl;
T im_line_norm = ceres::sqrt(im_line(0)*im_line(0) + im_line(1)*im_line(1));//求得相机内参的逆乘以当前帧到世界坐标系的法向量的模
residuals[0] = (im_line.transpose() * sp_.cast<T>())(0, 0) / im_line_norm;//求得相机内参的逆乘以当前帧到世界坐标系的法向量的模乘以线特征点的起点在当前帧的投影点
residuals[1] = (im_line.transpose() * ep_.cast<T>())(0, 0) / im_line_norm;
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const Eigen::Vector3d sp, const Eigen::Vector3d ep,
const Eigen::Vector3d norm_vec, const Eigen::Vector3d direct_vec, const Eigen::Matrix3d K) {
return (new ceres::AutoDiffCostFunction<LineFeatureProjFactor, 2, 4, 3>(
new LineFeatureProjFactor(sp, ep, norm_vec, direct_vec, K)));
}
};
5. Ceres中相机闭环误差
这部分是一个Ceres的代码片段，它定义了一个名为ClosedupError的结构体，用于处理闭环误差。这个闭环误差是在两帧之间的变换关系，它代表了当前帧和前一帧之间的变换。在ClosedupError中，它有一个重载的运算符，该运算符的作用是计算前一帧和当前帧之间的误差，并通过残差数组返回。具体而言，该代码首先将当前帧和前一帧的旋转向量转换为旋转矩阵，然后使用这两个旋转矩阵和对应的平移向量，构造出两帧到世界坐标系的变换矩阵。接下来，使用当前帧到前一帧的变换矩阵，计算出前一帧到当前帧的旋转矩阵，并通过这三个变换矩阵求得误差。
它通过给定两个相机位姿（Rt1，Rt2）计算闭环误差的两个组成部分：
- 两个相机位姿的间的平移误差，即两个相机的位置间的距离。
- 两个相机位姿的间的旋转误差，即两个相机的姿态差。
该算法的具体实现是：
- 将位姿转换为变换矩阵（通过角轴矩阵）
- 计算相机1到相机2的位姿差，以及相机1到相机2再到相机3的位姿差。
- 计算相机1到相机3的闭环位姿差。
- 计算闭环误差的两个组成部分，并将它们存储在residual数组中。
struct ClosedupError// 闭环误差
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 为了保证Eigen的内存对齐
ClosedupError(const Eigen::VectorXd &Rt_c1_c2,
const double &w) : m_Rt_c1_c2(Rt_c1_c2), m_w(w) {}// Rt_c1_c2代表当前帧到前一帧的变换，w代表权重
template<typename T>
bool operator()(const T* const Rt1, const T* const Rt2, T* residual) const{
T rot1[3*3];// 当前帧到世界坐标系的旋转
ceres::AngleAxisToRotationMatrix(Rt1, rot1);// 旋转矩阵转换为旋转向量
Eigen::Map<const Eigen::Matrix<T,3,3>> R1(rot1);// 旋转向量转换为旋转矩阵
Eigen::Map<const Eigen::Matrix<T,3,1>> t1(Rt1+3);// 当前帧到世界坐标系的平移
Eigen::Matrix<T,4,4> trans1 = Eigen::Matrix<T,4,4>::Identity();// 当前帧到世界坐标系的变换矩阵
trans1.block(0,0,3,3) = R1;// 当前帧到世界坐标系的旋转
trans1.block(0,3,3,1) = t1;// 当前帧到世界坐标系的平移
T rot2[3*3];
ceres::AngleAxisToRotationMatrix(Rt2, rot2);// 前一帧到世界坐标系的旋转
Eigen::Map<const Eigen::Matrix<T,3,3>> R2(rot2);// 旋转向量转换为旋转矩阵
Eigen::Map<const Eigen::Matrix<T,3,1>> t2(Rt2+3);// 前一帧到世界坐标系的平移
Eigen::Matrix<T,4,4> trans2 = Eigen::Matrix<T,4,4>::Identity();// 前一帧到世界坐标系的变换矩阵
trans2.block(0,0,3,3) = R2;// 前一帧到世界坐标系的旋转
trans2.block(0,3,3,1) = t2;// 前一帧到世界坐标系的平移
// T rot3[3*3];
// const T* const Rt3 = m_Rt_c1_c2.template cast<T>().data();
// ceres::AngleAxisToRotationMatrix(Rt3, rot3);
// Eigen::Map<const Eigen::Matrix<T,3,3>> R3(rot3);
// Eigen::Map<const Eigen::Matrix<T,3,1>> t3(Rt3+3);
Eigen::Matrix<T,4,4> trans3 = Eigen::Matrix<T,4,4>::Identity();//单位矩阵
Eigen::Matrix<T,3,3> R3 = Sophus::SO3d::exp(m_Rt_c1_c2.head(3)).matrix().template cast<T>();// 前一帧到当前帧的旋转
trans3.block(0,0,3,3) = R3;// 前一帧到当前帧的旋转
trans3.block(0,3,3,1) = m_Rt_c1_c2.tail(3).template cast<T>();// 前一帧到当前帧的平移
Eigen::Matrix<T,4,4> loop = trans1*(trans2.inverse().template cast<T>())*trans3;//通过将当前帧到世界坐标系的变换矩阵乘以前一帧到世界坐标系的逆变换矩阵乘以前一帧到当前帧的变换矩阵，得到当前帧到当前帧的变换矩阵
residual[0] = loop.block(0,3,3,1).norm();//求得当前帧到当前帧的平移
residual[0] *= m_w;//乘以权重
residual[1] = (Matrix<T,3,3>::Identity()-loop.block(0,0,3,3)).trace();//求得单位矩阵减去当前帧到当前帧的旋转矩阵的迹
residual[1] *= m_w;//乘以权重
return true;
}
static ceres::CostFunction* Create(const Eigen::VectorXd &Rt_c1_c2,
const double &w) {
return (new ceres::AutoDiffCostFunction<ClosedupError, 2, 6, 6>(
new ClosedupError(Rt_c1_c2, w)));
}
const double m_w;
const Eigen::VectorXd& m_Rt_c1_c2;
};
评论（0）
您还未登录，请登录后发表或查看评论