V-LOAM源码解析（五）
转载请注明出处：本文转自zhch_pan的博客http://www.cnblogs.com/zhchp-blog/
本博客为本人之前做项目时做的源码阅读工作，po到网上希望帮助其他人更好的理解V-LOAM的工程实现，有些地方代码做了修改，可能和原工程有出入，但对于该工程的整体流程理解没有妨碍。
源码下载链接：https://github.com/Jinqiang/demo_lidar
节点：bundleAdjust
功能：对视觉里程计获得的odom信息进行局部BA优化，获得精度更高的odom信息
1 #include <math.h> 2 #include <stdio.h> 3 #include <stdlib.h> 4 #include <ros/ros.h> 5 6 #include <nav_msgs/Odometry.h> 7 8 #include <tf/transform_datatypes.h> 9 #include <tf/transform_listener.h> 10 #include <tf/transform_broadcaster.h> 11 12 #include "isam/isam.h" 13 #include "isam/slam_depthmono.h" 14 #include "isam/robust.h" 15 16 #include "cameraParameters.h" 17 #include "pointDefinition.h" 18 19 using namespace std; 20 using namespace isam; 21 using namespace Eigen; 22 23 const double PI = 3.1415926; 24 25 const int keyframeNum = 5; 26 pcl::PointCloud<DepthPoint>::Ptr depthPoints[keyframeNum]; 27 pcl::PointCloud<DepthPoint>::Ptr depthPointsStacked(new pcl::PointCloud<DepthPoint>()); 28 29 double depthPointsTime; 30 bool newKeyframe = false; 31 32 double rollRec, pitchRec, yawRec; 33 double txRec, tyRec, tzRec; 34 35 double rollTest, pitchTest, yawTest; 36 double txTest, tyTest, tzTest; 37 38 double transformBefBA[6] = {0}; 39 double transformAftBA[6] = {0}; 40 41 void diffRotation(double cx, double cy, double cz, double lx, double ly, double lz, 42 double &ox, double &oy, double &oz) 43 { 44 /* 45 *计算相邻两帧的旋转角 46 *R_wc=[ccy 0 scy;0 1 0;-scy 0 ccy]*[1 0 0;0 ccx -scx;0 scx ccx]*[ccz -scz 0;scz ccz 0;0 0 1]; 47 *R_wl=[cly 0 sly;0 1 0;-sly 0 cly]*[1 0 0;0 clx -slx;0 slx clx]*[clz -slz 0;slz clz 0;0 0 1]; 48 *R_lc=(R_wl).'*R_wc; 49 */ 50 double srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 51 - cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx); 52 ox = -asin(srx); 53 54 double srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) 55 - cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz); 56 double crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly); 57 oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); 58 59 double srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy) 60 - cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 61 - (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)); 62 double crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz) 63 + cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz) 64 - cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz); 65 oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); 66 } 67 68 /* 69 *该函数将新的特征点序列从VO线转换到BA线，其坐标转换关系参考sketch文件夹下的“3.png” 70 *R_w_bc=[cbcy 0 sbcy;0 1 0;-sbcy 0 cbcy]*[1 0 0;0 cbcx -sbcx;0 sbcx cbcx]*[cbcz -sbcz 0;sbcz cbcz 0;0 0 1]; 71 *R_w_bl=[cbly 0 sbly;0 1 0;-sbly 0 cbly]*[1 0 0;0 cblx -sblx;0 sblx cblx]*[cblz -sblz 0;sblz cblz 0;0 0 1]; 72 *R_w_al=[caly 0 saly;0 1 0;-saly 0 caly]*[1 0 0;0 calx -salx;0 salx calx]*[calz -salz 0;salz calz 0;0 0 1]; 73 *R_bl_al= (R_w_bl).' * R_w_al; 74 *R_w_tb= R_w_bc * R_bl_al; 75 *R_w_tb就是将新特征序列首帧变到BA线后的旋转矩阵 76 */ 77 void transformAssociateToBA() 78 { 79 double txDiff = txRec - transformBefBA[3]; 80 double tyDiff = tyRec - transformBefBA[4]; 81 double tzDiff = tzRec - transformBefBA[5]; 82 83 //注意这里是将参考坐标系旋转到目标坐标系，所以roll,pitch,yaw都应该加一个负号 84 double x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff; 85 double y1 = tyDiff; 86 double z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff; 87 88 double x2 = x1; 89 double y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1; 90 double z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1; 91 92 txDiff = cos(rollRec) * x2 + sin(rollRec) * y2; 93 tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2; 94 tzDiff = z2; 95 96 double sbcx = sin(pitchRec); 97 double cbcx = cos(pitchRec); 98 double sbcy = sin(yawRec); 99 double cbcy = cos(yawRec); 100 double sbcz = sin(rollRec); 101 double cbcz = cos(rollRec); 102 103 double sblx = sin(transformBefBA[0]); 104 double cblx = cos(transformBefBA[0]); 105 double sbly = sin(transformBefBA[1]); 106 double cbly = cos(transformBefBA[1]); 107 double sblz = sin(transformBefBA[2]); 108 double cblz = cos(transformBefBA[2]); 109 110 double salx = sin(transformAftBA[0]); 111 double calx = cos(transformAftBA[0]); 112 double saly = sin(transformAftBA[1]); 113 double caly = cos(transformAftBA[1]); 114 double salz = sin(transformAftBA[2]); 115 double calz = cos(transformAftBA[2]); 116 117 double srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 118 - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 119 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 120 - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 121 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); 122 pitchRec = -asin(srx); 123 124 double srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 125 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 126 - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 127 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 128 + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 129 double crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 130 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 131 - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 132 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 133 + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 134 yawRec = atan2(srycrx / cos(pitchRec), crycrx / cos(pitchRec)); 135 136 double srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 137 - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 138 - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 139 + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 140 - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 141 + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 142 + calx*cblx*salz*sblz); 143 double crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 144 - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 145 + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 146 + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 147 + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 148 - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 149 - calx*calz*cblx*sblz); 150 rollRec = atan2(srzcrx / cos(pitchRec), crzcrx / cos(pitchRec)); 151 152 x1 = cos(rollRec) * txDiff - sin(rollRec) * tyDiff; 153 y1 = sin(rollRec) * txDiff + cos(rollRec) * tyDiff; 154 z1 = tzDiff; 155 156 x2 = x1; 157 y2 = cos(pitchRec) * y1 - sin(pitchRec) * z1; 158 z2 = sin(pitchRec) * y1 + cos(pitchRec) * z1; 159 160 txDiff = cos(yawRec) * x2 + sin(yawRec) * z2; 161 tyDiff = y2; 162 tzDiff = -sin(yawRec) * x2 + cos(yawRec) * z2; 163 164 txRec = transformAftBA[3] + txDiff; 165 tyRec = transformAftBA[4] + tyDiff; 166 tzRec = transformAftBA[5] + tzDiff; 167 168 rollTest=rollRec; 169 pitchTest=pitchRec; 170 yawTest=yawRec; 171 txTest=txRec; 172 tyTest=tyRec; 173 tzTest=tzRec; 174 } 175 176 void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2) 177 { 178 depthPointsTime = depthPoints2->header.stamp.toSec(); 179 180 depthPointsStacked->clear(); 181 pcl::fromROSMsg(*depthPoints2, *depthPointsStacked); 182 int depthPointsStackedNum = depthPointsStacked->points.size(); 183 184 for (int i = 0; i < keyframeNum; i++) { 185 depthPoints[i]->clear(); 186 } 187 188 int keyframeCount = -1; 189 for (int i = 0; i < depthPointsStackedNum; i++) { 190 if (depthPointsStacked->points[i].ind == -2) { 191 keyframeCount++; 192 } 193 194 if (keyframeCount >= 0 && keyframeCount < keyframeNum) { 195 depthPoints[keyframeCount]->push_back(depthPointsStacked->points[i]); 196 } 197 } 198 199 bool enoughPoints = true; 200 for (int i = 0; i < keyframeNum; i++) { 201 if (depthPoints[i]->points.size() < 10) { 202 enoughPoints = false; 203 } 204 } 205 206 if (enoughPoints) { 207 newKeyframe = true; 208 } 209 } 210 211 int main(int argc, char** argv) 212 { 213 ros::init(argc, argv, "bundleAdjust"); 214 ros::NodeHandle nh; 215 216 for (int i = 0; i < keyframeNum; i++) { 217 pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp(new pcl::PointCloud<DepthPoint>()); 218 depthPoints[i] = depthPointsTemp; 219 } 220 221 ros::Subscriber depthPointsSub = nh.subscribe<sensor_msgs::PointCloud2> 222 ("/depth_points_stacked", 1, depthPointsHandler); 223 224 ros::Publisher odomBefBAPub = nh.advertise<nav_msgs::Odometry> ("/bef_ba_to_init", 1); 225 226 ros::Publisher odomAftBAPub = nh.advertise<nav_msgs::Odometry> ("/aft_ba_to_init", 1); 227 228 tf::TransformBroadcaster tfBroadcaster; 229 230 Vector2d pp(0, 0); 231 DepthmonoCamera camera(1, pp); 232 233 MatrixXd mpNoise = eye(3); 234 mpNoise(2, 2) = 0.01; 235 Noise noise0 = Information(mpNoise); 236 237 MatrixXd dpNoise = eye(3); 238 //dpNoise(2, 2) = 1.; 239 Noise noise1 = Information(dpNoise); 240 241 MatrixXd ssNoise = 10000. * eye(6); 242 //ssNoise(3, 3) = 100; 243 //ssNoise(4, 4) = 100; 244 //ssNoise(5, 5) = 100; 245 Noise noise2 = Information(ssNoise); 246 247 MatrixXd psNoise = 10000. * eye(6); 248 psNoise(3, 3) = 100; 249 psNoise(4, 4) = 100; 250 psNoise(5, 5) = 100; 251 Noise noise3 = Information(psNoise); 252 253 bool status = ros::ok(); 254 while (status) { 255 ros::spinOnce(); 256 257 if (newKeyframe) { 258 newKeyframe = false; 259 260 Slam ba; 261 262 vector<Pose3d_Node*> poses; 263 Pose3d_Node* pose0 = new Pose3d_Node(); 264 poses.push_back(pose0); 265 ba.add_node(pose0); 266 267 rollRec = depthPoints[0]->points[0].depth; 268 pitchRec = depthPoints[0]->points[0].u; 269 yawRec = depthPoints[0]->points[0].v; 270 txRec = depthPoints[0]->points[1].u; 271 tyRec = depthPoints[0]->points[1].v; 272 tzRec = depthPoints[0]->points[1].depth; 273 274 transformAssociateToBA(); 275 276 /* 277 *这里有个坐标系的对应关系，在Pose3d()函数中的x,y,z轴分别对应着实际坐标系的z,x,y轴， 278 *所以Pose3d()中的x,y,z值分别为tzRec，txRec，tyRec，而roll,pitch,yaw是根据实际机体 279 *的旋转关系定义的，所以yawRec, pitchRec, rollRec就对应Pose3d()中传入值的roll,pitch,yaw 280 */ 281 Pose3d_Factor* poseFactors0 = new Pose3d_Factor(pose0, 282 Pose3d(tzRec, txRec, tyRec, yawRec, pitchRec, rollRec), noise2); 283 ba.add_factor(poseFactors0); 284 285 rollRec = depthPoints[0]->points[0].depth; 286 pitchRec = depthPoints[0]->points[0].u; 287 yawRec = depthPoints[0]->points[0].v; 288 txRec = depthPoints[0]->points[1].u; 289 tyRec = depthPoints[0]->points[1].v; 290 tzRec = depthPoints[0]->points[1].depth; 291 292 vector<Pose3d_Pose3d_Factor*> posePoseFactors; 293 for (int i = 1; i < keyframeNum; i++) { 294 Pose3d_Node* posen = new Pose3d_Node(); 295 poses.push_back(posen); 296 ba.add_node(posen); 297 298 double roll = depthPoints[i]->points[0].depth; 299 double pitch = depthPoints[i]->points[0].u; 300 double yaw = depthPoints[i]->points[0].v; 301 double tx = depthPoints[i]->points[1].u; 302 double ty = depthPoints[i]->points[1].v; 303 double tz = depthPoints[i]->points[1].depth; 304 305 double txDiff = tx - txRec; 306 double tyDiff = ty - tyRec; 307 double tzDiff = tz - tzRec; 308 309 double x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff; 310 double y1 = tyDiff; 311 double z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff; 312 313 double x2 = x1; 314 double y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1; 315 double z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1; 316 317 txDiff = cos(rollRec) * x2 + sin(rollRec) * y2; 318 tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2; 319 tzDiff = z2; 320 321 //rollDiff, pitchDiff, yawDiff存储的是后一帧到前一帧的旋转角度，参考系是前一帧 322 double rollDiff, pitchDiff, yawDiff; 323 diffRotation(pitch, yaw, roll, pitchRec, yawRec, rollRec, pitchDiff, yawDiff, rollDiff); 324 325 Pose3d_Pose3d_Factor* poseposeFactorn = new Pose3d_Pose3d_Factor 326 (poses[i - 1], posen, Pose3d(tzDiff, txDiff, tyDiff, 327 yawDiff, pitchDiff, rollDiff), noise3); 328 posePoseFactors.push_back(poseposeFactorn); 329 ba.add_factor(poseposeFactorn); 330 331 rollRec = roll; pitchRec = pitch; yawRec = yaw; 332 txRec = tx; tyRec = ty; tzRec = tz; 333 } 334 335 vector<Point3d_Node*> points; 336 std::vector<int> pointsInd; 337 vector<Depthmono_Factor*> depthmonoFactors; 338 for (int i = 0; i < keyframeNum; i++) { 339 pcl::PointCloud<DepthPoint>::Ptr dpPointer = depthPoints[i]; 340 int kfptNum = dpPointer->points.size(); 341 int ptRecNum = points.size(); 342 343 if (i == 0) { 344 pcl::PointCloud<DepthPoint>::Ptr dpPointerNext = depthPoints[i + 1]; 345 int kfptNumNext = dpPointerNext->points.size(); 346 347 int ptCountNext = 2; 348 for (int j = 2; j < kfptNum; j++) { 349 bool ptFound = false; 350 for (; ptCountNext < kfptNumNext; ptCountNext++) { 351 if (dpPointerNext->points[ptCountNext].ind == dpPointer->points[j].ind) { 352 ptFound = true; 353 } 354 355 if (dpPointerNext->points[ptCountNext].ind >= dpPointer->points[j].ind) { 356 break; 357 } 358 } 359 360 if (ptFound && dpPointer->points[j].label == 1) { 361 Point3d_Node* pointn = new Point3d_Node(); 362 points.push_back(pointn); 363 pointsInd.push_back(dpPointer->points[j].ind); 364 ba.add_node(pointn); 365 366 Depthmono_Factor* depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera, 367 DepthmonoMeasurement(dpPointer->points[j].u, 368 dpPointer->points[j].v, dpPointer->points[j].depth), 369 noise1); 370 depthmonoFactors.push_back(depthmonoFactorn); 371 ba.add_factor(depthmonoFactorn); 372 } 373 } 374 } else if (i == keyframeNum - 1) { 375 pcl::PointCloud<DepthPoint>::Ptr dpPointerLast = depthPoints[i - 1]; 376 int kfptNumLast = dpPointerLast->points.size(); 377 378 int ptCountLast = 2; 379 int ptRecCount = 0; 380 for (int j = 2; j < kfptNum; j++) { 381 bool ptFound = false; 382 for (; ptCountLast < kfptNumLast; ptCountLast++) { 383 if (dpPointerLast->points[ptCountLast].ind == dpPointer->points[j].ind) { 384 ptFound = true; 385 } 386 if (dpPointerLast->points[ptCountLast].ind >= dpPointer->points[j].ind) { 387 break; 388 } 389 } 390 391 if (ptFound /*&& dpPointer->points[j].label == 1*/) { 392 bool prFound = false; 393 for (; ptRecCount < ptRecNum; ptRecCount++) { 394 if (pointsInd[ptRecCount] == dpPointer->points[j].ind) { 395 prFound = true; 396 } 397 if (pointsInd[ptRecCount] >= dpPointer->points[j].ind) { 398 break; 399 } 400 } 401 402 Point3d_Node* pointn; 403 Depthmono_Factor* depthmonoFactorn; 404 if (prFound) { 405 pointn = points[ptRecCount]; 406 407 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera, 408 DepthmonoMeasurement(dpPointer->points[j].u, 409 dpPointer->points[j].v, dpPointer->points[j].depth), noise0); 410 //continue; 411 } else { 412 pointn = new Point3d_Node(); 413 points.push_back(pointn); 414 pointsInd.push_back(dpPointer->points[j].ind); 415 ba.add_node(pointn); 416 417 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera, 418 DepthmonoMeasurement(dpPointer->points[j].u, 419 dpPointer->points[j].v, dpPointer->points[j].depth), noise1); 420 } 421 422 depthmonoFactors.push_back(depthmonoFactorn); 423 ba.add_factor(depthmonoFactorn); 424 } 425 } 426 } else { 427 pcl::PointCloud<DepthPoint>::Ptr dpPointerNext = depthPoints[i + 1]; 428 pcl::PointCloud<DepthPoint>::Ptr dpPointerLast = depthPoints[i - 1]; 429 int kfptNumNext = dpPointerNext->points.size(); 430 int kfptNumLast = dpPointerLast->points.size(); 431 432 int ptCountNext = 2; 433 int ptCountLast = 2; 434 int ptRecCount = 0; 435 for (int j = 2; j < kfptNum; j++) { 436 bool ptFound = false; 437 for (; ptCountNext < kfptNumNext; ptCountNext++) { 438 if (dpPointerNext->points[ptCountNext].ind == dpPointer->points[j].ind) { 439 ptFound = true; 440 } 441 if (dpPointerNext->points[ptCountNext].ind >= dpPointer->points[j].ind) { 442 break; 443 } 444 } 445 446 if (!ptFound) { 447 for (; ptCountLast < kfptNumLast; ptCountLast++) { 448 if (dpPointerLast->points[ptCountLast].ind == dpPointer->points[j].ind) { 449 ptFound = true; 450 } 451 if (dpPointerLast->points[ptCountLast].ind >= dpPointer->points[j].ind) { 452 break; 453 } 454 } 455 } 456 457 if (ptFound /*&& dpPointer->points[j].label == 1*/) { 458 bool prFound = false; 459 for (; ptRecCount < ptRecNum; ptRecCount++) { 460 if (pointsInd[ptRecCount] == dpPointer->points[j].ind) { 461 prFound = true; 462 //modified at 2018/01/10 463 //if(dpPointer->points[j].label == 2) 464 // std::cout<<"comme on baby!!"<<std::endl; 465 } 466 if (pointsInd[ptRecCount] >= dpPointer->points[j].ind) { 467 break; 468 } 469 } 470 471 Point3d_Node* pointn; 472 Depthmono_Factor* depthmonoFactorn; 473 if (prFound) { 474 pointn = points[ptRecCount]; 475 476 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera, 477 DepthmonoMeasurement(dpPointer->points[j].u, 478 dpPointer->points[j].v, dpPointer->points[j].depth), noise0); 479 //continue; 480 } else { 481 pointn = new Point3d_Node(); 482 points.push_back(pointn); 483 pointsInd.push_back(dpPointer->points[j].ind); 484 ba.add_node(pointn); 485 486 depthmonoFactorn = new Depthmono_Factor(poses[i], pointn, &camera, 487 DepthmonoMeasurement(dpPointer->points[j].u, 488 dpPointer->points[j].v, dpPointer->points[j].depth), noise1); 489 } 490 491 depthmonoFactors.push_back(depthmonoFactorn); 492 ba.add_factor(depthmonoFactorn); 493 } 494 } 495 } 496 } 497 498 Properties prop = ba.properties(); 499 prop.method = DOG_LEG; 500 ba.set_properties(prop); 501 ba.batch_optimization(); 502 503 transformBefBA[0] = depthPoints[keyframeNum - 1]->points[0].u; 504 transformBefBA[1] = depthPoints[keyframeNum - 1]->points[0].v; 505 transformBefBA[2] = depthPoints[keyframeNum - 1]->points[0].depth; 506 transformBefBA[3] = depthPoints[keyframeNum - 1]->points[1].u; 507 transformBefBA[4] = depthPoints[keyframeNum - 1]->points[1].v; 508 transformBefBA[5] = depthPoints[keyframeNum - 1]->points[1].depth; 509 510 transformAftBA[0] = poses[keyframeNum - 1]->value().pitch(); 511 transformAftBA[1] = poses[keyframeNum - 1]->value().yaw(); 512 transformAftBA[2] = poses[keyframeNum - 1]->value().roll(); 513 transformAftBA[3] = poses[keyframeNum - 1]->value().y(); 514 transformAftBA[4] = poses[keyframeNum - 1]->value().z(); 515 transformAftBA[5] = poses[keyframeNum - 1]->value().x(); 516 517 transformAftBA[0] = (1 - 0.5) * transformAftBA[0] + 0.5 * transformBefBA[0]; 518 //transformAftBA[1] = (1 - 0.1) * transformAftBA[1] + 0.1 * transformBefBA[1]; 519 transformAftBA[2] = (1 - 0.5) * transformAftBA[2] + 0.5 * transformBefBA[2]; 520 521 int posesNum = poses.size(); 522 for (int i = 1; i < posesNum; i++) { 523 delete poses[i]; 524 } 525 poses.clear(); 526 527 delete poseFactors0; 528 529 int posePoseFactorsNum = posePoseFactors.size(); 530 for (int i = 1; i < posePoseFactorsNum; i++) { 531 delete posePoseFactors[i]; 532 } 533 posePoseFactors.clear(); 534 535 int pointsNum = points.size(); 536 for (int i = 1; i < pointsNum; i++) { 537 delete points[i]; 538 } 539 points.clear(); 540 541 int depthmonoFactorsNum = depthmonoFactors.size(); 542 for (int i = 1; i < depthmonoFactorsNum; i++) { 543 delete depthmonoFactors[i]; 544 } 545 depthmonoFactors.clear(); 546 547 geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw 548 (transformBefBA[2], -transformBefBA[0], -transformBefBA[1]); 549 550 nav_msgs::Odometry odomBefBA; 551 odomBefBA.header.frame_id = "/camera_init"; 552 odomBefBA.child_frame_id = "/bef_ba"; 553 odomBefBA.header.stamp = ros::Time().fromSec(depthPointsTime); 554 odomBefBA.pose.pose.orientation.x = -geoQuat.y; 555 odomBefBA.pose.pose.orientation.y = -geoQuat.z; 556 odomBefBA.pose.pose.orientation.z = geoQuat.x; 557 odomBefBA.pose.pose.orientation.w = geoQuat.w; 558 odomBefBA.pose.pose.position.x = transformBefBA[3]; 559 odomBefBA.pose.pose.position.y = transformBefBA[4]; 560 odomBefBA.pose.pose.position.z = transformBefBA[5]; 561 odomBefBAPub.publish(odomBefBA); 562 563 geoQuat = tf::createQuaternionMsgFromRollPitchYaw 564 (transformAftBA[2], -transformAftBA[0], -transformAftBA[1]); 565 566 nav_msgs::Odometry odomAftBA; 567 odomAftBA.header.frame_id = "/camera_init"; 568 odomAftBA.child_frame_id = "/aft_ba"; 569 odomAftBA.header.stamp = ros::Time().fromSec(depthPointsTime); 570 odomAftBA.pose.pose.orientation.x = -geoQuat.y; 571 odomAftBA.pose.pose.orientation.y = -geoQuat.z; 572 odomAftBA.pose.pose.orientation.z = geoQuat.x; 573 odomAftBA.pose.pose.orientation.w = geoQuat.w; 574 odomAftBA.pose.pose.position.x = transformAftBA[3]; 575 odomAftBA.pose.pose.position.y = transformAftBA[4]; 576 odomAftBA.pose.pose.position.z = transformAftBA[5]; 577 odomAftBAPub.publish(odomAftBA); 578 579 tf::StampedTransform tfAftBA; 580 tfAftBA.frame_id_ = "/camera_init"; 581 tfAftBA.child_frame_id_ = "/aft_ba"; 582 tfAftBA.stamp_ = ros::Time().fromSec(depthPointsTime); 583 tfAftBA.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 584 tfAftBA.setOrigin(tf::Vector3(transformAftBA[3], 585 transformAftBA[4], transformAftBA[5])); 586 tfBroadcaster.sendTransform(tfAftBA); 587 } 588 589 status = ros::ok(); 590 cv::waitKey(10); 591 } 592 593 return 0; 594 }
