版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/luo870604851/article/details/85095131
３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误差函数：
代码：
#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;
}