feat:给solver添加中文注释

oh_my_loam/solver/cost_function.h

@@ -7,99 +7,234 @@
 
 namespace oh_my_loam {
 
+// ============================================================================
+//                        数据结构定义
+// ============================================================================
+
+/**
+ * @brief 点-直线匹配对数据结构
+ *
+ * 包含一个待优化的点和对应的直线。直线由直线上任意两个点定义。
+ */
 struct PointLinePair {
-  TPoint pt;
+  TPoint pt;  // 待优化的点
+
+  // 内部结构体，用于描述直线
   struct Line {
-    TPoint pt1, pt2;
+    TPoint pt1, pt2;  // 定义直线的两个点
+
+    // 默认构造函数
     Line() = default;
+
+    // 构造函数，根据传入的两个点构造直线
     Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
   };
-  Line line;
+
+  Line line;  // 与点对应的直线
+
+  // 构造函数，根据点和直线结构体构造匹配对
   PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
+
+  // 构造函数，根据点和直线上的两个点构造匹配对
   PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
       : pt(pt), line(pt1, pt2) {}
 };
 
+/**
+ * @brief 点-平面对匹配对数据结构
+ *
+ * 包含一个待优化的点和对应的平面。平面由平面内任意三个点确定。
+ */
 struct PointPlanePair {
-  TPoint pt;
+  TPoint pt;  // 待优化的点
+
+  // 内部结构体，用于描述平面
   struct Plane {
-    TPoint pt1, pt2, pt3;
+    TPoint pt1, pt2, pt3;  // 定义平面的三个点
+
+    // 默认构造函数
     Plane() = default;
+
+    // 构造函数，根据三个点构造平面
     Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
         : pt1(pt1), pt2(pt2), pt3(pt3) {}
   };
-  Plane plane;
+
+  Plane plane;  // 与点对应的平面
+
+  // 构造函数，根据点和平面结构体构造匹配对
   PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
+
+  // 构造函数，根据点和平面上的三个点构造匹配对
   PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
                  const TPoint &pt3)
       : pt(pt), plane(pt1, pt2, pt3) {}
 };
 
+/**
+ * @brief 带系数的点-直线匹配对数据结构
+ *
+ * 包含一个待优化的点和对应的直线系数，系数存储在 6×1 的向量中，
+ * 前 3 个元素表示直线上一个点，后 3 个元素表示直线方向向量。
+ */
 struct PointLineCoeffPair {
-  TPoint pt;
+  TPoint pt;  // 待优化的点
-  Eigen::Matrix<double, 6, 1> line_coeff;
+  Eigen::Matrix<double, 6, 1> line_coeff;  // 直线系数（6维向量）
+
+  // 构造函数，根据点和直线系数构造匹配对
   PointLineCoeffPair(const TPoint &pt,
                      const Eigen::Matrix<double, 6, 1> &line_coeff)
       : pt(pt), line_coeff(line_coeff) {}
 };
 
+/**
+ * @brief 带系数的点-平面对匹配对数据结构
+ *
+ * 包含一个待优化的点和对应的平面系数，系数存储在 4 维向量中，
+ * 通常表示平面方程 ax+by+cz+d=0 的系数。
+ */
 struct PointPlaneCoeffPair {
-  TPoint pt;
+  TPoint pt;  // 待优化的点
-  Eigen::Vector4d plane_coeff;
+  Eigen::Vector4d plane_coeff;  // 平面系数（4维向量）
+
+  // 构造函数，根据点和平面系数构造匹配对
   PointPlaneCoeffPair(const TPoint &pt, const Eigen::Vector4d &plane_coeff)
       : pt(pt), plane_coeff(plane_coeff) {}
 };
 
+// ============================================================================
+//                        成本函数定义
+// ============================================================================
+
+/**
+ * @brief 点-直线代价函数
+ *
+ * 使用自动微分方式计算代价，用于衡量待优化点到直线的距离误差，
+ * 误差由构成直线的两个点确定。
+ */
 class PointLineCostFunction {
  public:
+  // 构造函数，传入匹配对和时间因子
   PointLineCostFunction(const PointLinePair &pair, double time)
-      : pair_(pair), time_(time){};
+      : pair_(pair), time_(time) {};
 
+  /**
+   * @brief 重载函数调用操作符
+   *
+   * 模板化实现，用于 Ceres 自动微分，计算当前参数下的残差。
+   *
+   * @param r_quat 旋转部分参数（四元数，长度为4）
+   * @param t_vec 平移部分参数（向量，长度为3）
+   * @param residual 输出残差数组（3个元素）
+   * @return true 表示计算成功
+   */
   template <typename T>
   bool operator()(const T *const r_quat, const T *const t_vec,
                   T *residual) const;
 
+  /**
+   * @brief 静态创建函数
+   *
+   * 创建一个自动微分类型的 Ceres 成本函数，并传入当前匹配对和时间因子。
+   *
+   * @param pair 点-直线匹配对
+   * @param time 时间因子，用于时间插值（运动补偿）
+   * @return ceres::CostFunction* 创建的成本函数指针
+   */
   static ceres::CostFunction *Create(const PointLinePair &pair, double time) {
     return new ceres::AutoDiffCostFunction<PointLineCostFunction, 3, 4, 3>(
         new PointLineCostFunction(pair, time));
   }
 
  private:
-  PointLinePair pair_;
+  PointLinePair pair_;  // 当前匹配对
-  double time_;
+  double time_;         // 时间因子
 
+  // 禁止拷贝构造函数和赋值操作（宏定义，确保对象不可复制）
   DISALLOW_COPY_AND_ASSIGN(PointLineCostFunction)
 };
 
+/**
+ * @brief 点-平面代价函数
+ *
+ * 使用自动微分方式计算代价，用于衡量待优化点到平面的距离误差，
+ * 平面由平面内的三个点确定。
+ */
 class PointPlaneCostFunction {
  public:
+  // 构造函数，传入匹配对和时间因子
   PointPlaneCostFunction(const PointPlanePair &pair, double time)
-      : pair_(pair), time_(time){};
+      : pair_(pair), time_(time) {};
 
+  /**
+   * @brief 重载函数调用操作符
+   *
+   * 模板化实现，用于计算当前参数下的残差（标量残差）。
+   *
+   * @param r_quat 旋转四元数参数（长度为4）
+   * @param t_vec 平移向量参数（长度为3）
+   * @param residual 输出残差（单个元素）
+   * @return true 表示计算成功
+   */
   template <typename T>
   bool operator()(const T *const r_quat, const T *const t_vec,
                   T *residual) const;
 
+  /**
+   * @brief 静态创建函数
+   *
+   * 创建一个自动微分类型的成本函数，用于点-平面匹配。
+   *
+   * @param pair 点-平面对匹配对
+   * @param time 时间因子
+   * @return ceres::CostFunction* 成本函数指针
+   */
   static ceres::CostFunction *Create(const PointPlanePair &pair, double time) {
     return new ceres::AutoDiffCostFunction<PointPlaneCostFunction, 1, 4, 3>(
         new PointPlaneCostFunction(pair, time));
   }
 
  private:
-  PointPlanePair pair_;
+  PointPlanePair pair_;  // 当前匹配对
-  double time_;
+  double time_;          // 时间因子
+
   DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction)
 };
 
+/**
+ * @brief 带系数的点-直线代价函数
+ *
+ * 用于含有额外直线系数信息的匹配对，计算待优化点到直线的距离误差。
+ */
 class PointLineCoeffCostFunction {
  public:
+  // 构造函数，传入匹配对和时间因子
   PointLineCoeffCostFunction(const PointLineCoeffPair &pair, double time)
-      : pair_(pair), time_(time){};
+      : pair_(pair), time_(time) {};
 
+  /**
+   * @brief 重载函数调用操作符
+   *
+   * 模板化实现，计算当前参数下的残差（3维向量残差）。
+   *
+   * @param r_quat 旋转参数（四元数，长度为4）
+   * @param t_vec 平移参数（向量，长度为3）
+   * @param residual 输出残差（3个元素）
+   * @return true 表示计算成功
+   */
   template <typename T>
   bool operator()(const T *const r_quat, const T *const t_vec,
                   T *residual) const;
 
+  /**
+   * @brief 静态创建函数
+   *
+   * 创建自动微分类型的成本函数，用于带系数的点-直线匹配。
+   *
+   * @param pair 带系数的点-直线匹配对
+   * @param time 时间因子
+   * @return ceres::CostFunction* 成本函数指针
+   */
   static ceres::CostFunction *Create(const PointLineCoeffPair &pair,
                                      double time) {
     return new ceres::AutoDiffCostFunction<PointLineCoeffCostFunction, 1, 4, 3>(
@@ -107,20 +242,46 @@ class PointLineCoeffCostFunction {
   }
 
  private:
-  PointLineCoeffPair pair_;
+  PointLineCoeffPair pair_;  // 当前匹配对（含直线系数）
-  double time_;
+  double time_;              // 时间因子
+
   DISALLOW_COPY_AND_ASSIGN(PointLineCoeffCostFunction)
 };
 
+/**
+ * @brief 带系数的点-平面代价函数
+ *
+ * 用于含有额外平面系数信息的匹配对，计算待优化点到平面的距离误差。
+ */
 class PointPlaneCoeffCostFunction {
  public:
+  // 构造函数，传入匹配对和时间因子
   PointPlaneCoeffCostFunction(const PointPlaneCoeffPair &pair, double time)
-      : pair_(pair), time_(time){};
+      : pair_(pair), time_(time) {};
 
+  /**
+   * @brief 重载函数调用操作符
+   *
+   * 模板化实现，计算当前参数下的残差（标量残差）。
+   *
+   * @param r_quat 旋转参数（四元数，长度为4）
+   * @param t_vec 平移参数（向量，长度为3）
+   * @param residual 输出残差（单个元素）
+   * @return true 表示计算成功
+   */
   template <typename T>
   bool operator()(const T *const r_quat, const T *const t_vec,
                   T *residual) const;
 
+  /**
+   * @brief 静态创建函数
+   *
+   * 创建自动微分类型的成本函数，用于带系数的点-平面匹配。
+   *
+   * @param pair 带系数的点-平面对匹配对
+   * @param time 时间因子
+   * @return ceres::CostFunction* 成本函数指针
+   */
   static ceres::CostFunction *Create(const PointPlaneCoeffPair &pair,
                                      double time) {
     return new ceres::AutoDiffCostFunction<PointPlaneCoeffCostFunction, 1, 4,
@@ -129,78 +290,144 @@ class PointPlaneCoeffCostFunction {
   }
 
  private:
-  PointPlaneCoeffPair pair_;
+  PointPlaneCoeffPair pair_;  // 当前匹配对（含平面系数）
-  double time_;
+  double time_;               // 时间因子
+
   DISALLOW_COPY_AND_ASSIGN(PointPlaneCoeffCostFunction)
 };
 
+// ============================================================================
+//                        成本函数实现（模板函数）
+// ============================================================================
+
+/**
+ * @brief 点-直线代价函数的模板实现
+ *
+ * 对于待优化的点，先通过当前位姿（旋转和平移）对其进行变换，
+ * 然后计算变换后的点到直线（由两个点确定）的距离误差，
+ * 此误差以三维残差的形式返回（利用交叉乘积计算三角形面积后除以直线基底长度）。
+ *
+ * @tparam T 数值类型模板参数（如 float 或 double）
+ * @param r_quat 旋转参数（四元数，长度为4）
+ * @param t_vec 平移参数（向量，长度为3）
+ * @param residual 输出残差数组（3个元素）
+ * @return true 表示计算成功
+ */
 template <typename T>
 bool PointLineCostFunction::operator()(const T *const r_quat,
                                        const T *const t_vec,
                                        T *residual) const {
+  // 提取匹配对中的点和直线上的两个点
   const auto &pt = pair_.pt, &pt1 = pair_.line.pt1, &pt2 = pair_.line.pt2;
+  // 将点转换为 Eigen 向量（3维）
   Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
   Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
   Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
 
+  // 根据输入的旋转参数构造四元数，注意：Ceres中四元数存储顺序可能为 [x,y,z,w]
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
+  // 构造平移向量
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - 1.0e-6) {
+  // 如果时间因子小于 1，则对旋转和平移进行插值（运动补偿）
+  if (time_ <= T(1.0) - T(1.0e-6)) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }
+  // 计算变换后的点位置
   Eigen::Matrix<T, 3, 1> p0 = r * p + t;
 
-  // norm of cross product: triangle area x 2
+  // 利用交叉乘积计算三角形面积（面积 = 0.5 * |(p0-p1) x (p0-p2)|）
-  Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * 0.5;
+  Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * T(0.5);
+  // 计算直线基底的长度（两点之间的距离）
   T base_length = (p2 - p1).norm();
+  // 将面积除以基底长度，即可得到点到直线的距离（与三角形高等价）
+  // 注意：这里残差为3维，分别保存了每个坐标轴上的误差（也可看作向量形式）
   residual[0] = area[0] / base_length;
   residual[1] = area[1] / base_length;
   residual[2] = area[2] / base_length;
   return true;
 }
 
+/**
+ * @brief 点-平面代价函数的模板实现
+ *
+ * 对于待优化的点，通过当前位姿对其进行变换，
+ * 然后计算变换后的点到平面（由三个点确定）的距离，
+ * 平面法向量由三个点计算得到，残差为点到平面的距离（标量）。
+ *
+ * @tparam T 数值类型模板参数
+ * @param r_quat 旋转参数（长度为4的四元数）
+ * @param t_vec 平移参数（3维向量）
+ * @param residual 输出残差（单个元素）
+ * @return true 表示计算成功
+ */
 template <typename T>
 bool PointPlaneCostFunction::operator()(const T *const r_quat,
                                         const T *const t_vec,
                                         T *residual) const {
+  // 提取匹配对中的点和平面上三个点
   const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2,
              &pt3 = pair_.plane.pt3;
+  // 将各点转换为 Eigen 向量
   Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
   Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
   Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
   Eigen::Matrix<T, 3, 1> p3(T(pt3.x), T(pt3.y), T(pt3.z));
 
+  // 构造旋转四元数和平移向量
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - 1.0e-6) {
+  // 进行运动补偿插值
+  if (time_ <= T(1.0) - T(1.0e-6)) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }
+  // 计算变换后的点位置
   Eigen::Matrix<T, 3, 1> p0 = r * p + t;
 
+  // 计算平面法向量：由平面内两个向量叉乘后归一化得到
   Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized();
+  // 计算点到平面的距离（点与平面上一点连线与法向量的内积）
   residual[0] = (p0 - p1).dot(normal);
   return true;
 }
 
+/**
+ * @brief 带系数的点-直线代价函数的模板实现
+ *
+ * 使用额外的直线系数信息，将系数向量分为两部分：前 3 个元素为直线上某一点，
+ * 后 3 个元素为直线方向。对待优化的点进行变换后，计算其到直线的距离误差，
+ * 该误差以3维残差输出（利用叉积计算）。
+ *
+ * @tparam T 数值类型模板参数
+ * @param r_quat 旋转参数（四元数，长度为4）
+ * @param t_vec 平移参数（向量，长度为3）
+ * @param residual 输出残差（3个元素）
+ * @return true 表示计算成功
+ */
 template <typename T>
 bool PointLineCoeffCostFunction::operator()(const T *const r_quat,
                                             const T *const t_vec,
                                             T *residual) const {
+  // 将待优化点转换为 Eigen 向量
   Eigen::Matrix<T, 3, 1> p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z));
+  // 将直线系数转换为模板类型，并分解为直线上的一点 p1 和方向向量 dir
   Eigen::Matrix<T, 6, 1> coeff = pair_.line_coeff.template cast<T>();
   Eigen::Matrix<T, 3, 1> p1 = coeff.topRows(3);
   Eigen::Matrix<T, 3, 1> dir = coeff.bottomRows(3);
 
+  // 构造旋转和平移参数
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - 1.0e-6) {
+  // 如果时间因子小于1，则进行插值
+  if (time_ <= T(1.0) - T(1.0e-6)) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }
+  // 计算变换后的点位置
   Eigen::Matrix<T, 3, 1> p0 = r * p + t;
 
+  // 计算待优化点到直线的距离误差：计算 (p0-p1) 与直线方向的叉积
   Eigen::Matrix<T, 3, 1> cross = (p0 - p1).cross(dir);
   residual[0] = cross[0];
   residual[1] = cross[1];
@@ -208,21 +435,39 @@ bool PointLineCoeffCostFunction::operator()(const T *const r_quat,
   return true;
 }
 
+/**
+ * @brief 带系数的点-平面代价函数的模板实现
+ *
+ * 利用传入的平面系数（4维向量，表示平面方程 ax+by+cz+d=0），
+ * 对待优化的点进行变换后计算其到平面的距离残差，残差为标量形式。
+ *
+ * @tparam T 数值类型模板参数
+ * @param r_quat 旋转参数（四元数，长度为4）
+ * @param t_vec 平移参数（向量，长度为3）
+ * @param residual 输出残差（单个标量）
+ * @return true 表示计算成功
+ */
 template <typename T>
 bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
                                              const T *const t_vec,
                                              T *residual) const {
+  // 将待优化点转换为 Eigen 向量
   Eigen::Matrix<T, 3, 1> p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z));
+  // 将平面系数转换为模板类型的 4 维向量
   Eigen::Matrix<T, 4, 1> coeff = pair_.plane_coeff.template cast<T>();
 
+  // 构造旋转四元数和平移向量
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - 1.0e-6) {
+  // 对时间因子小于1的情况进行插值
+  if (time_ <= T(1.0) - T(1.0e-6)) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }
+  // 计算变换后的点位置
   Eigen::Matrix<T, 3, 1> p0 = r * p + t;
 
+  // 计算点到平面的距离残差：将点 p0 代入平面方程，得到 ax+by+cz+d
   residual[0] = coeff.topRows(3).transpose() * p0;
   residual[0] += coeff(3);
   return true;

oh_my_loam/solver/solver.cc

@@ -8,58 +8,138 @@
 
 namespace oh_my_loam {
 
+// 定义一个局部变量，表示 Huber 损失的尺度参数
 namespace {
 double kHuberLossScale = 0.1;
 }
 
+/**
+ * @brief PoseSolver 构造函数
+ *
+ * 初始化位姿求解器，传入初始位姿（包括旋转和位移），并将其复制到内部参数数组中。
+ * 同时为 Ceres 问题添加参数块，其中旋转部分使用 Eigen 四元数参数化，
+ * 并设置 Huber 损失函数用于鲁棒优化。
+ *
+ * @param pose 初始位姿（类型为 common::Pose3d），包含旋转（四元数）和位移向量
+ */
 PoseSolver::PoseSolver(const common::Pose3d &pose) {
+  // 将输入位姿的旋转部分（四元数的系数）复制到内部数组 r_quat_ 中，四元数有 4 个元素
   std::copy_n(pose.r_quat().coeffs().data(), 4, r_quat_);
+  // 将输入位姿的平移向量复制到内部数组 t_vec_ 中，平移向量有 3 个元素
   std::copy_n(pose.t_vec().data(), 3, t_vec_);
+  // 创建一个 Ceres 的 Huber 损失函数对象，参数 kHuberLossScale 控制损失函数的尺度
   loss_function_ = new ceres::HuberLoss(kHuberLossScale);
-  problem_.AddParameterBlock(r_quat_, 4,
+  // 向问题中添加旋转参数块，使用 Eigen 四元数参数化以确保单位四元数的约束
-                             new ceres::EigenQuaternionParameterization());
+  problem_.AddParameterBlock(r_quat_, 4, new ceres::EigenQuaternionParameterization());
+  // 向问题中添加平移参数块（3个自由度）
   problem_.AddParameterBlock(t_vec_, 3);
 }
 
+/**
+ * @brief Solve 求解器入口函数
+ *
+ * 利用 Ceres 求解器求解当前的位姿优化问题。设定线性求解器类型、最大迭代次数等参数，
+ * 求解后更新内部的旋转和平移参数，同时将最新位姿保存到 pose 输出参数中。
+ *
+ * @param max_iter_num 最大迭代次数
+ * @param verbose 是否输出详细日志信息
+ * @param pose 求解后得到的最新位姿（输出参数）
+ * @return bool 如果优化收敛则返回 true，否则返回 false
+ */
 bool PoseSolver::Solve(int max_iter_num, bool verbose,
                        common::Pose3d *const pose) {
+  // 配置 Ceres 求解选项
   ceres::Solver::Options options;
-  options.linear_solver_type = ceres::DENSE_QR;
+  options.linear_solver_type = ceres::DENSE_QR;  // 使用密集 QR 分解作为线性求解器
-  options.max_num_iterations = max_iter_num;
+  options.max_num_iterations = max_iter_num;       // 最大迭代次数
-  options.minimizer_progress_to_stdout = false;
+  options.minimizer_progress_to_stdout = false;    // 是否将求解进度输出到标准输出
-  ceres::Solver::Summary summary;
+  ceres::Solver::Summary summary;                  // 用于存储求解过程和结果的摘要信息
+  // 调用 Ceres 求解器求解问题
   ceres::Solve(options, &problem_, &summary);
+  // 如果 verbose 为 true，则输出求解摘要报告
   AINFO_IF(verbose) << summary.BriefReport();
+  // 如果 pose 非空，则将当前求解得到的旋转和平移参数构造为 common::Pose3d 对象输出
   if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
+  // 返回求解是否收敛的状态（Ceres 的 termination_type 为 CONVERGENCE 表示收敛）
   return summary.termination_type == ceres::CONVERGENCE;
 }
 
+/**
+ * @brief 添加点-直线匹配项
+ *
+ * 根据输入的点-直线匹配对 pair 以及时间参数 time，
+ * 构造一个对应的代价函数（CostFunction），并将其加入到 Ceres 优化问题中。
+ *
+ * @param pair 点-直线匹配对，包含原始点和匹配直线上的两个点
+ * @param time 时间参数，用于对匹配代价进行时间权重调整
+ */
 void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
+  // 通过静态工厂函数创建点-直线匹配的代价函数
   ceres::CostFunction *cost_function =
       PointLineCostFunction::Create(pair, time);
+  // 将代价函数添加到问题中，设置损失函数（Huber 损失）和待优化参数（旋转和平移）
   problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
 }
 
+/**
+ * @brief 添加点-平面对匹配项
+ *
+ * 根据输入的点-平面对匹配 pair 以及时间参数 time，
+ * 构造一个对应的代价函数（CostFunction），并将其加入到 Ceres 优化问题中。
+ *
+ * @param pair 点-平面对匹配对，包含原始点和匹配平面上的三个点
+ * @param time 时间参数，用于对匹配代价进行时间权重调整
+ */
 void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
+  // 创建点-平面对代价函数
   ceres::CostFunction *cost_function =
       PointPlaneCostFunction::Create(pair, time);
+  // 添加残差块到优化问题中，待优化参数为旋转和平移
   problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
 }
 
+/**
+ * @brief 添加带有系数信息的点-直线匹配项
+ *
+ * 针对带有额外系数信息的点-直线匹配对，构造对应的代价函数，并加入优化问题中。
+ *
+ * @param pair 点-直线匹配对（含系数信息）
+ * @param time 时间参数
+ */
 void PoseSolver::AddPointLineCoeffPair(const PointLineCoeffPair &pair,
                                        double time) {
+  // 通过工厂函数创建带系数的点-直线代价函数
   ceres::CostFunction *cost_function =
       PointLineCoeffCostFunction::Create(pair, time);
+  // 将代价函数加入到问题中
   problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
 }
 
+/**
+ * @brief 添加带有系数信息的点-平面对匹配项
+ *
+ * 针对带有额外系数信息的点-平面对匹配对，构造对应的代价函数，并加入优化问题中。
+ *
+ * @param pair 点-平面对匹配对（含系数信息）
+ * @param time 时间参数
+ */
 void PoseSolver::AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair,
                                         double time) {
+  // 创建带系数的点-平面代价函数
   ceres::CostFunction *cost_function =
       PointPlaneCoeffCostFunction::Create(pair, time);
+  // 将代价函数添加到 Ceres 问题中，优化参数为旋转和平移
   problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
 }
 
+/**
+ * @brief 获取当前求解的位姿
+ *
+ * 根据内部存储的旋转和平移参数构造一个 common::Pose3d 对象，
+ * 用于返回当前的位姿估计结果。
+ *
+ * @return common::Pose3d 当前位姿
+ */
 common::Pose3d PoseSolver::GetPose() const {
   return common::Pose3d(r_quat_, t_vec_);
 }