C++手写高斯牛顿迭代法

手写高斯-牛顿迭代法

步骤：
1、为待解参数设置初始值。
2、在每一次迭代中，对每个参数单独求偏导数作为雅可比矩阵的一个值，得到雅可比矩阵J。
3、对每个数据，$H+=J{J}^T$, $g+=-f(x)J$。
4、遍历完所有数据后，求解Hx=g，然后对所有参数加上x中对应位置的偏差值。
5、进入下一次遍历，重复2-4步骤。

1. 计算ceres的powell问题

求解ceres中的powell问题
powell有四个输入参数，由四部分式子构成：
x1+10x2
√￣5 (x3-x4)
(x2-2x3)^2
√￣10 (x1-x4)^2
要拟合出x1, x2, x3, x4的最小二乘解使得4个式子的平方和尽可能小。

#include 
#include 
#include "math.h"

#include 
#include 

#include 
#include 

using namespace Eigen;
using namespace std;
using namespace cv;

// 手写高斯-牛顿迭代法求解ceres中的powell问题
// powell有四个输入参数，由四部分式子构成：
// x1+10x2
//  √￣5 (x3-x4)
// (x2-2x3)^2
// √￣10 (x1-x4)^2
// 要拟合出x1, x2, x3, x4的最小二乘解使得4个式子的平方和尽可能小

int main(int argc, char **argv)
{
    // 初始化需要预测的参数，自己先随便填写数字
    double x1 = 3.0, x2 = -1.0, x3 = 2.0, x4 = 1.0;

    // 迭代次数
    int iterations = 100;
    // 记录迭代的cost
    double cost = 0;

    // 开始一轮迭代
    for (int iter = 0; iter < iterations; iter++)
    {
        // 要解的Hx=g
        Matrix4d H = Matrix4d::Zero();
        Vector4d g = Vector4d::Zero();
        // 记录一轮的cost
        cost = 0;

        // 对四个组成部分分别求f(x)和J
        Vector4d J;
        double f;

        // ----------------- x1+10x2
        // 求f(x)
        f = x1 + 10 * x2;
        cost += f * f;
        // 求雅可比矩阵J
        J = Vector4d::Zero();
        J[0] = 1;
        J[1] = 10;

        H += J * J.transpose();
        g += -f * J;

        // ----------------- √￣5 (x3-x4)
        // 求f(x)
        f = sqrt(5) * (x3 - x4);
        cost += f * f;
        // 求雅可比矩阵J
        J = Vector4d::Zero();
        J[2] = sqrt(5);
        J[3] = -sqrt(5);

        H += J * J.transpose();
        g += -f * J;

        // ----------------- (x2-2x3)^2
        // 求f(x)
        f = (x2 - 2 * x3) * (x2 - 2 * x3);
        cost += f * f;
        // 求雅可比矩阵J
        J = Vector4d::Zero();
        J[1] = 2 * x2 - 4 * x3;
        J[2] = 8 * x3 - 4 * x2;

        H += J * J.transpose();
        g += -f * J;

        // ----------------- √￣10 (x1-x4)^2
        // 求f(x)
        f = sqrt(10) * (x1 - x4) * (x1 - x4);
        cost += f * f;
        // 求雅可比矩阵J
        J = Vector4d::Zero();
        J[0] = 2 * sqrt(10) * x1 - 2 * sqrt(10) * x4;
        J[3] = 2 * sqrt(10) * x4 - 2 * sqrt(10) * x1;

        H += J * J.transpose();
        g += -f * J;

        // 求解Hx=g
        Vector4d dx = H.ldlt().solve(g);
        if (isnan(dx[0]))
        {
            cout << "result is nan" << endl;
            break;
        }

        // 打印每一轮迭代的cost
        cout << "iter: " << iter << "  cost: " << cost;
        // 打印当前参数拟合值
        cout << "    x1: " << x1 << "  x2: " << x2 << "    x3: " << x3 << "  x4: " << x4 << endl;

        // 更新预测值
        x1 += dx[0];
        x2 += dx[1];
        x3 += dx[2];
        x4 += dx[3];
    }

    // 最终四个参数都逐渐收敛趋近于0

    return 0;
}

运行得到：

iter: 0  cost: 839    x1: 3  x2: -1    x3: 2  x4: 1
iter: 1  cost: 49.0625    x1: 2.14286  x2: -0.214286    x3: 1.14286  x4: 1.14286
iter: 2  cost: 3.06641    x1: 1.07143  x2: -0.107143    x3: 0.571429  x4: 0.571429
iter: 3  cost: 0.19165    x1: 0.535714  x2: -0.0535714    x3: 0.285714  x4: 0.285714
iter: 4  cost: 0.0119781    x1: 0.267857  x2: -0.0267857    x3: 0.142857  x4: 0.142857
iter: 5  cost: 0.000748634    x1: 0.133929  x2: -0.0133929    x3: 0.0714286  x4: 0.0714286
iter: 6  cost: 4.67896e-05    x1: 0.0669643  x2: -0.00669643    x3: 0.0357143  x4: 0.0357143
iter: 7  cost: 2.92435e-06    x1: 0.0334821  x2: -0.00334821    x3: 0.0178571  x4: 0.0178571
iter: 8  cost: 1.82772e-07    x1: 0.0167411  x2: -0.00167411    x3: 0.00892857  x4: 0.00892857
iter: 9  cost: 1.14233e-08    x1: 0.00837054  x2: -0.000837054    x3: 0.00446429  x4: 0.00446429
iter: 10  cost: 7.13953e-10    x1: 0.00418527  x2: -0.000418527    x3: 0.00223214  x4: 0.00223214
iter: 11  cost: 4.46221e-11    x1: 0.00209263  x2: -0.000209263    x3: 0.00111607  x4: 0.00111607
iter: 12  cost: 2.78888e-12    x1: 0.00104632  x2: -0.000104632    x3: 0.000558036  x4: 0.000558036
iter: 13  cost: 1.74305e-13    x1: 0.000523158  x2: -5.23158e-05    x3: 0.000279018  x4: 0.000279018
iter: 14  cost: 1.08941e-14    x1: 0.000261579  x2: -2.61579e-05    x3: 0.000139509  x4: 0.000139509
iter: 15  cost: 6.80879e-16    x1: 0.00013079  x2: -1.3079e-05    x3: 6.97545e-05  x4: 6.97545e-05
iter: 16  cost: 4.25549e-17    x1: 6.53948e-05  x2: -6.53948e-06    x3: 3.48772e-05  x4: 3.48772e-05
iter: 17  cost: 2.65968e-18    x1: 3.26974e-05  x2: -3.26974e-06    x3: 1.74386e-05  x4: 1.74386e-05
iter: 18  cost: 1.6623e-19    x1: 1.63487e-05  x2: -1.63487e-06    x3: 8.71931e-06  x4: 8.71931e-06
iter: 19  cost: 1.03894e-20    x1: 8.17435e-06  x2: -8.17435e-07    x3: 4.35965e-06  x4: 4.35965e-06
iter: 20  cost: 6.49337e-22    x1: 4.08718e-06  x2: -4.08718e-07    x3: 2.17983e-06  x4: 2.17983e-06
iter: 21  cost: 4.05835e-23    x1: 2.04359e-06  x2: -2.04359e-07    x3: 1.08991e-06  x4: 1.08991e-06
iter: 22  cost: 2.53647e-24    x1: 1.02179e-06  x2: -1.02179e-07    x3: 5.44957e-07  x4: 5.44957e-07
iter: 23  cost: 1.58529e-25    x1: 5.10897e-07  x2: -5.10897e-08    x3: 2.72478e-07  x4: 2.72478e-07
iter: 24  cost: 9.90809e-27    x1: 2.55448e-07  x2: -2.55448e-08    x3: 1.36239e-07  x4: 1.36239e-07
iter: 25  cost: 6.19256e-28    x1: 1.27724e-07  x2: -1.27724e-08    x3: 6.81196e-08  x4: 6.81196e-08
iter: 26  cost: 3.87035e-29    x1: 6.38621e-08  x2: -6.38621e-09    x3: 3.40598e-08  x4: 3.40598e-08
iter: 27  cost: 2.41897e-30    x1: 3.19311e-08  x2: -3.19311e-09    x3: 1.70299e-08  x4: 1.70299e-08
iter: 28  cost: 1.44168e-31    x1: 1.5708e-08  x2: -1.5708e-09    x3: 8.45057e-09  x4: 8.45057e-09
iter: 29  cost: 5.72765e-33    x1: 7.0615e-09  x2: -7.0615e-10    x3: 3.74673e-09  x4: 3.74673e-09
iter: 30  cost: 2.74598e-35    x1: 3.9613e-10  x2: -3.9613e-11    x3: -8.10577e-10  x4: -8.10577e-10
iter: 31  cost: 2.74598e-35    x1: 3.9613e-10  x2: -3.9613e-11    x3: -8.10577e-10  x4: -8.10577e-10
iter: 32  cost: 2.74598e-35    x1: 3.9613e-10  x2: -3.9613e-11    x3: -8.10577e-10  x4: -8.10577e-10
iter: 33  cost: 2.74598e-35    x1: 3.9613e-10  x2: -3.9613e-11    x3: -8.10577e-10  x4: -8.10577e-10
iter: 34  cost: 2.74598e-35    x1: 3.9613e-10  x2: -3.9613e-11    x3: -8.10577e-10  x4: -8.10577e-10
iter: 35  cost: 2.74598e-35    x1: 3.9613e-10  x2: -3.9613e-11    x3: -8.10577e-10  x4: -8.10577e-10
...

2. 计算ceres的curve_fitting问题

求解ceres中的curve_fitting问题
噪声：

noise = randn(size(x)) * 0.2;

拟合公式：

y = exp(m * x + c);

实现代码：

#include 
#include 
#include "math.h"

#include 
#include 

#include 
#include 

using namespace Eigen;
using namespace std;
using namespace cv;

// 手写高斯-牛顿迭代法求解ceres中的curve_fitting问题
// y = exp(m * x + c)

// ceres提供的数据部分
const int kNumObservations = 67;
// clang-format off
const double data[] = {
  0.000000e+00, 1.133898e+00,
  7.500000e-02, 1.334902e+00,
  1.500000e-01, 1.213546e+00,
  2.250000e-01, 1.252016e+00,
  3.000000e-01, 1.392265e+00,
  3.750000e-01, 1.314458e+00,
  4.500000e-01, 1.472541e+00,
  5.250000e-01, 1.536218e+00,
  6.000000e-01, 1.355679e+00,
  6.750000e-01, 1.463566e+00,
  7.500000e-01, 1.490201e+00,
  8.250000e-01, 1.658699e+00,
  9.000000e-01, 1.067574e+00,
  9.750000e-01, 1.464629e+00,
  1.050000e+00, 1.402653e+00,
  1.125000e+00, 1.713141e+00,
  1.200000e+00, 1.527021e+00,
  1.275000e+00, 1.702632e+00,
  1.350000e+00, 1.423899e+00,
  1.425000e+00, 1.543078e+00,
  1.500000e+00, 1.664015e+00,
  1.575000e+00, 1.732484e+00,
  1.650000e+00, 1.543296e+00,
  1.725000e+00, 1.959523e+00,
  1.800000e+00, 1.685132e+00,
  1.875000e+00, 1.951791e+00,
  1.950000e+00, 2.095346e+00,
  2.025000e+00, 2.361460e+00,
  2.100000e+00, 2.169119e+00,
  2.175000e+00, 2.061745e+00,
  2.250000e+00, 2.178641e+00,
  2.325000e+00, 2.104346e+00,
  2.400000e+00, 2.584470e+00,
  2.475000e+00, 1.914158e+00,
  2.550000e+00, 2.368375e+00,
  2.625000e+00, 2.686125e+00,
  2.700000e+00, 2.712395e+00,
  2.775000e+00, 2.499511e+00,
  2.850000e+00, 2.558897e+00,
  2.925000e+00, 2.309154e+00,
  3.000000e+00, 2.869503e+00,
  3.075000e+00, 3.116645e+00,
  3.150000e+00, 3.094907e+00,
  3.225000e+00, 2.471759e+00,
  3.300000e+00, 3.017131e+00,
  3.375000e+00, 3.232381e+00,
  3.450000e+00, 2.944596e+00,
  3.525000e+00, 3.385343e+00,
  3.600000e+00, 3.199826e+00,
  3.675000e+00, 3.423039e+00,
  3.750000e+00, 3.621552e+00,
  3.825000e+00, 3.559255e+00,
  3.900000e+00, 3.530713e+00,
  3.975000e+00, 3.561766e+00,
  4.050000e+00, 3.544574e+00,
  4.125000e+00, 3.867945e+00,
  4.200000e+00, 4.049776e+00,
  4.275000e+00, 3.885601e+00,
  4.350000e+00, 4.110505e+00,
  4.425000e+00, 4.345320e+00,
  4.500000e+00, 4.161241e+00,
  4.575000e+00, 4.363407e+00,
  4.650000e+00, 4.161576e+00,
  4.725000e+00, 4.619728e+00,
  4.800000e+00, 4.737410e+00,
  4.875000e+00, 4.727863e+00,
  4.950000e+00, 4.669206e+00,
};
// clang-format on

// 计算部分
int main(int argc, char **argv)
{
    // 初始化需要预测的参数，自己先随便填写数字
    double m = 1.0, c = 2.0;

    // 迭代次数
    int iterations = 100;
    // 记录迭代的cost
    double cost = 0;

    // 开始一轮迭代
    for (int iter = 0; iter < iterations; iter++)
    {
        // 要解的Hx=g
        Matrix2d H = Matrix2d::Zero();
        Vector2d g = Vector2d::Zero();
        // 记录一轮的cost
        cost = 0;

        // 对四个组成部分分别求f(x)和J
        Vector2d J;
        double x;
        double y;
        // y - exp(m*x+c)的值
        double f;

        for (int i = 0; i < kNumObservations; i++)
        {
            // 取出数据点
            x = data[2 * i];
            y = data[2 * i + 1];

            // 求cost
            f = y - exp(m * x + c);
            cost += f * f;

            // 求雅可比矩阵J
            J = Vector2d::Zero();
            // 对m求导
            J[0] = -x * exp(m * x + c);
            // 对c求导
            J[1] = -exp(m * x + c);

            // 更新H, g
            H += J * J.transpose();
            g += -f * J;
        }

        // 求解Hx=g
        Vector2d dx = H.ldlt().solve(g);
        if (isnan(dx[0]))
        {
            cout << "result is nan" << endl;
            break;
        }

        // 打印每一轮迭代的cost
        cout << "iter: " << iter << "  cost: " << cost;
        // 打印当前参数拟合值
        cout << "    m: " << m << "  c: " << c << endl;

        // 更新预测值
        m += dx[0];
        c += dx[1];
    }

    return 0;
}

运行得到：

iter: 0  cost: 7.70323e+06    m: 1  c: 2
iter: 1  cost: 1.03214e+06    m: 0.992464  c: 1.04079
iter: 2  cost: 135967    m: 0.972349  c: 0.149844
iter: 3  cost: 17128.4    m: 0.920316  c: -0.566805
iter: 4  cost: 1929.55    m: 0.796868  c: -0.885647
iter: 5  cost: 166.766    m: 0.566821  c: -0.557554
iter: 6  cost: 7.74483    m: 0.345839  c: 0.01391
iter: 7  cost: 2.1294    m: 0.29435  c: 0.12725
iter: 8  cost: 2.1135    m: 0.291882  c: 0.131378
iter: 9  cost: 2.1135    m: 0.291871  c: 0.131401
iter: 10  cost: 2.1135    m: 0.291871  c: 0.131401
iter: 11  cost: 2.1135    m: 0.291871  c: 0.131401
iter: 12  cost: 2.1135    m: 0.291871  c: 0.131401
iter: 13  cost: 2.1135    m: 0.291871  c: 0.131401
iter: 14  cost: 2.1135    m: 0.291871  c: 0.131401
iter: 15  cost: 2.1135    m: 0.291871  c: 0.131401
...

3. 重投影误差，BA优化相机位姿

《十四讲》样例代码使用BA优化相机位姿的李代数，个人添加了详细注解：
代码来源：https://github.com/gaoxiang12/slambook2/tree/master/ch7

#include 
#include 
#include "math.h"

#include 
#include 

#include 
#include 

#include 

using namespace Eigen;
using namespace std;
using namespace cv;

// 手写高斯-牛顿迭代法求解PnP问题
// 代码来自：https://github.com/gaoxiang12/slambook2/tree/master/ch7

// 在把Eigen中固定大小的类放入STL容器时需要使用Eigen自己定义的内存分配器Eigen::aligned_allocator
// Eigen管理内存和C++11中的方法不一样，所以需要单独强调元素的内存分配和管理，默认方式会在运行时报错
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

// 使用非线性优化方法计算相机位姿，进行BA优化
//
// 输入参数列表：世界坐标系3维点构成的vec，相机投影坐标系2维点构成的vec，相机内参矩阵K，原本的位姿pose
// 相机内参矩阵形式：
// [ fx  0   cx ]
// [ 0   fy  cy ]
// [ 0   0   1  ]
//
// 返回参数列表：完成优化之后更新的相机位姿pose(变换矩阵T的李代数se3)
// 李代数由6维向量【旋转向量+平移向量】经sophus库转换得到
void bundleAdjustmentGaussNewton(
    const VecVector3d &points_3d, const VecVector2d &points_2d, const Mat &K, Sophus::SE3d &pose)
{
    // 定义6维向量用于存储Hx=b中的x
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    // 设置迭代次数
    const int iterations = 10;
    // 当前cost与上一次的cost
    double cost = 0, lastCost = 0;
    // 相机的内参数
    double fx = K.at<double>(0, 0);
    double fy = K.at<double>(1, 1);
    double cx = K.at<double>(0, 2);
    double cy = K.at<double>(1, 2);

    // 开始迭代
    for (int iter = 0; iter < iterations; iter++)
    {
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
        // 用于存储Hx=b中的x
        Vector6d b = Vector6d::Zero();
        // 存储cost
        cost = 0;
        // 开始计算
        for (int i = 0; i < points_3d.size(); i++)
        {
            // 根据原本估计的位姿pose和3维点计算相机3维点的位置
            Eigen::Vector3d pc = pose * points_3d[i];
            // 计算出1/Z
            double inv_z = 1.0 / pc[2];
            // 计算（1/Z)^2
            double inv_z2 = inv_z * inv_z;
            // 根据3d点与位姿计算出的2维投影点坐标
            Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
            // 计算重投影误差，将计算出的2维点坐标与列表中检测到的2维点坐标相减
            Eigen::Vector2d e = points_2d[i] - proj;
            // 累加本轮迭代的cost
            cost += e.squaredNorm();
            // 根据《十四讲》187页推导出的2x6的用于优化位姿的雅可比矩阵
            Eigen::Matrix<double, 2, 6> J;
            J << -fx * inv_z, 0, fx * pc[0] * inv_z2, fx * pc[0] * pc[1] * inv_z2,
                -fx - fx * pc[0] * pc[0] * inv_z2, fx * pc[1] * inv_z, 0, -fy * inv_z,
                fy * pc[1] * inv_z2, fy + fy * pc[1] * pc[1] * inv_z2, -fy * pc[0] * pc[1] * inv_z2,
                -fy * pc[0] * inv_z;
            // 将每一对点计算出的结果更新到H，b中
            H += J.transpose() * J;
            b += -J.transpose() * e;
        }
        // 解Hx=b
        Vector6d dx;
        dx = H.ldlt().solve(b);
        // 如果解Hx=b失败
        if (isnan(dx[0]))
        {
            cout << "result is nan!" << endl;
            break;
        }
        // 如果cost在这一轮反而上升了，打印输出cost的上升
        if (iter > 0 && cost >= lastCost)
        {
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新位姿
        // 将计算出的旋转向量与平移向量构成的位姿转换为李代数se3
        // 用乘的方式更新李代数
        pose = Sophus::SE3d::exp(dx) * pose;

        // 记录上一次cost
        lastCost = cost;

        cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;

        // 如果误差已经小于1e-6，视为精度已经达到极限，提前结束迭代
        if (dx.norm() < 1e-6)
        {
            break;
        }
    }

    cout << "pose by g-n: \n" << pose.matrix() << endl;
}

// 主函数
int main(int argc, char **argv) { return 0; }