V-LOAM源碼解析（二）
轉載請註明出處：本文轉自zhch_pan的博客http://www.cnblogs.com/zhchp-blog/git
本博客爲本人以前作項目時作的源碼閱讀工做，po到網上但願幫助其餘人更好的理解V-LOAM的工程實現，有些地方代碼作了修改，可能和原工程有出入，但對於該工程的總體流程理解沒有妨礙。github
源碼下載連接：https://github.com/Jinqiang/demo_lidarweb
節點：visualOdometry算法
功能：訂閱"/image_points_last"和"/depth_cloud"消息，將圖像特徵與三維點雲匹配，得到圖像特徵點深度值，而後採用非線性優化的方法迭代收斂獲得兩幀圖像之間的變換矩陣，即融合了三維點雲信息的視覺里程計。app
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 #include <sensor_msgs/Imu.h> 8 9 #include <tf/transform_datatypes.h> 10 #include <tf/transform_listener.h> 11 #include <tf/transform_broadcaster.h> 12 13 #include "cameraParameters.h" 14 #include "pointDefinition.h" 15 16 const double PI = 3.1415926; 17 18 pcl::PointCloud<ImagePoint>::Ptr imagePointsCur(new pcl::PointCloud<ImagePoint>()); 19 pcl::PointCloud<ImagePoint>::Ptr imagePointsLast(new pcl::PointCloud<ImagePoint>()); 20 pcl::PointCloud<ImagePoint>::Ptr startPointsCur(new pcl::PointCloud<ImagePoint>()); 21 pcl::PointCloud<ImagePoint>::Ptr startPointsLast(new pcl::PointCloud<ImagePoint>()); 22 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransCur(new pcl::PointCloud<pcl::PointXYZHSV>()); 23 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransLast(new pcl::PointCloud<pcl::PointXYZHSV>()); 24 pcl::PointCloud<pcl::PointXYZHSV>::Ptr ipRelations(new pcl::PointCloud<pcl::PointXYZHSV>()); 25 pcl::PointCloud<pcl::PointXYZHSV>::Ptr ipRelations2(new pcl::PointCloud<pcl::PointXYZHSV>()); 26 pcl::PointCloud<pcl::PointXYZ>::Ptr imagePointsProj(new pcl::PointCloud<pcl::PointXYZ>()); 27 pcl::PointCloud<DepthPoint>::Ptr depthPointsCur(new pcl::PointCloud<DepthPoint>()); 28 pcl::PointCloud<DepthPoint>::Ptr depthPointsLast(new pcl::PointCloud<DepthPoint>()); 29 pcl::PointCloud<DepthPoint>::Ptr depthPointsSend(new pcl::PointCloud<DepthPoint>()); 30 31 std::vector<int> ipInd; 32 std::vector<float> ipy2; 33 34 std::vector<float>* ipDepthCur = new std::vector<float>(); 35 std::vector<float>* ipDepthLast = new std::vector<float>(); 36 37 double imagePointsCurTime; 38 double imagePointsLastTime; 39 40 int imagePointsCurNum = 0; 41 int imagePointsLastNum = 0; 42 43 int depthPointsCurNum = 0; 44 int depthPointsLastNum = 0; 45 46 pcl::PointCloud<pcl::PointXYZI>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZI>()); 47 pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdTree(new pcl::KdTreeFLANN<pcl::PointXYZI>()); 48 49 double depthCloudTime; 50 int depthCloudNum = 0; 51 52 std::vector<int> pointSearchInd; 53 std::vector<float> pointSearchSqrDis; 54 55 double transformSum[6] = {0}; 56 double angleSum[3] = {0}; 57 58 int imuPointerFront = 0; 59 int imuPointerLast = -1; 60 const int imuQueLength = 200; 61 bool imuInited = false; 62 63 double imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0; 64 double imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0; 65 66 double imuYawInit = 0; 67 double imuTime[imuQueLength] = {0}; 68 double imuRoll[imuQueLength] = {0}; 69 double imuPitch[imuQueLength] = {0}; 70 double imuYaw[imuQueLength] = {0}; 71 72 ros::Publisher *voDataPubPointer = NULL; 73 tf::TransformBroadcaster *tfBroadcasterPointer = NULL; 74 ros::Publisher *depthPointsPubPointer = NULL; 75 ros::Publisher *imagePointsProjPubPointer = NULL; 76 ros::Publisher *imageShowPubPointer; 77 78 const int showDSRate = 2; 79 80 void accumulateRotation(double cx, double cy, double cz, double lx, double ly, double lz, 81 double &ox, double &oy, double &oz) 82 { 83 /*R_wl=[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];（表示以world爲參考座標系） 84 *R_cl=[clz -slz 0;slz clz 0;0 0 1]*[1 0 0;0 clx -slx;0 slx clx]*[cly 0 sly;0 1 0;-sly 0 cly];（表示以current爲參考座標系） 85 *R_wc=R_wl*(R_cl).'; 86 *最後求出來(-sin(rx))=cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx) - cos(cx)*cos(lx)*sin(cz)*sin(ly) 87 *而程序中是(-sin(rx))= cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);（程序裏的srx=(-sin(rx))） 88 *能夠發現兩個公式之間差了lx,ly,lz的負號，因此accumulateRotation()函數傳入的是transform[0]~[2]的負值 89 *至於爲何-sinx等於上式，能夠經過看R_wl，發現第二行第三列的元素爲-sinx，所以兩個旋轉矩陣相乘後，對應位置上的元素就對應着新的pitch角的sin 值 90 */ 91 double srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx); 92 ox = -asin(srx); 93 94 double srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 95 + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy); 96 double crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 97 - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx)); 98 oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); 99 100 double srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 101 + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz); 102 double crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 103 - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)); 104 oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); 105 } 106 107 void diffRotation(double cx, double cy, double cz, double lx, double ly, double lz, 108 double &ox, double &oy, double &oz) 109 { 110 double srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 111 - cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx); 112 ox = -asin(srx); 113 114 double srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) 115 - cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz); 116 double crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly); 117 oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); 118 119 double srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy) 120 - cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 121 - (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)); 122 double crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz) 123 + cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz) 124 - cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz); 125 oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); 126 } 127 128 void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2) 129 { 130 imagePointsLastTime = imagePointsCurTime; 131 imagePointsCurTime = imagePoints2->header.stamp.toSec(); 132 133 imuRollLast = imuRollCur; 134 imuPitchLast = imuPitchCur; 135 imuYawLast = imuYawCur; 136 //transform用於記錄幀與幀之間的轉移矩陣，transformSum記錄當前幀與初始幀的轉移矩陣, 137 double transform[6] = {0}; 138 if (imuPointerLast >= 0) { 139 //將該幀圖像到來以前的全部IMU信息提取出來 140 while (imuPointerFront != imuPointerLast) { 141 if (imagePointsCurTime < imuTime[imuPointerFront]) { 142 break; 143 } 144 imuPointerFront = (imuPointerFront + 1) % imuQueLength; 145 } 146 147 if (imagePointsCurTime > imuTime[imuPointerFront]) { 148 imuRollCur = imuRoll[imuPointerFront]; 149 imuPitchCur = imuPitch[imuPointerFront]; 150 imuYawCur = imuYaw[imuPointerFront]; 151 } else { 152 int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; 153 double ratioFront = (imagePointsCurTime - imuTime[imuPointerBack]) 154 / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 155 double ratioBack = (imuTime[imuPointerFront] - imagePointsCurTime) 156 / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 157 158 //經過插值獲得img時刻的roll,pitch,yaw值 159 imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; 160 imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; 161 if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) { 162 imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack; 163 } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) { 164 imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack; 165 } else { 166 imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack; 167 } 168 } 169 170 if (imuInited) { 171 //transform[0] -= imuPitchCur - imuPitchLast; 172 //transform[1] -= imuYawCur - imuYawLast; 173 //transform[2] -= imuRollCur - imuRollLast; 174 } 175 } 176 177 pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast; 178 imagePointsLast = imagePointsCur; 179 imagePointsCur = imagePointsTemp; 180 181 imagePointsCur->clear(); 182 pcl::fromROSMsg(*imagePoints2, *imagePointsCur); 183 184 imagePointsLastNum = imagePointsCurNum; 185 imagePointsCurNum = imagePointsCur->points.size(); 186 187 pcl::PointCloud<ImagePoint>::Ptr startPointsTemp = startPointsLast; 188 startPointsLast = startPointsCur; 189 startPointsCur = startPointsTemp; 190 191 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransTemp = startTransLast; 192 startTransLast = startTransCur; 193 startTransCur = startTransTemp; 194 195 std::vector<float>* ipDepthTemp = ipDepthLast; 196 ipDepthLast = ipDepthCur; 197 ipDepthCur = ipDepthTemp; 198 199 int j = 0; 200 pcl::PointXYZI ips; 201 pcl::PointXYZHSV ipr; 202 ipRelations->clear(); 203 ipInd.clear(); 204 205 //這裏是以imagePointsLast爲基準進行查找，有些imagePointsCur中的點不會被查詢到 206 for (int i = 0; i < imagePointsLastNum; i++) { 207 bool ipFound = false; 208 //查找是否有匹配到的特徵點 209 for (; j < imagePointsCurNum; j++) { 210 if (imagePointsCur->points[j].ind == imagePointsLast->points[i].ind) { 211 ipFound = true; 212 } 213 if (imagePointsCur->points[j].ind >= imagePointsLast->points[i].ind) { 214 break; 215 } 216 } 217 218 //若是發現匹配的特徵點，嘗試獲取深度信息 219 if (ipFound) { 220 ipr.x = imagePointsLast->points[i].u; 221 ipr.y = imagePointsLast->points[i].v; 222 ipr.z = imagePointsCur->points[j].u; 223 ipr.h = imagePointsCur->points[j].v; 224 225 ips.x = 10 * ipr.x; 226 ips.y = 10 * ipr.y; 227 ips.z = 10; 228 229 if (depthCloudNum > 10) { 230 kdTree->nearestKSearch(ips, 3, pointSearchInd, pointSearchSqrDis); 231 232 double minDepth, maxDepth; 233 if (pointSearchSqrDis[0] < 0.5 && pointSearchInd.size() == 3) { 234 pcl::PointXYZI depthPoint = depthCloud->points[pointSearchInd[0]]; 235 double x1 = depthPoint.x * depthPoint.intensity / 10; 236 double y1 = depthPoint.y * depthPoint.intensity / 10; 237 double z1 = depthPoint.intensity; 238 minDepth = z1; 239 maxDepth = z1; 240 241 depthPoint = depthCloud->points[pointSearchInd[1]]; 242 double x2 = depthPoint.x * depthPoint.intensity / 10; 243 double y2 = depthPoint.y * depthPoint.intensity / 10; 244 double z2 = depthPoint.intensity; 245 minDepth = (z2 < minDepth)? z2 : minDepth; 246 maxDepth = (z2 > maxDepth)? z2 : maxDepth; 247 248 depthPoint = depthCloud->points[pointSearchInd[2]]; 249 double x3 = depthPoint.x * depthPoint.intensity / 10; 250 double y3 = depthPoint.y * depthPoint.intensity / 10; 251 double z3 = depthPoint.intensity; 252 minDepth = (z3 < minDepth)? z3 : minDepth; 253 maxDepth = (z3 > maxDepth)? z3 : maxDepth; 254 //目前只知道該特徵點在相機座標系下的歸一化座標[u,v]（即[X/Z,Y/Z,1]）， 255 //經過計算ipr.s得到對應於該特徵點的深度值,即係數Z，則Z*u和Z*v就可得到該特徵點在相機座標系下實際的X,Y,Z座標 256 double u = ipr.x; 257 double v = ipr.y; 258 ipr.s = (x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1) 259 / (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2 + u*y1*z2 - u*y2*z1 260 - v*x1*z2 + v*x2*z1 - u*y1*z3 + u*y3*z1 + v*x1*z3 - v*x3*z1 + u*y2*z3 261 - u*y3*z2 - v*x2*z3 + v*x3*z2); 262 ipr.v = 1; 263 264 if (maxDepth - minDepth > 2) { 265 ipr.s = 0; 266 ipr.v = 0; 267 } else if (ipr.s - maxDepth > 0.2) { 268 ipr.s = maxDepth; 269 } else if (ipr.s - minDepth < -0.2) { 270 ipr.s = minDepth; 271 } 272 } else { 273 ipr.s = 0; 274 ipr.v = 0; 275 } 276 } else { 277 ipr.s = 0; 278 ipr.v = 0; 279 } 280 //若是沒法從點雲獲取深度信息，三角測量？ 281 if (fabs(ipr.v) < 0.5) { 282 double disX = transformSum[3] - startTransLast->points[i].h; 283 double disY = transformSum[4] - startTransLast->points[i].s; 284 double disZ = transformSum[5] - startTransLast->points[i].v; 285 //若移動距離大於1m 286 if (sqrt(disX * disX + disY * disY + disZ * disZ) > 1) { 287 288 double u0 = startPointsLast->points[i].u; 289 double v0 = startPointsLast->points[i].v; 290 double u1 = ipr.x; 291 double v1 = ipr.y; 292 293 double srx0 = sin(-startTransLast->points[i].x); 294 double crx0 = cos(-startTransLast->points[i].x); 295 double sry0 = sin(-startTransLast->points[i].y); 296 double cry0 = cos(-startTransLast->points[i].y); 297 double srz0 = sin(-startTransLast->points[i].z); 298 double crz0 = cos(-startTransLast->points[i].z); 299 300 double srx1 = sin(-transformSum[0]); 301 double crx1 = cos(-transformSum[0]); 302 double sry1 = sin(-transformSum[1]); 303 double cry1 = cos(-transformSum[1]); 304 double srz1 = sin(-transformSum[2]); 305 double crz1 = cos(-transformSum[2]); 306 307 double tx0 = -startTransLast->points[i].h; 308 double ty0 = -startTransLast->points[i].s; 309 double tz0 = -startTransLast->points[i].v; 310 311 double tx1 = -transformSum[3]; 312 double ty1 = -transformSum[4]; 313 double tz1 = -transformSum[5]; 314 315 double x1 = crz0 * u0 + srz0 * v0; 316 double y1 = -srz0 * u0 + crz0 * v0; 317 double z1 = 1; 318 319 double x2 = x1; 320 double y2 = crx0 * y1 + srx0 * z1; 321 double z2 = -srx0 * y1 + crx0 * z1; 322 323 double x3 = cry0 * x2 - sry0 * z2; 324 double y3 = y2; 325 double z3 = sry0 * x2 + cry0 * z2; 326 327 double x4 = cry1 * x3 + sry1 * z3; 328 double y4 = y3; 329 double z4 = -sry1 * x3 + cry1 * z3; 330 331 double x5 = x4; 332 double y5 = crx1 * y4 - srx1 * z4; 333 double z5 = srx1 * y4 + crx1 * z4; 334 335 double x6 = crz1 * x5 - srz1 * y5; 336 double y6 = srz1 * x5 + crz1 * y5; 337 double z6 = z5; 338 339 u0 = x6 / z6; 340 v0 = y6 / z6; 341 342 x1 = cry1 * (tx1 - tx0) + sry1 * (tz1 - tz0); 343 y1 = ty1 - ty0; 344 z1 = -sry1 * (tx1 - tx0) + cry1 * (tz1 - tz0); 345 346 x2 = x1; 347 y2 = crx1 * y1 - srx1 * z1; 348 z2 = srx1 * y1 + crx1 * z1; 349 350 double tx = crz1 * x2 - srz1 * y2; 351 double ty = srz1 * x2 + crz1 * y2; 352 double tz = z2; 353 354 double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1)) 355 * cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1)); 356 double depth = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta; 357 358 if (depth > 0.5 && depth < 100) { 359 ipr.s = depth; 360 ipr.v = 2; 361 } 362 } 363 364 //每個匹配上的特徵點對都會被打上一個ipr.v標籤，ipr.v＝０表明未從點雲得到深度，ipr.v＝１ 365 //表明從點雲得到深度，ipr.v＝２表明此時能夠經過三角測量得到該特徵點的深度值。 366 /* 367 *ipDepthLast)[i]存儲的是第i個特徵點的三角測量值，若該特徵點從未被三角測量過， 368 *ipDepthLast)[i]＝－１；若該特徵點在一段時間內能一直被觀測到，且沒有從點雲中得到 369 *深度信息，則不斷經過三角測量的融合收斂該特徵點的深度值；若在這個過程當中某幾幀中能從 370 *點雲得到該特徵點的深度，則使用點雲的深度信息，三角測量的結果仍經過計算出的轉移矩陣 371 *進行維護，仍然存儲在*ipDepthLast)[i]中，一旦沒法從點雲得到深度，則仍使用 372 *ipDepthLast)[i]中保存的三角測量進行融合 373 */ 374 if (ipr.v == 2) { 375 if ((*ipDepthLast)[i] > 0) { 376 //這一步進行的是屢次三角測量的融合,低通濾波 377 ipr.s = 3 * ipr.s * (*ipDepthLast)[i] / (ipr.s + 2 * (*ipDepthLast)[i]); 378 } 379 (*ipDepthLast)[i] = ipr.s; 380 } else if ((*ipDepthLast)[i] > 0) { 381 ipr.s = (*ipDepthLast)[i]; 382 ipr.v = 2; 383 } 384 } 385 386 ipRelations->push_back(ipr); 387 ipInd.push_back(imagePointsLast->points[i].ind); 388 } 389 } 390 391 //迭代收斂得到兩幀圖像間的轉移矩陣 392 int iterNum = 100; 393 pcl::PointXYZHSV ipr2, ipr3, ipr4; 394 int ipRelationsNum = ipRelations->points.size(); 395 int ptNumNoDepthRec = 0; 396 int ptNumWithDepthRec = 0; 397 double meanValueWithDepthRec = 100000; 398 for (int iterCount = 0; iterCount < iterNum; iterCount++) { 399 ipRelations2->clear(); 400 ipy2.clear(); 401 int ptNumNoDepth = 0; 402 int ptNumWithDepth = 0; 403 double meanValueNoDepth = 0; 404 double meanValueWithDepth = 0; 405 for (int i = 0; i < ipRelationsNum; i++) { 406 ipr = ipRelations->points[i]; 407 408 double u0 = ipr.x; 409 double v0 = ipr.y; 410 double u1 = ipr.z; 411 double v1 = ipr.h; 412 413 double srx = sin(transform[0]); 414 double crx = cos(transform[0]); 415 double sry = sin(transform[1]); 416 double cry = cos(transform[1]); 417 double srz = sin(transform[2]); 418 double crz = cos(transform[2]); 419 double tx = transform[3]; 420 double ty = transform[4]; 421 double tz = transform[5]; 422 423 if (fabs(ipr.v) < 0.5) { 424 /* 425 * 這裏R矩陣使用歐拉角roll,pitch yaw來表示的，下面六個公式是論文公式（6）在對roll,pitch,yaw和tx,ty,tz求偏導 426 *transform[0]存儲的是繞x軸旋轉的角度，transform[1]存繞y軸角度，transform[2]存繞z軸角度 427 *這裏計算的R、T矩陣是k-1時刻旋轉到k時刻的R、T矩陣，注意這個主次關係,參考座標系是current,即k時刻座標系 428 *從k-1旋轉到k的順序是：z軸->x軸->y軸，注意順序 429 *R_cl=[crz -srz 0;srz crz 0;0 0 1]*[1 0 0;0 crx -srx;0 srx crx]*[cry 0 sry;0 1 0;-sry 0 cry]; 430 */ 431 ipr2.x = v0*(crz*srx*(tx - tz*u1) - crx*(ty*u1 - tx*v1) + srz*srx*(ty - tz*v1)) 432 - u0*(sry*srx*(ty*u1 - tx*v1) + crz*sry*crx*(tx - tz*u1) + sry*srz*crx*(ty - tz*v1)) 433 + cry*srx*(ty*u1 - tx*v1) + cry*crz*crx*(tx - tz*u1) + cry*srz*crx*(ty - tz*v1); 434 435 ipr2.y = u0*((tx - tz*u1)*(srz*sry - crz*srx*cry) - (ty - tz*v1)*(crz*sry + srx*srz*cry) 436 + crx*cry*(ty*u1 - tx*v1)) - (tx - tz*u1)*(srz*cry + crz*srx*sry) 437 + (ty - tz*v1)*(crz*cry - srx*srz*sry) + crx*sry*(ty*u1 - tx*v1); 438 439 ipr2.z = -u0*((tx - tz*u1)*(cry*crz - srx*sry*srz) + (ty - tz*v1)*(cry*srz + srx*sry*crz)) 440 - (tx - tz*u1)*(sry*crz + cry*srx*srz) - (ty - tz*v1)*(sry*srz - cry*srx*crz) 441 - v0*(crx*crz*(ty - tz*v1) - crx*srz*(tx - tz*u1)); 442 443 ipr2.h = cry*crz*srx - v0*(crx*crz - srx*v1) - u0*(cry*srz + crz*srx*sry + crx*sry*v1) 444 - sry*srz + crx*cry*v1; 445 446 ipr2.s = crz*sry - v0*(crx*srz + srx*u1) + u0*(cry*crz + crx*sry*u1 - srx*sry*srz) 447 - crx*cry*u1 + cry*srx*srz; 448 449 ipr2.v = u1*(sry*srz - cry*crz*srx) - v1*(crz*sry + cry*srx*srz) + u0*(u1*(cry*srz + crz*srx*sry) 450 - v1*(cry*crz - srx*sry*srz)) + v0*(crx*crz*u1 + crx*srz*v1); 451 //將六個變量值代入論文（公式6）計算獲得殘差值 452 double y2 = (ty - tz*v1)*(crz*sry + cry*srx*srz) - (tx - tz*u1)*(sry*srz - cry*crz*srx) 453 - v0*(srx*(ty*u1 - tx*v1) + crx*crz*(tx - tz*u1) + crx*srz*(ty - tz*v1)) 454 + u0*((ty - tz*v1)*(cry*crz - srx*sry*srz) - (tx - tz*u1)*(cry*srz + crz*srx*sry) 455 + crx*sry*(ty*u1 - tx*v1)) - crx*cry*(ty*u1 - tx*v1); 456 457 if (ptNumNoDepthRec < 50 || iterCount < 25 || fabs(y2) < 2 * meanValueWithDepthRec / 10000) { 458 double scale = 100; 459 ipr2.x *= scale; 460 ipr2.y *= scale; 461 ipr2.z *= scale; 462 ipr2.h *= scale; 463 ipr2.s *= scale; 464 ipr2.v *= scale; 465 y2 *= scale; 466 467 ipRelations2->push_back(ipr2); 468 ipy2.push_back(y2); 469 470 ptNumNoDepth++; 471 } else { 472 ipRelations->points[i].v = -1; 473 } 474 } else if (fabs(ipr.v - 1) < 0.5 || fabs(ipr.v - 2) < 0.5) { 475 476 double d0 = ipr.s; 477 478 ipr3.x = d0*(cry*srz*crx + cry*u1*srx) - d0*u0*(sry*srz*crx + sry*u1*srx) 479 - d0*v0*(u1*crx - srz*srx); 480 481 ipr3.y = d0*(crz*cry + crx*u1*sry - srx*srz*sry) - d0*u0*(crz*sry - crx*u1*cry + srx*srz*cry); 482 483 ipr3.z = -d0*(sry*srz - cry*srx*crz) - d0*u0*(cry*srz + srx*sry*crz) - crx*d0*v0*crz; 484 485 ipr3.h = 1; 486 487 ipr3.s = 0; 488 489 ipr3.v = -u1; 490 491 double y3 = tx - tz*u1 + d0*(crz*sry - crx*cry*u1 + cry*srx*srz) - d0*v0*(crx*srz + srx*u1) 492 + d0*u0*(cry*crz + crx*sry*u1 - srx*sry*srz); 493 494 ipr4.x = d0*(cry*v1*srx - cry*crz*crx) + d0*u0*(crz*sry*crx - sry*v1*srx) 495 - d0*v0*(crz*srx + v1*crx); 496 497 ipr4.y = d0*(srz*cry + crz*srx*sry + crx*v1*sry) + d0*u0*(crz*srx*cry - srz*sry + crx*v1*cry); 498 499 ipr4.z = d0*(sry*crz + cry*srx*srz) + d0*u0*(cry*crz - srx*sry*srz) - crx*d0*v0*srz; 500 501 ipr4.h = 0; 502 503 ipr4.s = 1; 504 505 ipr4.v = -v1; 506 507 double y4 = ty - tz*v1 - d0*(cry*crz*srx - sry*srz + crx*cry*v1) + d0*v0*(crx*crz - srx*v1) 508 + d0*u0*(cry*srz + crz*srx*sry + crx*sry*v1); 509 510 if (ptNumWithDepthRec < 50 || iterCount < 25 || 511 sqrt(y3 * y3 + y4 * y4) < 2 * meanValueWithDepthRec) { 512 ipRelations2->push_back(ipr3); 513 ipy2.push_back(y3); 514 515 ipRelations2->push_back(ipr4); 516 ipy2.push_back(y4); 517 518 ptNumWithDepth++; 519 meanValueWithDepth += sqrt(y3 * y3 + y4 * y4); 520 } else { 521 ipRelations->points[i].v = -1; 522 } 523 } 524 } 525 //加 0.01 是爲了防止 ptNumWithDepth 爲 0 526 meanValueWithDepth /= (ptNumWithDepth + 0.01); 527 ptNumNoDepthRec = ptNumNoDepth; 528 ptNumWithDepthRec = ptNumWithDepth; 529 meanValueWithDepthRec = meanValueWithDepth; 530 531 int ipRelations2Num = ipRelations2->points.size(); 532 if (ipRelations2Num > 10) { 533 cv::Mat matA(ipRelations2Num, 6, CV_32F, cv::Scalar::all(0)); 534 cv::Mat matAt(6, ipRelations2Num, CV_32F, cv::Scalar::all(0)); 535 cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0)); 536 cv::Mat matB(ipRelations2Num, 1, CV_32F, cv::Scalar::all(0)); 537 cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); 538 cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); 539 540 for (int i = 0; i < ipRelations2Num; i++) { 541 ipr2 = ipRelations2->points[i]; 542 543 matA.at<float>(i, 0) = ipr2.x; 544 matA.at<float>(i, 1) = ipr2.y; 545 matA.at<float>(i, 2) = ipr2.z; 546 matA.at<float>(i, 3) = ipr2.h; 547 matA.at<float>(i, 4) = ipr2.s; 548 matA.at<float>(i, 5) = ipr2.v; 549 matB.at<float>(i, 0) = -0.2 * ipy2[i]; 550 } 551 cv::transpose(matA, matAt); 552 matAtA = matAt * matA; 553 matAtB = matAt * matB; 554 //根據《14講》式（6.21），這裏用的是高斯牛頓法而不是LM算法 555 cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); 556 557 //if (fabs(matX.at<float>(0, 0)) < 0.1 && fabs(matX.at<float>(1, 0)) < 0.1 && 558 // fabs(matX.at<float>(2, 0)) < 0.1) { 559 transform[0] += matX.at<float>(0, 0); 560 transform[1] += matX.at<float>(1, 0); 561 transform[2] += matX.at<float>(2, 0); 562 transform[3] += matX.at<float>(3, 0); 563 transform[4] += matX.at<float>(4, 0); 564 transform[5] += matX.at<float>(5, 0); 565 //} 566 567 float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI 568 + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI 569 + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI); 570 float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100 571 + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100 572 + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100); 573 574 if (deltaR < 0.00001 && deltaT < 0.00001) { 575 break; 576 } 577 578 //ROS_INFO ("iter: %d, deltaR: %f, deltaT: %f", iterCount, deltaR, deltaT); 579 } 580 } 581 582 if (!imuInited) { 583 imuYawInit = imuYawCur; 584 transform[0] -= imuPitchCur; 585 transform[2] -= imuRollCur; 586 587 imuInited = true; 588 } 589 590 //rx,ry,rz表示當前幀與初始幀的pitch,yaw，roll角度 591 double rx, ry, rz; 592 accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 593 -transform[0], -transform[1], -transform[2], rx, ry, rz); 594 595 if (imuPointerLast >= 0) { 596 double drx, dry, drz; 597 diffRotation(imuPitchCur, imuYawCur - imuYawInit, imuRollCur, rx, ry, rz, drx, dry, drz); 598 599 transform[0] -= 0.1 * drx; 600 /*if (dry > PI) { 601 transform[1] -= 0.1 * (dry - 2 * PI); 602 } else if (imuYawCur - imuYawInit - ry < -PI) { 603 transform[1] -= 0.1 * (dry + 2 * PI); 604 } else { 605 transform[1] -= 0.1 * dry; 606 }*/ 607 transform[2] -= 0.1 * drz; 608 609 accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 610 -transform[0], -transform[1], -transform[2], rx, ry, rz); 611 } 612 613 double x1 = cos(rz) * transform[3] - sin(rz) * transform[4]; 614 double y1 = sin(rz) * transform[3] + cos(rz) * transform[4]; 615 double z1 = transform[5]; 616 617 double x2 = x1; 618 double y2 = cos(rx) * y1 - sin(rx) * z1; 619 double z2 = sin(rx) * y1 + cos(rx) * z1; 620 621 //當前幀與上一幀的位移量經過rx,ry,rz的旋轉，計算當前幀和初始幀的位移增量，疊加到transformSum[]中 622 //該增量計算獲得的是last幀相對於current幀在世界座標系下的位移,因此current相對於Last在世界座標系下的位移爲負值,因此是減去 623 double tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2); 624 double ty = transformSum[4] - y2; 625 double tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2); 626 //當前幀與初始幀的轉移矩陣保存在transformSum中,transformSum中存的是將當前幀旋轉到起始幀的旋轉矩陣 627 transformSum[0] = rx; 628 transformSum[1] = ry; 629 transformSum[2] = rz; 630 transformSum[3] = tx; 631 transformSum[4] = ty; 632 transformSum[5] = tz; 633 634 pcl::PointXYZHSV spc; 635 spc.x = transformSum[0]; 636 spc.y = transformSum[1]; 637 spc.z = transformSum[2]; 638 spc.h = transformSum[3]; 639 spc.s = transformSum[4]; 640 spc.v = transformSum[5]; 641 642 double crx = cos(transform[0]); 643 double srx = sin(transform[0]); 644 double cry = cos(transform[1]); 645 double sry = sin(transform[1]); 646 647 j = 0; 648 //這裏是以imagePointsCur爲基準進行查找，會遍歷imagePointsCur中的每個點 649 for (int i = 0; i < imagePointsCurNum; i++) { 650 bool ipFound = false; 651 for (; j < imagePointsLastNum; j++) { 652 if (imagePointsLast->points[j].ind == imagePointsCur->points[i].ind) { 653 ipFound = true; 654 } 655 if (imagePointsLast->points[j].ind >= imagePointsCur->points[i].ind) { 656 break; 657 } 658 } 659 660 if (ipFound) { 661 /* 662 *若是在連續的多幀特徵圖像之間，imagePointsCur中的某個特徵點可以與上一幀 663 *imagePointsLast匹配到，表明該特徵點可以被連續觀測到，則將該特徵點第一次出如今這些連續幀 664 *的座標以及該座標相對於初始幀的轉移矩陣保存在startPointsCur中，用做三角測量時的第一幀 665 *特徵點；若該特徵點一旦與前一幀匹配失敗，就表示該特徵點爲一系列特徵點幀中的一個新出現的 666 *特徵點，則將該特徵點當前的座標與轉移矩陣保存在startPointsCur，認爲它第一次出現，並在後續幀中 667 *若能一直觀測到該特徵點，startPointsCur中仍保存第一次出現的座標與轉移矩陣 668 */ 669 startPointsCur->push_back(startPointsLast->points[j]); 670 startTransCur->push_back(startTransLast->points[j]); 671 672 if ((*ipDepthLast)[j] > 0) { 673 /* 674 *transform[]裏存的就是T_cl,因此將Last座標系的點按照zxy(從右往左看)的順序旋轉,再加上位移就變換到了current座標系 675 *而R_lc就是把transform[0]~transform[2]的pitch,yaw,roll角取負值而後按照yxz(從右往左看)的順序變換就可獲得 676 *這裏有一點注意的是,將transform[0]~[2]按照yxz相乘獲得的R_lc和直接將R_cl取轉置獲得的R_lc差了三個角度的負值 677 *因此經過旋轉相乘獲得R_lc時transform[0]~[2]要先取負值 678 */ 679 double ipz = (*ipDepthLast)[j]; 680 double ipx = imagePointsLast->points[j].u * ipz; 681 double ipy = imagePointsLast->points[j].v * ipz; 682 683 x1 = cry * ipx + sry * ipz; 684 y1 = ipy; 685 z1 = -sry * ipx + cry * ipz; 686 687 x2 = x1; 688 y2 = crx * y1 - srx * z1; 689 z2 = srx * y1 + crx * z1; 690 691 ipDepthCur->push_back(z2 + transform[5]); 692 } else { 693 ipDepthCur->push_back(-1); 694 } 695 } else { 696 startPointsCur->push_back(imagePointsCur->points[i]); 697 startTransCur->push_back(spc); 698 ipDepthCur->push_back(-1); 699 } 700 } 701 startPointsLast->clear(); 702 startTransLast->clear(); 703 ipDepthLast->clear(); 704 705 angleSum[0] -= transform[0]; 706 angleSum[1] -= transform[1]; 707 angleSum[2] -= transform[2]; 708 709 /* rz,rx,ry分別對應着標準右手座標系中的roll,pitch,yaw角,經過查看createQuaternionMsgFromRollPitchYaw()的函數定義能夠發現. 710 * 當pitch和yaw角給負值後,四元數中的y和z會變成負值,x和w不受影響.由四元數定義能夠知道,x,y,z是指旋轉軸在三個軸上的投影,w影響 711 * 旋轉角度,因此由createQuaternionMsgFromRollPitchYaw()計算獲得四元數後,其在通常右手座標系中的x,y,z份量對應到該應用場景下 712 * 的座標系中,geoQuat.x對應實際座標系下的z軸份量,geoQuat.y對應x軸份量,geoQuat.z對應實際的y軸份量,而因爲rx和ry在計算四元數 713 * 時給的是負值,因此geoQuat.y和geoQuat.z取負值,這樣就等於沒變 714 */ 715 716 geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry); 717 //geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, rx, ry); 718 719 nav_msgs::Odometry voData; 720 voData.header.frame_id = "/camera_init"; 721 voData.child_frame_id = "/camera"; 722 voData.header.stamp = imagePoints2->header.stamp; 723 voData.pose.pose.orientation.x = -geoQuat.y; 724 voData.pose.pose.orientation.y = -geoQuat.z; 725 //voData.pose.pose.orientation.x = geoQuat.y; 726 //voData.pose.pose.orientation.y = geoQuat.z; 727 voData.pose.pose.orientation.z = geoQuat.x; 728 voData.pose.pose.orientation.w = geoQuat.w; 729 voData.pose.pose.position.x = tx; 730 voData.pose.pose.position.y = ty; 731 voData.pose.pose.position.z = tz; 732 voData.twist.twist.angular.x = angleSum[0]; 733 voData.twist.twist.angular.y = angleSum[1]; 734 voData.twist.twist.angular.z = angleSum[2]; 735 voDataPubPointer->publish(voData); 736 737 tf::StampedTransform voTrans; 738 voTrans.frame_id_ = "/camera_init"; 739 voTrans.child_frame_id_ = "/camera"; 740 voTrans.stamp_ = imagePoints2->header.stamp; 741 voTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 742 //voTrans.setRotation(tf::Quaternion(geoQuat.y, geoQuat.z, geoQuat.x, geoQuat.w)); 743 voTrans.setOrigin(tf::Vector3(tx, ty, tz)); 744 tfBroadcasterPointer->sendTransform(voTrans); 745 746 pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp = depthPointsLast; 747 depthPointsLast = depthPointsCur; 748 depthPointsCur = depthPointsTemp; 749 750 DepthPoint ipd; 751 depthPointsCur->clear(); 752 753 ipd.u = transformSum[0]; 754 ipd.v = transformSum[1]; 755 ipd.depth = transformSum[2]; 756 ipd.ind = -2; 757 depthPointsCur->push_back(ipd); 758 759 ipd.u = transformSum[3]; 760 ipd.v = transformSum[4]; 761 ipd.depth = transformSum[5]; 762 ipd.ind = -1; 763 depthPointsCur->push_back(ipd); 764 765 depthPointsLastNum = depthPointsCurNum; 766 depthPointsCurNum = 2; 767 768 j = 0; 769 pcl::PointXYZ ipp; 770 depthPointsSend->clear(); 771 imagePointsProj->clear(); 772 for (int i = 0; i < ipRelationsNum; i++) { 773 if (fabs(ipRelations->points[i].v - 1) < 0.5 || fabs(ipRelations->points[i].v - 2) < 0.5) { 774 775 ipd.u = ipRelations->points[i].z; 776 ipd.v = ipRelations->points[i].h; 777 //這一步是對標號爲ind的特徵點深度進行的一個粗略估計,後面若是該特徵點能夠直接從點雲或者三角測量得到深度值, 778 //則這個估計值失效,若是後面不能獲得該特徵深度值,則仍使用該估計值 779 //////////////////////////////////////////////////////////////////// 780 /*double ipz = ipRelations->points[i].s; 781 double ipx = ipRelations->points[i].x * ipz; 782 double ipy = ipRelations->points[i].y * ipz; 783 784 double x1 = cry * ipx + sry * ipz; 785 double y1 = ipy; 786 double z1 = -sry * ipx + cry * ipz; 787 788 double x2 = x1; 789 double y2 = crx * y1 - srx * z1; 790 double z2 = srx * y1 + crx * z1; 791 ipd.depth = z2 + transform[5]; 792 */ 793 //////////////////////////////////////////////////////////////////// 794 795 ipd.depth = ipRelations->points[i].s + transform[5]; 796 ipd.label = ipRelations->points[i].v; 797 ipd.ind = ipInd[i]; 798 799 depthPointsCur->push_back(ipd); 800 depthPointsCurNum++; 801 802 for (; j < depthPointsLastNum; j++) { 803 if (depthPointsLast->points[j].ind < ipInd[i]) { 804 depthPointsSend->push_back(depthPointsLast->points[j]); 805 } else if (depthPointsLast->points[j].ind > ipInd[i]) { 806 break; 807 } 808 } 809 810 ipd.u = ipRelations->points[i].x; 811 ipd.v = ipRelations->points[i].y; 812 ipd.depth = ipRelations->points[i].s; 813 814 //如今有Last幀和Cur幀，depthPointsSend中存儲的是對Last幀中特徵點深度的估計和確切計算到的深度 815 depthPointsSend->push_back(ipd); 816 817 ipp.x = ipRelations->points[i].x * ipRelations->points[i].s; 818 ipp.y = ipRelations->points[i].y * ipRelations->points[i].s; 819 ipp.z = ipRelations->points[i].s; 820 821 imagePointsProj->push_back(ipp); 822 } 823 } 824 825 sensor_msgs::PointCloud2 depthPoints2; 826 pcl::toROSMsg(*depthPointsSend, depthPoints2); 827 depthPoints2.header.frame_id = "camera2"; 828 depthPoints2.header.stamp = ros::Time().fromSec(imagePointsLastTime); 829 depthPointsPubPointer->publish(depthPoints2); 830 831 sensor_msgs::PointCloud2 imagePointsProj2; 832 pcl::toROSMsg(*imagePointsProj, imagePointsProj2); 833 imagePointsProj2.header.frame_id = "camera2"; 834 imagePointsProj2.header.stamp = ros::Time().fromSec(imagePointsLastTime); 835 imagePointsProjPubPointer->publish(imagePointsProj2); 836 } 837 838 void depthCloudHandler(const sensor_msgs::PointCloud2ConstPtr& depthCloud2) 839 { 840 depthCloudTime = depthCloud2->header.stamp.toSec(); 841 842 depthCloud->clear(); 843 pcl::fromROSMsg(*depthCloud2, *depthCloud); 844 depthCloudNum = depthCloud->points.size(); 845 //將整個點雲投影到焦距爲單位距離=10的平面上 846 if (depthCloudNum > 10) { 847 for (int i = 0; i < depthCloudNum; i++) { 848 depthCloud->points[i].intensity = depthCloud->points[i].z; 849 depthCloud->points[i].x *= 10 / depthCloud->points[i].z; 850 depthCloud->points[i].y *= 10 / depthCloud->points[i].z; 851 depthCloud->points[i].z = 10; 852 } 853 854 kdTree->setInputCloud(depthCloud); 855 } 856 } 857 858 void imuDataHandler(const sensor_msgs::Imu::ConstPtr& imuData) 859 { 860 double roll, pitch, yaw; 861 tf::Quaternion orientation; 862 tf::quaternionMsgToTF(imuData->orientation, orientation); 863 tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 864 865 imuPointerLast = (imuPointerLast + 1) % imuQueLength; 866 867 imuTime[imuPointerLast] = imuData->header.stamp.toSec() - 0.1068; 868 imuRoll[imuPointerLast] = roll; 869 imuPitch[imuPointerLast] = pitch; 870 imuYaw[imuPointerLast] = yaw; 871 } 872 873 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 874 { 875 cv_bridge::CvImagePtr bridge = cv_bridge::toCvCopy(imageData, "bgr8"); 876 877 int ipRelationsNum = ipRelations->points.size(); 878 for (int i = 0; i < ipRelationsNum; i++) { 879 if (fabs(ipRelations->points[i].v) < 0.5) { 880 //這裏運用《14講》公式5.5將歸一化座標平面上的點變換到圖像平面，這裏的相減是由於在featureTrack部分將圖像座標 881 //變到歸一化圖像平面時加了個負號,又考慮到顯示的圖像是縮小一倍後的，因此要再除以showDSRate 882 cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, 883 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(255, 0, 0), 2); 884 } else if (fabs(ipRelations->points[i].v - 1) < 0.5) { 885 cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, 886 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 255, 0), 2); 887 } else if (fabs(ipRelations->points[i].v - 2) < 0.5) { 888 cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, 889 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 255), 2); 890 } /*else { 891 cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate, 892 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 0), 2); 893 }*/ 894 } 895 896 sensor_msgs::Image::Ptr imagePointer = bridge->toImageMsg(); 897 imageShowPubPointer->publish(imagePointer); 898 } 899 900 int main(int argc, char** argv) 901 { 902 ros::init(argc, argv, "visualOdometry"); 903 ros::NodeHandle nh; 904 905 ros::Subscriber imagePointsSub = nh.subscribe<sensor_msgs::PointCloud2> 906 ("/image_points_last", 5, imagePointsHandler); 907 908 ros::Subscriber depthCloudSub = nh.subscribe<sensor_msgs::PointCloud2> 909 ("/depth_cloud", 5, depthCloudHandler); 910 911 /* 912 * whether you need IMU information 913 */ 914 //ros::Subscriber imuDataSub = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 5, imuDataHandler); 915 916 ros::Publisher voDataPub = nh.advertise<nav_msgs::Odometry> ("/cam_to_init", 5); 917 voDataPubPointer = &voDataPub; 918 919 tf::TransformBroadcaster tfBroadcaster; 920 tfBroadcasterPointer = &tfBroadcaster; 921 922 ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_last", 5); 923 depthPointsPubPointer = &depthPointsPub; 924 925 ros::Publisher imagePointsProjPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_proj", 1); 926 imagePointsProjPubPointer = &imagePointsProjPub; 927 928 ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/image/show", 1, imageDataHandler); 929 930 ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show_2", 1); 931 imageShowPubPointer = &imageShowPub; 932 933 ros::spin(); 934 935 return 0; 936 }
相關文章
- 1. Picasso-源碼解析(二)
- 2. Vue源碼解析（二）
- 3. ReactNative iOS源碼解析（二）
- 4. LinkedList源碼解析（二）
- 5. SpringMVC-源碼解析(二)
- 6. nacos 1.3.0源碼解析（二）
- 7. Spring源碼解析（二）
- 8. Vue的源碼解析（二）
- 9. RxJava2 源碼解析（二）
- 10. Spring 源碼解析（二）
- 更多相關文章...
- • XML DOM 解析器 - XML DOM 教程
- • TCP報文格式解析 - TCP/IP教程
- • Java Agent入門實戰（二）-Instrumentation源碼概述
- • Scala 中文亂碼解決
相關標籤/搜索
每日一句
-
每一个你不满意的现在，都有一个你没有努力的曾经。
最新文章
- 1. 字節跳動21屆秋招運營兩輪面試經驗分享
- 2. Java 3 年，25K 多嗎？
- 3. mysql安裝部署
- 4. web前端開發中父鏈和子鏈方式實現通信
- 5. 3.1.6 spark體系之分佈式計算-scala編程-scala中trait特性
- 6. dataframe2
- 7. ThinkFree在線
- 8. 在線畫圖
- 9. devtools熱部署
- 10. 編譯和鏈接
歡迎關注本站公眾號,獲取更多信息
相關文章
- 1. Picasso-源碼解析(二)
- 2. Vue源碼解析（二）
- 3. ReactNative iOS源碼解析（二）
- 4. LinkedList源碼解析（二）
- 5. SpringMVC-源碼解析(二)
- 6. nacos 1.3.0源碼解析（二）
- 7. Spring源碼解析（二）
- 8. Vue的源碼解析（二）
- 9. RxJava2 源碼解析（二）
- 10. Spring 源碼解析（二）