From 3c4963d28920c5309a69b055300dd4876ed267e4 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Sun, 2 Feb 2025 01:54:21 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E7=BB=99solver=E6=B7=BB=E5=8A=A0=E4=B8=AD?= =?UTF-8?q?=E6=96=87=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- oh_my_loam/solver/cost_function.h | 303 +++++++++++++++++++++++++++--- oh_my_loam/solver/solver.cc | 94 ++++++++- 2 files changed, 361 insertions(+), 36 deletions(-) diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index e21b815..c4bab8c 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/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; - Eigen::Matrix line_coeff; + TPoint pt; // 待优化的点 + Eigen::Matrix line_coeff; // 直线系数（6维向量） + + // 构造函数，根据点和直线系数构造匹配对 PointLineCoeffPair(const TPoint &pt, const Eigen::Matrix &line_coeff) : pt(pt), line_coeff(line_coeff) {} }; +/** + * @brief 带系数的点-平面对匹配对数据结构 + * + * 包含一个待优化的点和对应的平面系数，系数存储在 4 维向量中， + * 通常表示平面方程 ax+by+cz+d=0 的系数。 + */ struct PointPlaneCoeffPair { - TPoint pt; - Eigen::Vector4d plane_coeff; + TPoint pt; // 待优化的点 + 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 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( new PointLineCostFunction(pair, time)); } private: - PointLinePair pair_; - double time_; + PointLinePair pair_; // 当前匹配对 + 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 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( new PointPlaneCostFunction(pair, time)); } private: - PointPlanePair pair_; - double time_; + PointPlanePair pair_; // 当前匹配对 + 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 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( @@ -107,20 +242,46 @@ class PointLineCoeffCostFunction { } private: - PointLineCoeffPair pair_; - double time_; + PointLineCoeffPair pair_; // 当前匹配对（含直线系数） + 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 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 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 p(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix p1(T(pt1.x), T(pt1.y), T(pt1.z)); Eigen::Matrix p2(T(pt2.x), T(pt2.y), T(pt2.z)); + // 根据输入的旋转参数构造四元数，注意：Ceres中四元数存储顺序可能为 [x,y,z,w] Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); + // 构造平移向量 Eigen::Matrix 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::Identity().slerp(T(time_), r); t = T(time_) * t; } + // 计算变换后的点位置 Eigen::Matrix p0 = r * p + t; - // norm of cross product: triangle area x 2 - Eigen::Matrix area = (p0 - p1).cross(p0 - p2) * 0.5; + // 利用交叉乘积计算三角形面积（面积 = 0.5 * |(p0-p1) x (p0-p2)|） + Eigen::Matrix 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 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 p(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix p1(T(pt1.x), T(pt1.y), T(pt1.z)); Eigen::Matrix p2(T(pt2.x), T(pt2.y), T(pt2.z)); Eigen::Matrix p3(T(pt3.x), T(pt3.y), T(pt3.z)); + // 构造旋转四元数和平移向量 Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Matrix 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::Identity().slerp(T(time_), r); t = T(time_) * t; } + // 计算变换后的点位置 Eigen::Matrix p0 = r * p + t; + // 计算平面法向量：由平面内两个向量叉乘后归一化得到 Eigen::Matrix 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 bool PointLineCoeffCostFunction::operator()(const T *const r_quat, const T *const t_vec, T *residual) const { + // 将待优化点转换为 Eigen 向量 Eigen::Matrix p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z)); + // 将直线系数转换为模板类型，并分解为直线上的一点 p1 和方向向量 dir Eigen::Matrix coeff = pair_.line_coeff.template cast(); Eigen::Matrix p1 = coeff.topRows(3); Eigen::Matrix dir = coeff.bottomRows(3); + // 构造旋转和平移参数 Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Matrix 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::Identity().slerp(T(time_), r); t = T(time_) * t; } + // 计算变换后的点位置 Eigen::Matrix p0 = r * p + t; + // 计算待优化点到直线的距离误差：计算 (p0-p1) 与直线方向的叉积 Eigen::Matrix cross = (p0 - p1).cross(dir); residual[0] = cross[0]; residual[1] = cross[1]; @@ -208,24 +435,42 @@ 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 bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat, const T *const t_vec, T *residual) const { + // 将待优化点转换为 Eigen 向量 Eigen::Matrix p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z)); + // 将平面系数转换为模板类型的 4 维向量 Eigen::Matrix coeff = pair_.plane_coeff.template cast(); + // 构造旋转四元数和平移向量 Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Matrix 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::Identity().slerp(T(time_), r); t = T(time_) * t; } + // 计算变换后的点位置 Eigen::Matrix p0 = r * p + t; + // 计算点到平面的距离残差：将点 p0 代入平面方程，得到 ax+by+cz+d residual[0] = coeff.topRows(3).transpose() * p0; residual[0] += coeff(3); return true; } -} // namespace oh_my_loam \ No newline at end of file +} // namespace oh_my_loam diff --git a/oh_my_loam/solver/solver.cc b/oh_my_loam/solver/solver.cc index 8629236..efa536a 100644 --- a/oh_my_loam/solver/solver.cc +++ b/oh_my_loam/solver/solver.cc @@ -8,60 +8,140 @@ 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, - new ceres::EigenQuaternionParameterization()); + // 向问题中添加旋转参数块，使用 Eigen 四元数参数化以确保单位四元数的约束 + 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.max_num_iterations = max_iter_num; - options.minimizer_progress_to_stdout = false; - ceres::Solver::Summary summary; + options.linear_solver_type = ceres::DENSE_QR; // 使用密集 QR 分解作为线性求解器 + options.max_num_iterations = max_iter_num; // 最大迭代次数 + options.minimizer_progress_to_stdout = false; // 是否将求解进度输出到标准输出 + 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_); } -} // namespace oh_my_loam \ No newline at end of file +} // namespace oh_my_loam