手写高斯牛顿法求解重投影误差优化问题

企业开发 2023-07-01 05:24:17 阅读次数: 0

本文使用 李代数扰动模型 中的推导，给出采用高斯牛顿法求解简单的重投影误差优化问题的代码。
手写高斯牛顿法，不使用优化库。

目录

一、 s e ( 3 ) \mathfrak{se}(3) se(3) 左乘扰动模型

#include <iostream>
#include <eigen3/Eigen/Dense>
#include <vector>
#include <float.h>

using namespace Eigen;
using namespace std;

// 反对称
Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d &v){
    
    
    Eigen::Matrix3d ans;
    ans << 0.0, -v(2), v(1),
            v(2), 0.0, -v(0),
            -v(1), v(0), 0.0;
    return ans;
}

// so3指数映射
Eigen::Matrix3d exp_so3(const Eigen::Vector3d &so3){
    
    
    double norm = so3.norm();
    if(norm < DBL_EPSILON) return Matrix3d::Identity();

    Matrix<double, 3, 3> so3_wedge = skewSymmetric(so3);
    Matrix3d SO3 = Matrix3d::Identity() + sin(norm) / norm * so3_wedge + (1.0 - cos(norm)) / norm / norm * so3_wedge * so3_wedge;

    return SO3;
}

// se3指数映射
Eigen::Matrix4d exp_se3(const Eigen::Matrix<double, 6, 1> &se3){
    
    
    Matrix4d SE3;
    SE3 << exp_so3(se3.tail<3>()), se3.head<3>(), Matrix<double, 1, 3>::Zero(), 1.0;
    // SE3 << Matrix4d::Identity();
    return SE3;
}

int main(int argc, char **argv){
    
    

    Vector3d point1(0.0, 0.0, 1.0);     // 3个观测点
    Vector3d point2(1.0, 0.0, 1.0);
    Vector3d point3(0.0, 1.0, 1.0);
    vector<Vector3d> point{
    
    point1, point2, point3};

    Vector3d Point1 = point1 * 10.0;     // 对应的3个空间点
    Vector3d Point2 = point2 * 20.0;
    Vector3d Point3 = point3 * 30.0;
    vector<Vector3d> Point{
    
    Point1, Point2, Point3};

    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();

    T.block<3,3>(0, 0) = Eigen::AngleAxisd(M_PI_4, Vector3d(1.0, 1.0, 1.0).normalized()).toRotationMatrix();
    T.block<3,1>(0, 3) = Vector3d(1.0, 2.0, 3.0);

    cout << "initial T:"  << endl;
    cout << T << endl << endl;

    double pre_dx = 0.0;
    double pre_cost = 0.0;
    for(int iters = 0; iters < 50; iters++){
    
        // 开始迭代
        Eigen::Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();   // 海森矩阵 JTJ
        Eigen::Matrix<double, 6, 1> b = Matrix<double, 6, 1>::Zero();   // -JTe
        double cost = 0;
        for(int i = 0; i < 3; i++){
    
         // 3个残差项
            Eigen::Matrix<double, 2, 6> J;  // 雅克比矩阵
            Eigen::Vector2d err;

            Vector3d Pc = T.block<3, 3>(0, 0) * Point[i] + T.block<3,1>(0, 3);  // 空间点转移到相机坐标系下
            Vector2d Pc_norm = Pc.head<2>() / Pc(2);                            // 归一化平面坐标

            err = Pc_norm - point[i].head<2>();                                 // 误差项： 投影点 - 观测点 

            Matrix<double, 2, 3> J_err_Pc;  // 误差相对于相机坐标系下空间点的坐标的雅克比矩阵
            J_err_Pc << 1.0 / Pc(2), 0.0, - Pc(0) / Pc(2) / Pc(2),
                        0.0, 1.0 / Pc(2), - Pc(1) / Pc(2) / Pc(2);
            
            Matrix<double, 3, 6> J_Pc_T;    // 相机坐标系下空间点的坐标相对于世界坐标系下空间点坐标的雅克比矩阵
            J_Pc_T << Eigen::Matrix3d::Identity(), -skewSymmetric(Pc);

            J = J_err_Pc * J_Pc_T;          // 最终的雅克比矩阵

            H += J.transpose() * J;

            b -= J.transpose() * err;

            cost += err.norm();
        }

        Matrix<double, 6, 1> dx = H.ldlt().solve(b);
        T = exp_se3(dx) * T;
        cout << "iter: " << iters << ", cost: " << cost <<endl;
        cout << T << endl << endl;

        if(dx.norm() < 0.1 * pre_dx || cost < 0.1 * pre_cost){
    
          // 简单设置两个终止条件
            break;
        }

        pre_dx = dx.norm();
        pre_cost = cost;
    }

    return 0;
}

迭代过程：

initial T:
 0.804738 -0.310617  0.505879         1
 0.505879  0.804738 -0.310617         2
-0.310617  0.505879  0.804738         3
        0         0         0         1

iter: 0 cost: 2.38448
 0.965242 -0.121832  0.231226  0.896445
0.0648343   0.96867   0.23974  -2.24463
 -0.25319 -0.216416  0.942899   3.30829
        0         0         0         1

iter: 1 cost: 1.14855
   0.999718  0.00400888   0.0234193    -1.34271
-0.00409842    0.999984   0.0037767   0.0366941
 -0.0234037 -0.00387162    0.999719    -1.58768
          0           0           0           1

iter: 2 cost: 0.266329
    0.999972  0.000583823   0.00746819    -0.271978
-0.000584302            1  6.19286e-05    0.0124495
 -0.00746816 -6.62906e-05     0.999972   -0.0930468
           0            0            0            1

iter: 3 cost: 0.0298453
           1  6.83949e-06  7.65969e-05  -0.00260385
 -6.8396e-06            1   1.3285e-06  8.77872e-05
-7.65969e-05 -1.32902e-06            1 -0.000997241
           0            0            0            1

iter: 4 cost: 0.00029594
           1  7.12615e-10  7.84924e-09 -2.61727e-07
-7.12615e-10            1 -2.30105e-10  9.76211e-09
-7.84924e-09  2.30105e-10            1 -1.31601e-07
           0            0            0            1

二、 s e ( 3 ) \mathfrak{se}(3) se(3) 右乘扰动模型

/** 此处省略头文件和几个函数，请参考第一节进行补充 **/

int main(int argc, char **argv){
    
    

    /** 此处省略关键点和空间点的声明定义，请参考第一节进行补充 **/

    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();

    T.block<3,3>(0, 0) = Eigen::AngleAxisd(M_PI_4, Vector3d(1.0, 1.0, 1.0).normalized()).toRotationMatrix();
    T.block<3,1>(0, 3) = Vector3d(1.0, 2.0, 3.0);

    cout << "initial T:"  << endl;
    cout << T << endl << endl;

    double pre_dx = 0.0;
    double pre_cost = 0.0;
    for(int iters = 0; iters < 50; iters++){
    
        // 开始迭代
        Eigen::Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();   // 海森矩阵 JTJ
        Eigen::Matrix<double, 6, 1> b = Matrix<double, 6, 1>::Zero();   // -JTe
        double cost = 0;
        for(int i = 0; i < 3; i++){
    
         // 3个残差项
            Eigen::Matrix<double, 2, 6> J;  // 雅克比矩阵
            Eigen::Vector2d err;

            Vector3d Pc = T.block<3, 3>(0, 0) * Point[i] + T.block<3,1>(0, 3);  // 空间点转移到相机坐标系下
            Vector2d Pc_norm = Pc.head<2>() / Pc(2);                            // 归一化平面坐标

            err = Pc_norm - point[i].head<2>();                                 // 误差项： 投影点 - 观测点 

            Matrix<double, 2, 3> J_err_Pc;  // 误差相对于相机坐标系下空间点的坐标的雅克比矩阵
            J_err_Pc << 1.0 / Pc(2), 0.0, - Pc(0) / Pc(2) / Pc(2),
                        0.0, 1.0 / Pc(2), - Pc(1) / Pc(2) / Pc(2);
            
            Matrix<double, 3, 6> J_Pc_T;    // 相机坐标系下空间点的坐标相对于世界坐标系下空间点坐标的雅克比矩阵
            J_Pc_T << T.block<3, 3>(0, 0), -T.block<3, 3>(0, 0) * skewSymmetric(Point[i]);

            J = J_err_Pc * J_Pc_T;          // 最终的雅克比矩阵

            H += J.transpose() * J;

            b -= J.transpose() * err;

            cost += err.norm();
        }

        Matrix<double, 6, 1> dx = H.ldlt().solve(b);
        T = T * exp_se3(dx);
        cout << "iter: " << iters << ", cost: " << cost <<endl;
        cout << T << endl << endl;

        if(dx.norm() < 0.1 * pre_dx || cost < 0.1 * pre_cost){
    
          // 简单设置两个终止条件
            break;
        }

        pre_dx = dx.norm();
        pre_cost = cost;
    }

    return 0;
}

迭代过程：

initial T:
 0.804738 -0.310617  0.505879         1
 0.505879  0.804738 -0.310617         2
-0.310617  0.505879  0.804738         3
        0         0         0         1

iter: 0, cost: 2.38448
 0.965242 -0.121832  0.231226  0.488738
0.0648343   0.96867   0.23974  -1.72712
 -0.25319 -0.216416  0.942899   3.85985
        0         0         0         1

iter: 1, cost: 1.04843
     0.99981 -0.000563353    0.0195013     -1.22537
 0.000396223     0.999963   0.00857301      -0.2689
  -0.0195054  -0.00856365     0.999773      -1.1643
           0            0            0            1

iter: 2, cost: 0.212416
   0.999986 -0.00016772  0.00522819   -0.184814
0.000161249    0.999999  0.00123801  -0.0359215
-0.00522839 -0.00123715    0.999986  -0.0317462
          0           0           0           1

iter: 3, cost: 0.0189555
           1  2.68088e-06  8.09217e-06 -0.000500737
-2.68089e-06            1   1.3193e-06 -8.32469e-05
-8.09216e-06 -1.31933e-06            1 -0.000574443
           0            0            0            1

三、3 维平移量与 s o ( 3 ) \mathfrak{so}(3) so(3) 左乘扰动模型

/** 此处省略头文件和几个函数，请参考第一节进行补充 **/

int main(int argc, char **argv){
    
    

    /** 此处省略关键点和空间点的声明定义，请参考第一节进行补充 **/

    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI_4, Vector3d(1.0, 1.0, 1.0).normalized()).toRotationMatrix();
    Eigen::Vector3d t = Vector3d(1.0, 2.0, 3.0);

    cout << "initial R and t:"  << endl;
    cout << R << "\t" << t.transpose() << endl << endl;

    double pre_dx = 0.0;
    double pre_cost = 0.0;
    for(int iters = 0; iters < 50; iters++){
    
        // 开始迭代
        Eigen::Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();   // 海森矩阵 JTJ
        Eigen::Matrix<double, 6, 1> b = Matrix<double, 6, 1>::Zero();   // -JTe
        double cost = 0;
        for(int i = 0; i < 3; i++){
    
         // 3个残差项
            Eigen::Matrix<double, 2, 6> J;  // 雅克比矩阵
            Eigen::Vector2d err;

            Vector3d Pc = R * Point[i] + t;  // 空间点转移到相机坐标系下
            Vector2d Pc_norm = Pc.head<2>() / Pc(2);                            // 归一化平面坐标

            err = Pc_norm - point[i].head<2>();                                 // 误差项： 投影点 - 观测点 

            Matrix<double, 2, 3> J_err_Pc;  // 误差相对于相机坐标系下空间点的坐标的雅克比矩阵
            J_err_Pc << 1.0 / Pc(2), 0.0, - Pc(0) / Pc(2) / Pc(2),
                        0.0, 1.0 / Pc(2), - Pc(1) / Pc(2) / Pc(2);
            
            Matrix<double, 3, 6> J_Pc_T;    // 相机坐标系下空间点的坐标相对于世界坐标系下空间点坐标的雅克比矩阵
            J_Pc_T << Eigen::Matrix3d::Identity(), -skewSymmetric(R * Point[i]);

            J = J_err_Pc * J_Pc_T;          // 最终的雅克比矩阵

            H += J.transpose() * J;

            b -= J.transpose() * err;

            cost += err.norm();
        }

        Matrix<double, 6, 1> dx = H.ldlt().solve(b);
        t = t + dx.head<3>();
        R = exp_so3(dx.tail<3>()) * R;
        cout << "iter: " << iters << ", cost: " << cost <<endl;
        cout << R << "\t" << t.transpose() << endl << endl;

        if(dx.norm() < 0.1 * pre_dx || cost < 0.1 * pre_cost){
    
          // 简单设置两个终止条件
            break;
        }

        pre_dx = dx.norm();
        pre_cost = cost;
    }

    return 0;
}

迭代过程：

initial R and t:
 0.804738 -0.310617  0.505879
 0.505879  0.804738 -0.310617
-0.310617  0.505879  0.804738	1 2 3

iter: 0, cost: 2.38448
 0.965242 -0.121832  0.231226
0.0648343   0.96867   0.23974
 -0.25319 -0.216416  0.942899	0.488738 -1.72712  3.85985

iter: 1, cost: 1.04843
     0.99981 -0.000563353    0.0195013
 0.000396223     0.999963   0.00857301
  -0.0195054  -0.00856365     0.999773	-1.22537  -0.2689  -1.1643

iter: 2, cost: 0.212416
   0.999986 -0.00016772  0.00522819
0.000161249    0.999999  0.00123801
-0.00522839 -0.00123715    0.999986	 -0.184814 -0.0359215 -0.0317462

iter: 3, cost: 0.0189555
           1  2.68088e-06  8.09217e-06
-2.68089e-06            1   1.3193e-06
-8.09216e-06 -1.31933e-06            1	-0.000500737 -8.32469e-05 -0.000574443

四、 s o ( 3 ) \mathfrak{s o}(3) so(3) 右乘扰动模型

/** 此处省略头文件和几个函数，请参考第一节进行补充 **/

int main(int argc, char **argv){
    
    

    /** 此处省略关键点和空间点的声明定义，请参考第一节进行补充 **/

    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI_4, Vector3d(1.0, 1.0, 1.0).normalized()).toRotationMatrix();
    Eigen::Vector3d t = Vector3d(1.0, 2.0, 3.0);

    cout << "initial R and t:"  << endl;
    cout << R << "\t" << t.transpose() << endl << endl;

    double pre_dx = 0.0;
    double pre_cost = 0.0;
    for(int iters = 0; iters < 50; iters++){
    
        // 开始迭代
        Eigen::Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();   // 海森矩阵 JTJ
        Eigen::Matrix<double, 6, 1> b = Matrix<double, 6, 1>::Zero();   // -JTe
        double cost = 0;
        for(int i = 0; i < 3; i++){
    
         // 3个残差项
            Eigen::Matrix<double, 2, 6> J;  // 雅克比矩阵
            Eigen::Vector2d err;

            Vector3d Pc = R * Point[i] + t;  // 空间点转移到相机坐标系下
            Vector2d Pc_norm = Pc.head<2>() / Pc(2);                            // 归一化平面坐标

            err = Pc_norm - point[i].head<2>();                                 // 误差项： 投影点 - 观测点 

            Matrix<double, 2, 3> J_err_Pc;  // 误差相对于相机坐标系下空间点的坐标的雅克比矩阵
            J_err_Pc << 1.0 / Pc(2), 0.0, - Pc(0) / Pc(2) / Pc(2),
                        0.0, 1.0 / Pc(2), - Pc(1) / Pc(2) / Pc(2);
            
            Matrix<double, 3, 6> J_Pc_T;    // 相机坐标系下空间点的坐标相对于世界坐标系下空间点坐标的雅克比矩阵
            J_Pc_T << Eigen::Matrix3d::Identity(), -R * skewSymmetric(Point[i]);

            J = J_err_Pc * J_Pc_T;          // 最终的雅克比矩阵

            H += J.transpose() * J;

            b -= J.transpose() * err;

            cost += err.norm();
        }

        Matrix<double, 6, 1> dx = H.ldlt().solve(b);
        t = t + dx.head<3>();
        R = R * exp_so3(dx.tail<3>());
        cout << "iter: " << iters << ", cost: " << cost <<endl;
        cout << R << "\t" << t.transpose() << endl << endl;

        if(dx.norm() < 0.1 * pre_dx || cost < 0.1 * pre_cost){
    
          // 简单设置两个终止条件
            break;
        }

        pre_dx = dx.norm();
        pre_cost = cost;
    }

    return 0;
}

迭代过程：

initial R and t:
 0.804738 -0.310617  0.505879
 0.505879  0.804738 -0.310617
-0.310617  0.505879  0.804738	1 2 3

iter: 0, cost: 2.38448
 0.965242 -0.121832  0.231226
0.0648343   0.96867   0.23974
 -0.25319 -0.216416  0.942899	0.488738 -1.72712  3.85985

iter: 1, cost: 1.04843
     0.99981 -0.000563353    0.0195013
 0.000396223     0.999963   0.00857301
  -0.0195054  -0.00856365     0.999773	-1.22537  -0.2689  -1.1643

iter: 2, cost: 0.212416
   0.999986 -0.00016772  0.00522819
0.000161249    0.999999  0.00123801
-0.00522839 -0.00123715    0.999986	 -0.184814 -0.0359215 -0.0317462

iter: 3, cost: 0.0189555
           1  2.68088e-06  8.09217e-06
-2.68089e-06            1   1.3193e-06
-8.09216e-06 -1.31933e-06            1	-0.000500737 -8.32469e-05 -0.000574443

结果分析

通过观察迭代过程发现，使用 s o ( 3 ) \mathfrak{s o}(3) so(3) 左乘扰动模型 和 s o ( 3 ) \mathfrak{s o}(3) so(3) 右乘扰动模型 的迭代结果是一模一样的。
而 使用 s e ( 3 ) \mathfrak{s e}(3) se(3) 左乘扰动模型 和 s e ( 3 ) \mathfrak{s e}(3) se(3) 右乘扰动模型 的迭代结果就不一样。在该例问题中，使用 s e ( 3 ) \mathfrak{s e}(3) se(3) 右乘扰动模型下降速度会快一些。

猜你喜欢

转载自blog.csdn.net/weixin_43196818/article/details/130399452
今日推荐
《美国对全球网络空间安全与发展的威胁和破坏》报告发布
火速冲上 GitHub 热榜 —— 开源编程语言、框架哪有这么可爱？
北京人形机器人创新中心发布全球首个纯电驱拟人奔跑的全尺寸人形机器人“天工”
LFOSSA 源来如此公开课 | 掌握云原生未来：CNCF 认证全面攻略与备考秘籍
国产云输入法——仅华为无云端数据上传安全问题
周排行
Python环境安装与基础语法（1）——计算机基础知识
IMU预积分
ADAS中的LDW、FCW、BSD、LCA、ACC、AEB、APA、DMS代表的含义
B站笔试两道题
skyeye arm 硬件虚拟机环境的搭建
Web前端静态页面示例
数组-合并排序数组 II-简单
springcloud之版本问题启动报错
面向对象-------------匿名对象(六)
输入URL到页面呈现中间发生了什么？
每日归档
更多
2024-04-30(1)
2024-04-29(40)
2024-04-28(0)
2024-04-27(56)
2024-04-26(39)
2024-04-25(22)
2024-04-24(36)
2024-04-23(26)
2024-04-22(39)
2024-04-21(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022