-
本篇目标：拿《数值分析》中经典的例题 4.21（求三个圆的最近交点）开刀，手写代码验证。这只是个起点，后续会深入非线性最优化和 LM 算法。
1. 例题 4.21 题目文字内容（可复制）
为了方便你复制，我把书中例题 4.21 及延伸的 4.23 文字提取如下：
例 4.21
考虑中心在(x1,y1)=(−1,0), (x2,y2)=(1,1/2), (x3,y3)=(1,−1/2) 的三个圆，半径分别是R1=1, R2=1/2, R3=1/2。使用高斯-牛顿方法找出一个点，使得它到这三个圆的距离的平方和最小。
(注：在工程上，这就是求解 3 个球面方程在最小二乘意义下的最优交点，也是 GPS 定位的基础原理。)
2.条件数（Condition Number）是怎么算出来的？
你代码里这段：
Eigen::JacobiSVD<Eigen::Matrix2d> svd(A);
double cond_num = svd.singularValues()(0) / svd.singularValues()(1);
这就是计算条件数（Condition Number）的终极标准方法：最大奇异值除以最小奇异值。
💡 为什么需要条件数？
想象你在解方程Ax=b。如果矩阵 A 稍微变一点点，解x就发生剧变，那这个方程就是“病态”的。在计算机里，浮点数本身就有微小的舍入误差，如果矩阵病态，误差会被无限放大，算出来的结果全错。
📐 它是怎么算出来的？
对于一个矩阵A，我们怎么衡量它到底“放大”了多少误差？
数学上，把矩阵的最大奇异值
与最小奇异值
的比值，定义为条件数：
代表矩阵能把一个向量“拉长”的最大倍数。
代表矩阵能把一个向量“压扁”的最小倍数。- 如果
接近于 0，说明矩阵把某个方向的空间直接压扁成了 0（也就是矩阵奇异/不可逆）。此时分母接近 0，条件数κ趋近于无穷大。
⚠️ 工程判定标准
- κ 接近 1：极其完美（比如单位矩阵 I，转置矩阵 Q）。
- κ<100：很好，计算机随便算。
- κ>10E6：警告！矩阵开始病态，结果可能丢掉一半的有效数字。
- κ>10E15：灾难（双精度 double 有效数字才 15 位），说明矩阵已经奇异，准备除以 0 或者程序崩溃。
在代码里，
只有 2×2。Eigen 的 SVD 底层会极其迅速地把两个奇异值算出来，一除，你就知道当前的点是不是处于“退化”位置（比如三个圆心恰好共线，导致方向重合）。如果条件数太大了，你就得切到 LM 算法去加阻尼λ拯救它。
3.colPivHouseholderQr() 和 .solve() 底层在干嘛？
这段代码：
Eigen::Vector2d delta = A.colPivHouseholderQr().solve(b);
它等价于在解方程：
。
这里的A是
，b 是
。
为什么不直接求逆？( 不要写 A.inverse() * b )
因为算逆矩阵极慢，且浮点误差极大。Eigen 底层是这样执行的：
。
。所以方程两边乘
，变成了
。4.代码
用 C++ 和 Eigen 写了一个完整的高斯-牛顿法求解器。
这段代码不仅会计算，还会把每次迭代的雅可比矩阵
、法线矩阵
的条件数、步长和残差平方和全部打印出来。这样可以直观地看到高斯-牛顿法是如何一步步逼近解，以及在初值极差时是如何“发疯”的。
#include <iostream>
#include <vector>
#include <cmath>
#include <iomanip>
#include <Eigen/Dense>
/**
* @brief 圆的参数结构体
*/
struct Circle {
double x; ///< 圆心 x 坐标
double y; ///< 圆心 y 坐标
double r; ///< 圆半径
};
/**
* @brief 计算当前点的残差向量 r
* @param pos 当前估计点 (x, y)
* @param circles 所有的圆
* @return 残差向量，长度为圆的个数
*/
Eigen::VectorXd computeResiduals(const Eigen::Vector2d& pos, const std::vector<Circle>& circles) {
Eigen::VectorXd r(circles.size());
for (size_t i = 0; i < circles.size(); ++i) {
double dx = pos.x() – circles[i].x;
double dy = pos.y() – circles[i].y;
double dist = std::sqrt(dx * dx + dy * dy);
// 工程防坑：防止点恰好落在圆心上导致除以 0（虽然在雅可比里处理，但残差也要防一下）
if (dist < 1e-12) dist = 1e-12;
r(i) = dist – circles[i].r;//距离减去半径
}
return r;
}
/**
* @brief 计算雅可比矩阵 Dr (Jacobian)
* @param pos 当前估计点 (x, y)
* @param circles 所有的圆
* @return 雅可比矩阵 (3 x 2)
*/
Eigen::MatrixXd computeJacobian(const Eigen::Vector2d& pos, const std::vector<Circle>& circles) {
//雅可比矩阵：列数：未知数个数；行数：方程个数
Eigen::MatrixXd Dr(circles.size(), 2);
for (size_t i = 0; i < circles.size(); ++i) {
double dx = pos.x() – circles[i].x;
double dy = pos.y() – circles[i].y;
double S = std::sqrt(dx * dx + dy * dy);
// 极端防坑：如果当前点就在圆心上，导数趋于无穷大，用一个极小值截断保护
if (S < 1e-12) S = 1e-12;
Dr(i, 0) = dx / S; // 对 x 的偏导
Dr(i, 1) = dy / S; // 对 y 的偏导
}
return Dr;
}
int main() {
// 1. 构造例题 4.21 的三个圆数据
std::vector<Circle> circles = {
{-1.0, 0.0, 1.0}, // 圆心 (-1, 0), 半径 1
{ 1.0, 0.5, 0.5}, // 圆心 (1, 1/2), 半径 1/2
{ 1.0, -0.5, 0.5} // 圆心 (1, -1/2), 半径 1/2
};
// 2. 设置初始猜测值 (你可以修改这里来测试病态发散)
// 正常初值: (0, 0)
// 极端初值(测试病态): Eigen::Vector2d pos(1000.0, 1000.0);
Eigen::Vector2d pos(10000, 10000);
std::cout << "====================================================\\n";
std::cout << "开始高斯-牛顿法迭代, 初始点: (" << pos.x() << ", " << pos.y() << ")\\n";
std::cout << "====================================================\\n";
int max_iter = 100;
double tolerance = 1e-6;//容差
for (int iter = 0; iter < max_iter; ++iter) {
// 1. 计算当前残差 r
Eigen::VectorXd r = computeResiduals(pos, circles);
double cost = 0.5 * r.squaredNorm(); // 最小化目标函数 E(x) = 1/2 * r^T * r
// 2. 计算雅可比矩阵 Dr
Eigen::MatrixXd Dr = computeJacobian(pos, circles);
// 3. 计算 A = Dr^T * Dr (即法线矩阵) 和 b = -Dr^T * r
Eigen::Matrix2d A = Dr.transpose() * Dr;
Eigen::Vector2d b = -Dr.transpose() * r;
// 【工程亮点 1】检查 A 的条件数，看看矩阵是不是病态了
// 条件数越大，说明矩阵越接近奇异，高斯-牛顿越容易发散
Eigen::JacobiSVD<Eigen::Matrix2d> svd(A);
double cond_num = svd.singularValues()(0) / svd.singularValues()(1);
// 4. 解方程 A * delta = b 求步长
// 使用带列主元的 QR 分解，保证即使 A 稍微病态也能稳定求解
Eigen::Vector2d delta = A.colPivHouseholderQr().solve(b);//求步长
// — 打印详细信息 —
std::cout << "\\n— 迭代次数: " << iter << " —\\n";
std::cout << "当前点 (x, y) : [" << std::setw(10) << pos.x() << ", " << std::setw(10) << pos.y() << "]\\n";
std::cout << "目标函数值 (0.5*E^2) : " << cost << "\\n";
std::cout << "矩阵 A 的条件数 : " << cond_num << (cond_num > 1e6 ? " <– 警告: 极其病态!" : "") << "\\n";
std::cout << "雅可比矩阵 Dr:\\n" << Dr << "\\n";
std::cout << "步长 delta (v^k) : [" << delta.x() << ", " << delta.y() << "]\\n";
// 5. 更新参数
pos += delta;
// 6. 检查是否收敛 (步长足够小)
if (delta.norm() < tolerance) {
std::cout << "\\n=> 满足收敛条件 ||delta|| < " << tolerance << "\\n";
break;
}
}
std::cout << "\\n====================================================\\n";
std::cout << "最终结果: 最优拟合点 (x, y) = (" << pos.x() << ", " << pos.y() << ")\\n";
std::cout << "理论期望值: (0.412891, 0)\\n";
std::cout << "====================================================\\n";
return 0;
}
5.代码在书中的解释
最终结果与书中一致。
6.病态测试运行结果
初始点设置：10000，10000；条件数非常大，运行到90次后就收敛了。
刚开始几次运行结果
最后几次运行结果
