手写高斯牛顿法实现３d-2d位姿估计

最新推荐文章于 2023-07-19 17:04:11 发布

luo870604851

最新推荐文章于 2023-07-19 17:04:11 发布

阅读量541

点赞数

分类专栏：高斯牛顿法文章标签：ＳＬＡＭＢＡ高斯牛顿法位姿

本文链接：https://blog.csdn.net/luo870604851/article/details/85095131

版权

高斯牛顿法专栏收录该内容

1 篇文章 0 订阅

订阅专栏

３d路标坐标：p3d.txt

-0.0374123 -0.830816 2.7448
-0.243698 -0.117719 1.5848
-0.627753 0.160186 1.3396
-0.323443 0.104873 1.4266
-0.627221 0.101454 1.3116
0.402045 -0.341821 2.2068
-0.687785 0.0430873 1.2976
0.627166 -0.0166317 1.5202
0.65817 -0.00198633 1.4784
-0.0897164 -0.073607 1.5526
-0.522843 -0.214436 1.4956
0.139792 -0.0954137 1.5202
0.594266 -0.0256024 1.5332
-0.0891202 -0.0718189 1.5526
0.427399 -0.333001 2.1552
0.399677 -0.344018 2.1938
-0.627209 0.10885 1.3158
-0.244307 -0.120761 1.5848
-0.637861 0.0641397 1.2706
0.661009 -0.000283745 1.4784
0.032143 -0.816064 2.7448
-0.0374123 -0.822386 2.7448
0.601751 -0.0249094 1.5268
0.136873 -0.0948301 1.5202
-0.329006 0.106137 1.4438
0.593677 -0.0250138 1.5332
-0.24332 -0.123909 1.5784
0.136061 -0.0987584 1.5268
0.515821 0.0343076 1.4438
0.42442 -0.335979 2.1552
0.401698 -0.341997 2.1938
0.590593 -0.00582347 1.502
-0.0891202 -0.0746797 1.5526
-0.331572 0.106534 1.4492
0.624455 -0.00247196 1.4838
0.136061 -0.0987584 1.5268
-0.242554 -0.118279 1.5848
0.587601 -0.00248262 1.4902
-0.0467705 -0.851215 2.7448
0.423345 -0.33438 2.168
0.403968 -0.3497 2.2196
-0.0283474 -0.81778 2.7244
-0.313646 0.0344556 1.5988
-0.637121 0.0619232 1.2878
0.607097 -0.0227993 1.5268
-0.649687 0.0695246 1.2706
-0.336318 0.104556 1.4664
-0.611718 -0.0528933 1.7914
0.416153 -0.340132 2.168
0.135048 -0.0997712 1.5268
0.0351785 -0.823904 2.7448
-0.238348 -0.122484 1.5848
-0.327565 0.101407 1.4492
-0.474742 -0.0386718 1.5138
-0.616088 -0.05458 1.8
-0.23961 -0.123745 1.5848
0.133832 -0.104632 1.5268
-0.593113 -0.0721167 1.8086
-0.0864075 -0.0767394 1.5526
-0.331102 0.101785 1.4546
0.147499 0.242462 1.163
0.129166 0.232126 1.1782
0.132374 -0.101716 1.5268
-0.235068 -0.123745 1.5848
-0.397808 -0.253889 2.0486
-0.0793163 -0.0681207 1.559
0.16172 0.233819 1.1598
0.139069 0.235996 1.1706
-0.236885 -0.127379 1.5848
0.128316 -0.111731 1.5202
-0.0825507 -0.0714003 1.5526
-0.476188 -0.0383825 1.5138
0.144134 0.188769 1.2878
-0.109385 -0.0301998 1.6344
-0.326273 0.102792 1.4492
0.0766801 -0.0599636 1.5332

２d特征点坐标

323 109
231 219
65 308
185 292
61 287
414 188
37 264
510 266
528 272
279 236
135 182
350 232
500 263
278.4 236.4
422 188
414 188
62.4 289.2
231.6 219.6
50.4 273.6
528 272.4
337.2 114
324 111.6
502.8 262.8
350.4 231.6
185 292
499.2 262.8
231.552 219.456
349.92 230.4
478.08 282.24
420.48 187.2
413.28 187.2
501.12 267.84
277.92 236.16
184.32 290.88
514.944 271.296
349.056 229.824
231.552 219.456
501.12 269.568
321.408 105.408
419.904 188.352
413.28 187.2
324.864 110.592
205.632 267.84
55.296 273.024
502.848 262.656
46.08 273.6
184.32 290.88
136.858 236.39
418.867 186.624
350.438 230.17
337.997 111.974
232.243 217.728
184.896 290.304
151.373 240.538
136.858 236.39
232.243 217.728
350.438 230.17
144.323 233.902
281.18 236.39
186.624 290.304
349.36 367.276
343.388 364.29
346.374 226.935
232.907 217.977
217.977 194.089
280.683 235.893
354.735 365.485
343.388 364.29
232.907 214.991
347.569 225.74
283.071 236.49
150.494 243.656
347.569 336.819
272.322 250.823
186.325 290.238
333.236 240.073

误差函数：

　　　　　　　　 $e=\sum_{i=1}^{n}(y_i-\frac{1}{s_i}Kexp(\xi^{\circ})P_i)$

代码：

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>

#include "sophus/se3.h"

using namespace std;

typedef vector<Vector3d, Eigen::aligned_allocator<Vector3d>> VecVector3d;
typedef vector<Vector2d, Eigen::aligned_allocator<Vector3d>> VecVector2d;
typedef Matrix<double, 6, 1> Vector6d;

string p3d_file = "../p3d.txt";
string p2d_file = "../p2d.txt";

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

    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d K;
    double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    // load points in to p3d and p2d 
    // START YOUR CODE HERE
    fstream i3dFile(p3d_file);
    fstream i2dFile(p2d_file);
    string line;
    while(getline(i3dFile,line))
    {
        stringstream record(line);
        Vector3d vEle;
        for(auto i=0;i<3;i++)
            record>>vEle[i];
        p3d.push_back(vEle);
    }
    while(getline(i2dFile,line))
    {
        stringstream record(line);
        Vector2d vEle;
        for(auto i=0;i<2;i++)
            record>>vEle[i];
        p2d.push_back(vEle);
    }
    // END YOUR CODE HERE
    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    Sophus::SE3 T_esti; // estimated pose
    for (int iter = 0; iter < iterations; iter++) {
        //为什么是６维，因为ｅ是2×１，Ｔ是６×１，从而Ｊ是2×６，Ｈ＝Ｊ转置乘Ｊ，为６×６
        Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();//b=J转置×f(x)，为６×2×2×１＝６＊１

        cost = 0;
        // compute cost
        for (int i = 0; i < nPoints; i++) {
            // compute cost for p3d[I] and p2d[I]
            // START YOUR CODE HERE
            Vector3d pCam=T_esti.rotation_matrix()*p3d[i]+T_esti.translation();
            Vector3d pImg=K*pCam;
            Vector2d pCamNormal(pImg[0]/pImg[2],pImg[1]/pImg[2]);
            Vector2d error=p2d[i]-pCamNormal;
            cost+=error.squaredNorm()/2;
	    // END YOUR CODE HERE
	    // compute jacobian
            Matrix<double, 2, 6> J;
            // START YOUR CODE HERE
            double zCam2=pCam[2]*pCam[2];
            J<<-fx/pCam[2],0,fx*pCam[0]/zCam2,fx*pCam[0]*pCam[1]/zCam2,-(fx+fx*pCam[0]*pCam[0]/zCam2),
         fx*pCam[1]/pCam[2], 0,-fy/pCam[2],fy*pCam[1]/zCam2,(fy+fy*pCam[1]*pCam[1]/zCam2),
                 -fy*pCam[0]*pCam[1]/zCam2,-fy*pCam[0]/pCam[2];
            H += J.transpose() * J;
            b += -J.transpose() * error;
        }

	// solve dx 
        Vector6d dx;

        // START YOUR CODE HERE 
          dx=H.ldlt().solve(b);
        // END YOUR CODE HERE

//        if (isnan(dx[0])) {
//            cout << "result is nan!" << endl;
//            break;
//        }

        if (iter > 0 && cost >=lastCost) {
            // cost increase, update is not good
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // update your estimation
        // START YOUR CODE HERE 
        T_esti=Sophus::SE3::exp(dx)*T_esti;
        // END YOUR CODE HERE
        
        lastCost = cost;

        cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    }

    cout << "estimated pose: \n" << T_esti.matrix() << endl;
    return 0;
}

luo870604851

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
手写高斯牛顿法实现３d-2d位姿估计

３d路标坐标：p3d.txt-0.0374123 -0.830816 2.7448-0.243698 -0.117719 1.5848-0.627753 0.160186 1.3396-0.323443 0.104873 1.4266-0.627221 0.101454 1.31160.402045 -0.341821 2.2068-0.687785 0.0430873 1.29...
复制链接

扫一扫

专栏目录