1. 项目概述当点云遇上高斯牛顿法在三维视觉和机器人领域我们经常遇到一个核心问题如何让两个点云或者一个点云与一个模型精确地对齐无论是自动驾驶中的激光雷达定位与建图SLAM还是工业检测中的三维零件与CAD模型比对其背后都离不开一个强大的数学工具——非线性最小二乘优化。而高斯牛顿法正是解决这类问题的一把经典且高效的“瑞士军刀”。最近在基于Open3D的C项目里我重新梳理和实现了高斯牛顿法用于点云配准。Open3D本身提供了成熟的ICP迭代最近点算法接口但当你需要定制化误差函数、处理带有特殊约束的配准问题或者单纯想深入理解优化算法如何驱动三维数据对齐时亲手实现一遍高斯牛顿法就变得非常必要。这不仅仅是调用一个registration.icp那么简单而是深入到雅可比矩阵计算、海森矩阵近似、线性方程求解的层面去掌控优化的每一个迭代步骤。通过这个项目你将能彻底搞懂高斯牛顿法在三维空间中的应用逻辑掌握如何用C和Open3D从零搭建一个可用的优化器并学会如何诊断和解决迭代过程中可能出现的发散、收敛慢等问题。无论你是正在学习非线性优化的学生还是需要在项目中实现定制化配准算法的工程师这篇内容都能提供从理论到代码的完整路径。2. 高斯牛顿法核心原理与Open3D适配性分析2.1 从最小二乘问题到高斯牛顿迭代我们首先把问题形式化。假设我们有一组观测数据想要找到一个参数向量x在我们这里是旋转矩阵R和平移向量t共6个自由度使得某个模型函数f(x)尽可能接近观测值。这个“接近”的程度用误差的平方和来衡量这就是经典的非线性最小二乘问题最小化目标函数F(x) 0.5 * Σ || r_i(x) ||² 其中r_i(x) 是第i个数据点的残差向量。在点云配准中r_i(x)通常表示源点云中第i个点经过当前位姿变换T(x)后与目标点云中对应点之间的坐标差。高斯牛顿法的核心思想是对非线性函数f(x)进行一阶泰勒展开将原始的非线性最小二乘问题在每次迭代时转化为一个线性最小二乘问题来求解。具体推导如下线性化在当前估计值x_k处对残差函数r(x)进行一阶泰勒展开r(x_k Δx) ≈ r(x_k) J(x_k) * Δx。这里J(x_k)是残差函数关于参数x的雅可比矩阵每一行是单个残差对参数的梯度。构造线性子问题将线性化后的残差代入目标函数得到关于增量Δx的近似二次函数F(x_k Δx) ≈ 0.5 * || r_k J_k * Δx ||²。求解增量为了最小化这个近似目标函数令其关于Δx的导数为零就得到了著名的高斯牛顿方程(J_k^T * J_k) * Δx -J_k^T * r_k这个方程可以看作是用J_k^T * J_k近似了牛顿法中的海森矩阵Hessian。等式左边是一个对称半正定矩阵右边是负梯度。我们求解这个线性方程组就得到了本次迭代的参数更新量Δx。迭代更新更新参数x_{k1} x_k Δx然后重复步骤1-3直到Δx的范数或目标函数值的变化小于某个阈值。与最速下降法相比高斯牛顿法利用了目标函数的曲率信息通过J^T J通常具有更快的收敛速度。与完整的牛顿法相比它省去了计算二阶海森矩阵的巨大开销是一种在计算效率和收敛速度之间取得很好平衡的算法。2.2 为什么在Open3D C中实现高斯牛顿法你可能会问Open3D的RegistrationICP类已经很好用了为什么还要自己造轮子原因在于灵活性和深度控制。定制化误差模型标准的ICP使用点对点或点对平面距离。但如果你有特殊的需求呢比如你想在误差项中加入颜色一致性约束、法向量对齐惩罚或者使用一种鲁棒的核函数如Huber损失来抑制离群点。通过自己实现高斯牛顿框架你可以自由定义残差函数r_i(x)和计算其雅可比矩阵从而融入任何你想要的误差项。理解算法黑箱直接调用库函数是一个“黑箱”。当配准失败时你很难判断问题是出在数据本身、参数设置还是算法内部的某一步。自己实现一遍你可以打印每一迭代的残差、雅可比矩阵的条件数、增量Δx的大小从而对优化过程进行细致的诊断。处理复杂参数化三维刚体变换有6个自由度常用的参数化方式有欧拉角、轴角、四元数平移向量等。不同的参数化方式会影响雅可比矩阵的计算和优化的稳定性。在自定义实现中你可以尝试不同的参数化并观察其对收敛性的影响这是使用固定接口难以做到的。性能考量与集成对于极其庞大的点云你可能需要实现特定的加速结构如自定义的KD树或利用并行计算。在自己的优化循环中你可以更直接地集成这些优化而不是受限于库函数提供的选项。Open3D的C接口为我们提供了强大的基础数据结构PointCloud,KDTreeFlann和数学工具Eigen库集成使得实现高斯牛顿法时我们可以专注于算法逻辑本身而无需从零开始编写最近邻搜索或矩阵运算这大大提高了开发效率。3. 基于Open3D C的高斯牛顿法实现详解3.1 环境搭建与数据准备首先确保你的开发环境就绪。你需要安装Open3D C库建议使用v0.15.2或更高版本。可以通过源码编译安装确保在CMake中勾选了-DBUILD_SHARED_LIBSON和-DBUILD_EXAMPLESON这有助于后续调试。编译完成后设置好CMAKE_PREFIX_PATH以便find_package能定位到Open3D。创建CMake项目一个典型的CMakeLists.txt关键部分如下cmake_minimum_required(VERSION 3.18) project(GaussNewtonRegistration) find_package(Open3D REQUIRED) add_executable(gn_registration main.cpp) target_link_libraries(gn_registration Open3D::Open3D)准备测试点云我们可以使用Open3D内置的数据生成功能创建两个有已知变换关系的点云用于验证算法。#include open3d/Open3D.h using namespace open3d; // 生成源点云一个简单的立方体 auto source_pc std::make_sharedgeometry::PointCloud(); for (double x -0.5; x 0.5; x 0.1) for (double y -0.5; y 0.5; y 0.1) for (double z -0.5; z 0.5; z 0.1) source_pc-points_.push_back(Eigen::Vector3d(x, y, z)); // 定义一個真实的变换矩阵绕Z轴旋转15度沿X轴平移0.5米 Eigen::Matrix4d T_true Eigen::Matrix4d::Identity(); double angle 15.0 * M_PI / 180.0; T_true.block3, 3(0, 0) Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()).toRotationMatrix(); T_true.block3, 1(0, 3) Eigen::Vector3d(0.5, 0.1, 0.2); // 应用变换生成目标点云并添加少量噪声 auto target_pc std::make_sharedgeometry::PointCloud(); target_pc-points_.resize(source_pc-points_.size()); std::random_device rd; std::mt19937 gen(rd()); std::normal_distribution d(0, 0.005); // 高斯噪声标准差5mm for (size_t i 0; i source_pc-points_.size(); i) { Eigen::Vector3d p source_pc-points_[i]; Eigen::Vector3d p_transformed (T_true * p.homogeneous()).head3(); p_transformed Eigen::Vector3d(d(gen), d(gen), d(gen)); target_pc-points_[i] p_transformed; }3.2 参数化与雅可比矩阵计算这是实现中的第一个关键点。我们选择李代数se(3)作为参数化方式。一个刚体变换T属于SE(3)其对应的李代数 ξ 是一个6维向量ξ [ρ, φ]^T其中φ是旋转部分轴角模长为旋转角度ρ与平移相关。变换矩阵T与李代数ξ之间通过指数映射T exp(ξ^∧)关联。对于空间一点p其变换后的坐标为T*p。当我们在当前估计T_k附近考虑一个微小扰动Δξ时有T_k * exp(Δξ^∧) * p ≈ T_k * (I Δξ^∧) * p因此变换后点关于李代数扰动Δξ的导数即雅可比矩阵的一行为∂(T*p) / ∂Δξ [ I, -(T*p)^∧ ] (这是一个3x6的矩阵)这里(T*p)^∧是将三维向量变换为反对称矩阵。这个雅可比矩阵描述了当李代数参数发生微小变化时变换后的点坐标如何变化。在点对点误差r_i T*p_i - q_i其中q_i是目标点云中的对应点中残差r_i关于参数Δξ的雅可比就是J_i [ I, -(T*p_i)^∧ ]。在代码中我们需要为一个点计算这个雅可比Eigen::Matrixdouble, 3, 6 ComputePointJacobian(const Eigen::Vector3d transformed_point) { Eigen::Matrixdouble, 3, 6 J; J.setIdentity(); // 设置左上角3x3的单位矩阵对应平移部分的导数 // 设置右下角3x3的反对称矩阵部分对应旋转部分的导数 J.block3, 3(0, 3) -SkewSymmetric(transformed_point); return J; } Eigen::Matrix3d SkewSymmetric(const Eigen::Vector3d v) { Eigen::Matrix3d S; S 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; return S; }注意这里雅可比的计算是在扰动模型下进行的。我们优化的增量Δξ是定义在当前估计T_k的切空间中即T_{k1} T_k * exp(Δξ^∧)。这种右乘扰动模型在视觉SLAM中非常常用。另一种是左乘扰动T_{k1} exp(Δξ^∧) * T_k其雅可比形式略有不同需要保持一致。3.3 高斯牛顿迭代循环的实现有了雅可比计算的基础我们可以构建完整的高斯牛顿迭代循环。核心步骤如下初始化设定初始变换T_init通常是单位阵或一个粗略估计最大迭代次数max_iteration收敛阈值epsilon。构建KDTree为目标点云target_pc构建一个KDTreeFlann用于在每次迭代中快速查找最近邻点。迭代循环 a.数据关联对于源点云中的每一个点p_src用当前变换T_current将其变换到目标坐标系p_transformed T_current * p_src。然后在目标点云的KDTree中搜索最近邻点q_target。这一步确定了残差项r_i p_transformed - q_target。 b.构造线性系统对于每一个有效的对应点对计算残差r_i和雅可比矩阵J_i。然后累加得到线性系统的系数矩阵H Σ(J_i^T * J_i)和右侧向量b -Σ(J_i^T * r_i)。注意H是一个6x6的矩阵b是6维向量。 c.求解增量求解线性方程H * Δξ b。由于H可能不满秩例如所有点共面时我们通常使用稳健的求解器如LDLT或QR分解。在C中使用Eigen库非常方便cpp Eigen::Matrixdouble, 6, 1 delta_xi H.ldlt().solve(b); // 使用LDLT分解求解 // 或者使用更稳健的带阻尼的高斯牛顿法Levenberg-Marquardt // Eigen::Matrixdouble, 6, 1 delta_xi (H lambda * Eigen::Matrix6d::Identity()).ldlt().solve(b);d.更新变换将李代数增量Δξ转换为变换矩阵ΔT exp(Δξ^∧)然后更新当前估计T_current T_current * ΔT。 e.检查收敛计算增量Δξ的范数或前后两次迭代目标函数值的变化。如果小于阈值epsilon则提前终止循环。输出结果返回最终优化得到的变换矩阵T_current。一个简化的核心循环代码框架如下Eigen::Matrix4d GaussNewtonRegistration( const geometry::PointCloud source, const geometry::PointCloud target, const Eigen::Matrix4d init_transformation, int max_iterations, double epsilon) { Eigen::Matrix4d T init_transformation; geometry::KDTreeFlann kdtree(target); double prev_cost std::numeric_limitsdouble::max(); for (int iter 0; iter max_iterations; iter) { Eigen::Matrixdouble, 6, 6 H Eigen::Matrixdouble, 6, 6::Zero(); Eigen::Matrixdouble, 6, 1 b Eigen::Matrixdouble, 6, 1::Zero(); double total_cost 0.0; int effective_correspondence 0; for (size_t i 0; i source.points_.size(); i) { Eigen::Vector3d p_src source.points_[i]; Eigen::Vector3d p_transformed (T * p_src.homogeneous()).head3(); std::vectorint indices(1); std::vectordouble dists(1); if (kdtree.SearchKNN(p_transformed, 1, indices, dists) 0) { Eigen::Vector3d q_tar target.points_[indices[0]]; Eigen::Vector3d r p_transformed - q_tar; total_cost r.squaredNorm(); Eigen::Matrixdouble, 3, 6 J ComputePointJacobian(p_transformed); H J.transpose() * J; b - J.transpose() * r; effective_correspondence; } } if (effective_correspondence 0) { utility::LogWarning(No valid correspondence found in iteration {}., iter); break; } // 求解高斯牛顿方程 Eigen::Matrixdouble, 6, 1 delta_xi H.ldlt().solve(b); // 更新变换矩阵 T T * exp(delta_xi^∧) T T * TransformVector6dToMatrix4d(delta_xi); double mean_cost total_cost / effective_correspondence; utility::LogInfo(Iteration {}: cost {}, |delta_xi| {}, iter, mean_cost, delta_xi.norm()); // 检查收敛条件 if (delta_xi.norm() epsilon || std::abs(prev_cost - mean_cost) epsilon * prev_cost) { utility::LogInfo(Converged at iteration {}., iter); break; } prev_cost mean_cost; } return T; }其中TransformVector6dToMatrix4d函数负责将6维李代数向量[ρ, φ]转换为变换矩阵exp(ξ^∧)这涉及到罗德里格斯公式。3.4 可视化与调试技巧在开发过程中可视化是强大的调试工具。Open3D提供了便捷的可视化接口。初始状态可视化在优化开始前将源点云设为红色、目标点云设为蓝色以及初始变换后的源点云设为绿色一起显示可以直观看到初始对齐的偏差。visualization::Visualizer vis; vis.CreateVisualizerWindow(Gauss-Newton Registration, 1600, 900); auto source_copy std::make_sharedgeometry::PointCloud(source); source_copy-PaintUniformColor(Eigen::Vector3d(1, 0, 0)); // 红色-源点云 auto target_copy std::make_sharedgeometry::PointCloud(target); target_copy-PaintUniformColor(Eigen::Vector3d(0, 0, 1)); // 蓝色-目标点云 vis.AddGeometry(source_copy); vis.AddGeometry(target_copy); vis.Run(); // 阻塞式显示关闭窗口后继续迭代过程可视化在每次高斯牛顿迭代后更新变换矩阵并刷新显示可以动态观察点云是如何一步步对齐的。这对于理解算法的收敛行为非常有帮助也能快速发现是否陷入了局部极小值或发生发散。数值监控除了可视化将每次迭代的关键数值打印出来至关重要。需要监控目标函数值平均残差平方它应该随着迭代单调下降。如果出现上升说明步长可能太大或者线性化在当前位置不成立需要考虑使用带阻尼的策略即Levenberg-Marquardt。增量Δξ的范数它衡量了参数更新的幅度是判断收敛的重要指标。矩阵H的条件数cond(H) |λ_max| / |λ_min|。如果条件数非常大例如 1e10说明H接近奇异线性方程求解不稳定。这通常意味着数据退化例如点云共面或者参数化存在万向节锁问题。此时增加一个阻尼因子λH λI可以改善数值稳定性。有效对应点数量如果大量源点找不到最近邻例如距离超过阈值有效对应点会很少导致H矩阵信息不足优化不可靠。需要检查最近邻搜索的距离阈值设置是否合理。4. 性能优化与鲁棒性增强实战一个基础版本的高斯牛顿法实现后我们通常会面临两个挑战速度太慢和对异常值太敏感。下面分享几个实战中的优化和增强技巧。4.1 加速策略从O(N^2)到O(N log N)最耗时的操作是最近邻搜索。在每次迭代中对每个变换后的源点都在目标点云中进行全局搜索是O(N*M)的复杂度N和M是点云大小这是不可接受的。使用KDTree正如我们之前做的为目标点云构建一个KDTree可以将每次最近邻搜索的复杂度从O(M)降低到O(log M)。Open3D的KDTreeFlann类对此提供了很好的支持。对应关系缓存在高斯牛顿法收敛的过程中点云之间的对应关系变化不会特别剧烈。因此不必在每一次迭代中都为每一个点重新搜索最近邻。可以采用一种“懒惰更新”策略每隔2-3次迭代才完全更新一次对应关系在中间迭代中复用之前的对应关系。这能显著减少KDTree搜索的调用次数。多线程并行构造线性系统计算每个点的残差和雅可比并累加到H和b中是一个天然的并行任务。可以使用OpenMP或C标准库的thread或execution来并行化这个循环。#include execution #include mutex std::mutex H_b_mutex; // ... 在迭代循环内 ... std::for_each(std::execution::par, indices.begin(), indices.end(), [](size_t i) { // 为第i个点计算残差和雅可比 Eigen::Matrixdouble, 3, 6 J_i ...; Eigen::Vector3d r_i ...; Eigen::Matrixdouble, 6, 6 H_local J_i.transpose() * J_i; Eigen::Matrixdouble, 6, 1 b_local -J_i.transpose() * r_i; // 使用互斥锁保护对全局H和b的累加 std::lock_guardstd::mutex lock(H_b_mutex); H H_local; b b_local; });注意频繁的加锁可能成为瓶颈。另一种模式是每个线程计算自己的局部H和b最后再合并效率更高。4.2 提升鲁棒性应对离群点与局部极小值点云数据中不可避免地存在噪声、遮挡和错误匹配离群点。标准的最小二乘L2范数对离群点非常敏感因为残差平方会放大大误差的影响。使用鲁棒核函数核心思想是降低大残差项的权重。我们引入一个鲁棒核函数ρ(s)其中s ||r||²。新的目标函数变为Σ ρ(||r_i||²)。对应地高斯牛顿方程中的b项需要乘以一个权重因子w_i ρ(s_i)。常见的核函数有Huber Lossρ(s) { s (if s δ), 2δ√s - δ² (otherwise) }。它对小误差使用L2范数对大误差使用L1范数平滑过渡。Cauchy Lossρ(s) c² * log(1 s/c²)。它对大误差的抑制更强。 在代码中计算完残差r_i后根据其范数计算权重w_i然后将J_i和r_i分别乘以sqrt(w_i)再累加到H和b中。这等价于求解加权最小二乘问题。距离阈值过滤最简单有效的方法。在搜索到最近邻点q后计算距离d ||T*p - q||。如果d大于一个预设阈值例如点云 bounding box 对角线的5%则认为这个对应关系不可靠在本次迭代中将其丢弃不参与线性系统的构建。这个阈值可以随着迭代的进行逐渐减小称为“递减距离阈值”策略初期允许较大的对应距离以捕获大范围的变换后期收紧以提高精度。从点到面的距离对于具有法向量的点云如从网格或RGB-D相机获得使用点对平面距离作为残差比点对点距离收敛更快、更稳定。残差定义为源点变换后到目标点对应点切平面的距离r_i n_i · (T*p_i - q_i)其中n_i是目标点q_i的法向量。此时的雅可比矩阵也需要相应调整变为J_i n_i^T * [ I, -(T*p_i)^∧ ]。这通常需要目标点云预先计算好法向量。4.3 阻尼高斯牛顿法Levenberg-Marquardt 实践基础高斯牛顿法在初始估计很差或问题高度非线性时J^T J矩阵可能不是正定的导致求解的增量Δx方向错误甚至发散。Levenberg-Marquardt (LM) 算法通过引入一个阻尼因子λ来混合高斯牛顿法和最速下降法极大地增强了稳定性。LM算法求解的方程是(J^T J λ I) * Δx -J^T r当λ很大时方程近似为λ I * Δx -J^T r即Δx ≈ - (1/λ) J^T r这等价于最速下降法的一小步方向稳定但收敛慢。当λ很小时方程退化为标准高斯牛顿法收敛快但不稳定。LM算法的自适应阻尼调整策略计算增益比ρ (实际代价下降量) / (线性模型预测的代价下降量)。如果ρ很大例如 0.75说明线性模型拟合得很好可以增大λ例如除以一个因子v2~10以更接近高斯牛顿法加快收敛。如果ρ很小例如 0.25说明线性模型拟合得很差需要减小λ乘以因子v以更接近最速下降法保证稳定性。如果ρ适中则保持λ不变。在代码中实现LM算法就是在求解线性方程前给H矩阵的对角线加上λEigen::Matrixdouble, 6, 6 H_lm H lambda * Eigen::Matrixdouble, 6, 6::Identity(); Eigen::Matrixdouble, 6, 1 delta_xi H_lm.ldlt().solve(b);然后根据本次迭代后的代价函数实际下降情况按照上述策略更新λ。一个简单的LM实现就能让你的优化器在面对糟糕初始值时依然稳健。5. 常见问题排查与性能诊断指南即使算法实现正确在实际运行中也可能遇到各种问题。下面是一个基于实践经验的排查清单。5.1 优化过程发散或不收敛症状目标函数值残差平方和随着迭代不断增大或者震荡不定无法降低到一个稳定值。排查步骤检查雅可比矩阵在第一次迭代时打印出几个点的雅可比矩阵J_i检查其数值是否合理没有NaN或Inf。确保ComputePointJacobian函数中反对称矩阵的计算是正确的。检查H矩阵打印H矩阵及其特征值。如果H有负特征值或条件数极大说明问题病态。解决方案立即切换到LM算法使用一个较大的初始λ如1.0。检查增量Δξ打印每次迭代的Δξ。如果其模长非常大例如 10说明步长太大。解决方案在更新变换时尝试使用更小的步长例如T T * exp(0.5 * delta_xi^∧)或者使用线搜索Armijo规则来找到一个能保证代价下降的步长αT T * exp(α * delta_xi^∧)。检查数据关联可视化当前迭代的对应关系。可能会发现大量错误的匹配一个源点匹配到了很远的目标点。解决方案严格的距离阈值过滤至关重要。初始阶段可以设置较大的阈值随着迭代逐步收紧。初始值太差如果初始变换离真实值太远点云几乎没有重叠区域那么基于最近邻的数据关联会完全错误导致优化走向歧途。解决方案需要提供一个更好的初始估计。可以通过手动选取3-4个对应点进行SVD分解求初始变换或者使用全局描述子如FPFH进行粗配准。5.2 收敛速度慢症状目标函数值下降但非常缓慢需要很多次迭代才能达到收敛阈值。排查与优化使用点对面距离如果点云有法向量务必使用点对面距离。它的收敛盆地收敛域比点对点距离大得多通常能以更少的迭代次数达到更高的精度。检查H矩阵的条件数条件数过大意味着某些参数方向上的曲率远大于其他方向优化会像在狭窄的山谷中行走进展缓慢。解决方案可以对参数进行缩放预处理使所有自由度的量级大致相同。例如将旋转部分弧度制和平移部分米调整到相近的数值范围。启用LM算法的自适应策略在收敛后期高斯牛顿法接近二次收敛速度极快。确保你的LM实现能在ρ较大时显著减小λ以充分利用高斯牛顿法的快速收敛性。对应关系更新频率如4.1节所述不必每次迭代都更新所有对应关系。尝试每2-3次迭代更新一次KDTree和对应点可以节省大量时间且对收敛速度影响很小。5.3 最终配准精度不足症状算法收敛了但变换后的源点云和目标点云之间仍有肉眼可见的偏差。排查与优化检查收敛阈值可能迭代停止得太早了。尝试减小收敛阈值epsilon例如从1e-6降到1e-8让算法运行更多迭代。离群点干扰即使算法收敛残留的离群点匹配仍会拉偏结果。解决方案采用更严格的距离阈值或者引入鲁棒核函数如Cauchy核在迭代后期进一步抑制大残差的影响。数据本身的问题点云噪声过大、密度不均匀、或者存在非重叠区域。优化算法只能对齐重叠部分。解决方案预处理点云进行下采样使用VoxelDownSample使其密度均匀并统计有效匹配点的比例。如果比例过低如30%说明重叠区域太小需要考虑其他配准策略或获取更多数据。陷入局部极小值这是非线性优化的固有问题。解决方案尝试多个不同的初始变换例如在初始估计上加一些随机小扰动从多个起点运行优化选择最终代价函数最小的那个结果。5.4 一个实用的调试输出模板在开发时在每次迭代中输出以下关键信息可以快速定位问题utility::LogInfo(Iter {}: cost {:.6e}, lambda {:.2e}, |dx| {:.4e}, cond(H) {:.2e}, inliers {}/{}, iter, current_cost, lambda, delta_xi.norm(), H_condition_number, inlier_count, total_points);cost监控下降趋势。lambdaLM算法监控阻尼因子的调整。|dx|监控更新量判断收敛。cond(H)判断问题是否病态。inliers有效内点比例判断数据关联质量。实现一个稳定、高效的高斯牛顿法优化器是深入理解三维视觉中优化问题的绝佳途径。它不仅仅是一个配准工具其框架可以广泛应用于Bundle Adjustment、传感器标定、模型拟合等任何可以表示为非线性最小二乘的问题。当你亲手实现了它并成功地将两片杂乱的点云严丝合缝地对齐时那种对底层原理的掌控感是单纯调用库函数无法比拟的。在后续的项目中你可以尝试将误差项替换为点对线的距离用于配准线段特征或者加入IMU预积分约束构建更复杂的融合优化模型这套高斯牛顿法的骨架依然会是你的核心工具。