本文主要是介绍A-loam源码注释-头文件lidarFactor.hpp，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
本篇博客是A-loam学习的笔记，用于SLAM初学者一起学习。
lidarFactor.hpp
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <eigen3/Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
//pcl_conversions是ROS中的一个包，它提供了将PCL数据类型（如pcl::PointCloud<pcl::PointXYZ>）转换为ROS消息类型（如sensor_msgs/PointCloud2）的功能。这对于在ROS节点之间传递点云数据非常有用。
struct LidarEdgeFactor //用于发现边缘点 EdgePoint
{
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(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(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())};
//cp: 表示当前点的三维坐标。
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
//lpa: 表示上一个点A的三维坐标。
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
//lpb: 表示上一个点B的三维坐标。
//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_last_curr{q[3], q[0], q[1], q[2]};：这行代码创建了一个四元数q_last_curr，它表示从上一个点到当前点的旋转。四元数的参数是从数组q中提取的，其中q[0], q[1], q[2]是四元数的向量部分（x, y, z），而q[3]是标量部分（w）。
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
//Eigen::Quaternion<T>{(T)(T)((int)1), (T)(T)((int)0), (T)(T)((int)0), (T)(T)((int)0)}，定义了一个单位四元数。
q_last_curr = q_identity.slerp(T(s), q_last_curr);
//这行代码使用球面线性插值（Slerp）来计算从单位四元数到q_last_curr的插值。参数T(s)是插值的权重，它由成员变量s决定，这个权重通常在0到1之间，用于平滑地从一个旋转过渡到另一个旋转。
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
//这行代码创建了一个三维向量t_last_curr，它表示从上一个点到当前点的平移。这个向量的每个分量是t数组中对应分量的加权版本，权重是T(s)。
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);//(P-A)×（P-B)
Eigen::Matrix<T, 3, 1> de = lpa - lpb;//||AB||
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_)
//这是一个静态方法，它返回一个指向ceres::CostFunction的指针。
{
return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(
//LidarEdgeFactor：这是代价函数的类型。3：表示残差向量的维度。4：表示第一个参数块的维度。3：表示第二个参数块的维度。
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
//这里创建了一个LidarEdgeFactor对象，它将被用作代价函数。
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
//将成员变量定义放在结构体或类的末尾是一种常见的编码风格
};
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_)
{
ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
ljm_norm.normalize();
//调用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); //相当于||OA·n||/||n||
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>(
//LidarEdgeFactor：这是代价函数的类型。1：表示残差向量的维度。为什么这里是1？因为点到直线距离采用叉乘除以常量，结果是一个向量；而点到面距离采用点乘方向量除以法向量的模长，结果是一个数。
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_) {}
//
Eigen::Vector3d curr_point: 当前点的三维坐标。Eigen::Vector3d plane_unit_norm: 平面的单位法向量。double 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]};
//从指针 q 中读取四元数值，并创建一个四元数对象 q_w_curr，表示从世界坐标系到当前坐标系的旋转。
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;
//计算点 cp 在世界坐标系中的位置。这通过将四元数 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);
//通过计算向量 norm 和 point_w 的点积，然后加上 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;
};
struct LidarDistanceFactor
{
LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_)
: curr_point(curr_point_), closed_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());
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;
};
//在雷达坐标系下计算残差和世界坐标系下计算残差的公式略有区别，在后续代码中再来分析原因。
这篇关于A-loam源码注释-头文件lidarFactor.hpp的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
