1 #include "dbtype.h" 2 #include "dbkdtree.h" 3 4 #include <pcl/point_cloud.h> 5 #include <pcl/io/pcd_io.h> 6 #include <pcl/point_types.h> 7 #include <pcl/visualization/pcl_visualizer.h> 8 #include <pcl/common/transforms.h> 9 10 #include <pcl/registration/icp.h> 11 12 #include <iostream> 13 #include <time.h> 14 #include <algorithm> 15 #include <Eigen/Dense> 16 #include <valarray> 17 #include <random> 18 #include <vector> 19 #include <set> 20 21 //二维点转平面 22 void toProjecImage(float& x, float& y, unsigned int& rows, unsigned int& cols) // 640X640的图像 23 { 24 // int nthrds = 4; 25 // omp_set_num_threads(nthrds); 26 //#pragma omp parallel for 27 /*int rows = 0; 28 int cols = 0;*/ 29 30 if (x >= -16.0 && x<0.0) 31 { 32 cols = (unsigned int)((x + 16) * 20); 33 if (y >= -16.0 && y<0.0) 34 { 35 rows = (unsigned int)(y*(-20)) + 320; 36 } 37 if (y >= 0.0 && y <= 16.0) 38 { 39 rows = (unsigned int)((16 - y) * 20); 40 } 41 } 42 else if (x >= 0.0 && x <= 16.0) 43 { 44 cols = (unsigned int)((x) * 20) + 320; 45 if (y >= -16.0 && y<0.0) 46 { 47 rows = (unsigned int)(y*(-20)) + 320; 48 } 49 if (y >= 0.0 && y <= 16.0) 50 { 51 rows = (unsigned int)((16 - y) * 20); 52 } 53 } 54 } 55 56 //输入一系列点的坐标，输出这些点所拟合的线的k b值 最小二乘法 57 std::vector<double> LineFitting(std::vector<db::Point2f> &rPoints) 58 { 59 // y = Ax + B，根据最小二乘法求出A，B 60 std::vector<double > resLine(2); 61 int size = rPoints.size(); 62 float *x = new float[size]; 63 float *y = new float[size]; 64 float A=1.0, B=0.0; 65 float xmean = 0.0f; 66 float ymean = 0.0f; 67 68 for (int i = 0; i < size; i++) 69 { 70 x[i] = rPoints[i].x; 71 y[i] = rPoints[i].y; 72 } 73 74 for (int i = 0; i < size; i++) 75 { 76 xmean += x[i]; 77 ymean += y[i]; 78 } 79 xmean /= size; 80 ymean /= size; 81 float sumx2 = 0.0f; 82 float sumxy = 0.0f; 83 for (int i = 0; i < size; i++) 84 { 85 sumx2 += (x[i] - xmean) * (x[i] - xmean); 86 sumxy += (y[i] - ymean) * (x[i] - xmean); 87 } 88 89 if (sumx2!=0) 90 { 91 A = sumxy / sumx2; 92 B = ymean - A*xmean; 93 } 94 else 95 { 96 A = 1.0; 97 B = 0.0; 98 } 99 100 resLine[0] = A; 101 resLine[1] = B; 102 return resLine; 103 } 104 std::vector<double> leastSquareFitting(std::vector<db::Point2f> &rPoints) 105 { 106 std::vector<double > resLine(2); 107 int num_points = rPoints.size(); 108 std::valarray<float> data_x(num_points); 109 std::valarray<float> data_y(num_points); 110 for (int i = 0; i < num_points; i++) 111 { 112 data_x[i] = rPoints[i].x; 113 data_y[i] = rPoints[i].y; 114 } 115 float A = 0.0; 116 float B = 0.0; 117 float C = 0.0; 118 float D = 0.0; 119 A = (data_x*data_x).sum(); 120 B = data_x.sum(); 121 C = (data_x*data_y).sum(); 122 D = data_y.sum(); 123 float k, b, tmp = 0; 124 if (tmp = (A*data_x.size() - B*B)) //temp！=0 125 { 126 k = (C*data_x.size() - B*D) / tmp; 127 b = (A*D - C*B) / tmp; 128 } 129 else 130 { 131 k = 1; 132 b = 0; 133 } 134 resLine[0] = k; 135 resLine[1] = b; 136 return resLine; 137 } 138 139 //已知直线方程 线外一点 求其垂足 140 db::Point2f getFootPoint(std::vector<double>& kb, const db::Point2f& P1) 141 { 142 db::Point2f FootPoint; 143 double x = (kb[0]* P1.y+ P1.x- kb[0]*kb[1])/(1+ kb[0]* kb[0]); 144 double y = x*kb[0] + kb[1]; 145 FootPoint.x = (float)x; 146 FootPoint.y = (float)y; 147 148 return FootPoint; 149 } 150 //已知直线方程 线外一点 求点到直线距离 151 double getPointToline_Distance(std::vector<double>& kb, const db::Point2f& P1) 152 { 153 db::Point2f pointtemp = getFootPoint(kb, P1); 154 double distance=0.0; 155 distance = sqrt((pointtemp.x - P1.x)*(pointtemp.x - P1.x) + (pointtemp.y - P1.y)*(pointtemp.y - P1.y)); 156 return distance; 157 } 158 //求平面 两点间距离 159 double getPointToPoint_Distance(const db::Point2f& P1, const db::Point2f& P2) 160 { 161 double distance = 0.0; 162 distance = sqrt((P2.x - P1.x)*(P2.x - P1.x) + (P2.y - P1.y)*(P2.y - P1.y)); 163 return distance; 164 } 165 166 //获取pointset中的直线段 167 void getLinesFromPointset(const std::vector<db::Point2f> &rPoints, std::vector<db::line> &lineSets) //2018.11.7 修正 168 { 169 170 if (rPoints.size()<3) 171 { 172 std::cout << "point cloud have NuLL data !!!" << std::endl; 173 return; 174 } 175 176 double avg = 0.0,sum=0.0; 177 std::vector<double> kb; 178 std::vector<db::Point2f> tempPoints; 179 db::line tempLine; 180 std::vector<double> kb1; 181 182 std::vector<double> kb2; 183 std::vector<double> kb3; 184 185 for (size_t i = 0; i < rPoints.size(); i++) 186 { 187 if (tempPoints.size()==0) 188 { 189 if (i == 0) 190 { 191 double distance_0 = getPointToPoint_Distance(rPoints[i], rPoints[i + 1]); 192 if (distance_0 <0.5) 193 { 194 tempPoints.push_back(rPoints[i]); 195 196 tempLine.Points.push_back(rPoints[i]); 197 tempLine.PointsIndex.push_back(i); 198 } 199 continue; 200 } 201 if (i == rPoints.size()-1) 202 { 203 double distance_0 = getPointToPoint_Distance(rPoints[i], rPoints[i -1]); 204 if (distance_0 <0.5) 205 { 206 tempPoints.push_back(rPoints[i]); 207 208 tempLine.Points.push_back(rPoints[i]); 209 tempLine.PointsIndex.push_back(i); 210 } 211 continue; 212 } 213 214 //判断孤点 215 double distance_1 = getPointToPoint_Distance(rPoints[i], rPoints[i - 1]); 216 double distance_2 = getPointToPoint_Distance(rPoints[i], rPoints[i + 1]); 217 if (distance_1<0.5 || distance_2<0.5) 218 { 219 tempPoints.push_back(rPoints[i]); 220 221 tempLine.Points.push_back(rPoints[i]); 222 tempLine.PointsIndex.push_back(i); 223 } 224 225 continue; 226 } 227 else 228 { 229 tempPoints.push_back(rPoints[i]); 230 } 231 232 if (tempPoints.size() < 4) 233 { 234 double distance_1 = getPointToPoint_Distance(rPoints[i], rPoints[i - 1]); 235 if (distance_1>0.5) 236 { 237 tempLine.Points.swap(std::vector<db::Point2f>()); 238 tempLine.PointsIndex.swap(std::vector<int>()); 239 tempLine.kb.swap(std::vector<double>()); 240 241 tempPoints.swap(std::vector<db::Point2f>()); 242 continue; 243 } 244 kb = leastSquareFitting(tempPoints); 245 kb1 = kb; 246 kb2 = kb1; 247 kb3 = kb1; 248 249 tempLine.Points.push_back(rPoints[i]); 250 tempLine.PointsIndex.push_back(i); 251 continue; 252 } 253 254 //点到直线的距离 255 double dis_1 = getPointToline_Distance(kb1, rPoints[i]); 256 double dis_2 = getPointToline_Distance(kb2, rPoints[i]); 257 double dis_3 = getPointToline_Distance(kb3, rPoints[i]); 258 259 double distance_2 = getPointToPoint_Distance(rPoints[i], rPoints[i - 1]); 260 261 if (dis_1 > 0.05|| distance_2 > 0.5) //偏差较大的 点20cm 262 { 263 //sum = 0.0; 264 if (i!=0) 265 i--; 266 267 tempLine.kb = kb1; 268 if (atan(tempLine.kb[0]) >= 0) 269 tempLine.sigma = atan(tempLine.kb[0]); //0-180度 270 else 271 tempLine.sigma = atan(tempLine.kb[0]) + 3.1415926; 272 273 lineSets.push_back(tempLine); 274 275 tempLine.Points.swap(std::vector<db::Point2f>()); 276 tempLine.PointsIndex.swap(std::vector<int>()); 277 tempLine.kb.swap(std::vector<double>()); 278 279 tempPoints.swap(std::vector<db::Point2f>()); 280 } 281 else 282 { 283 if (dis_2 > 0.08) 284 { 285 if (i != 0) 286 i--; 287 288 tempLine.kb = kb2; 289 if (atan(tempLine.kb[0]) >= 0) 290 tempLine.sigma = atan(tempLine.kb[0]); //0-180度 291 else 292 tempLine.sigma = atan(tempLine.kb[0]) + 3.1415926; 293 294 lineSets.push_back(tempLine); 295 296 tempLine.Points.swap(std::vector<db::Point2f>()); 297 tempLine.PointsIndex.swap(std::vector<int>()); 298 tempLine.kb.swap(std::vector<double>()); 299 300 tempPoints.swap(std::vector<db::Point2f>()); 301 continue; 302 } 303 if (dis_3 > 0.08) 304 { 305 if (i != 0) 306 i--; 307 308 tempLine.kb = kb3; 309 if (atan(tempLine.kb[0]) >= 0) 310 tempLine.sigma = atan(tempLine.kb[0]); //0-180度 311 else 312 tempLine.sigma = atan(tempLine.kb[0]) + 3.1415926; 313 314 lineSets.push_back(tempLine); 315 316 tempLine.Points.swap(std::vector<db::Point2f>()); 317 tempLine.PointsIndex.swap(std::vector<int>()); 318 tempLine.kb.swap(std::vector<double>()); 319 320 tempPoints.swap(std::vector<db::Point2f>()); 321 continue; 322 } 323 kb = LineFitting(tempPoints); 324 //kb = leastSquareFitting(tempPoints); 325 kb3 = kb2; 326 kb2 = kb1; 327 kb1 = kb; 328 329 tempLine.Points.push_back(rPoints[i]); 330 tempLine.PointsIndex.push_back(i); 331 332 if (i == (rPoints.size() - 1)) 333 { 334 tempLine.kb = kb1; 335 if (atan(tempLine.kb[0]) >= 0) 336 tempLine.sigma = atan(tempLine.kb[0]); //0-180度 337 else 338 tempLine.sigma = atan(tempLine.kb[0]) + 3.1415926; 339 340 lineSets.push_back(tempLine); 341 } 342 } 343 } 344 return; 345 } 346 347 void getLinesFromPointset2(const std::vector<db::Point2f> &rPoints, std::vector<db::line> &lineSets) 348 { 349 //std::vector<db::line> lineSets; 350 double avg = 0.0, sum = 0.0; 351 std::vector<double> kb; 352 std::vector<db::Point2f> tempPoints; 353 db::line tempLine; 354 std::vector<double> tempkb; 355 356 //利用最小二乘拟合直线 357 for (size_t i = 0; i < rPoints.size(); i++) 358 { 359 if (tempPoints.size()<5) 360 { 361 tempPoints.push_back(rPoints[i]); 362 363 tempLine.Points.push_back(rPoints[i]); 364 tempLine.PointsIndex.push_back(i); 365 continue; 366 } 367 tempPoints.push_back(rPoints[i]); 368 kb = leastSquareFitting(tempPoints); 369 370 //残差 371 double def = 0.0; 372 if (tempPoints.size()==6) 373 { 374 def = abs(kb[0] * rPoints[i - 5].x + kb[1] - rPoints[i].y); //点到直线的位置 375 sum += def; 376 def = abs(kb[0] * rPoints[i - 4].x + kb[1] - rPoints[i].y); //点到直线的位置 377 sum += def; 378 def = abs(kb[0] * rPoints[i - 3].x + kb[1] - rPoints[i].y); //点到直线的位置 379 sum += def; 380 def = abs(kb[0] * rPoints[i-2].x + kb[1] - rPoints[i].y); //点到直线的位置 381 sum += def; 382 def = abs(kb[0] * rPoints[i-1].x + kb[1] - rPoints[i].y); //点到直线的位置 383 sum += def; 384 } 385 386 def = abs(kb[0] * rPoints[i].x + kb[1] - rPoints[i].y); //点到直线的位置 387 sum += def; 388 avg = sum / tempPoints.size(); 389 390 if (1.2* avg < def) //偏差较大的 点 391 { 392 sum = 0.0; 393 tempPoints.swap(std::vector<db::Point2f>()); 394 //tempPoints.push_back(rPoints[i]); 395 if (i != 0) 396 i--; 397 398 if (tempkb.size()==0) 399 { 400 tempLine.kb = kb; 401 } 402 else 403 { 404 tempLine.kb = tempkb; 405 } 406 407 if (atan(tempLine.kb[0]) >= 0) 408 tempLine.sigma = atan(tempLine.kb[0]); //0-180度 409 else 410 tempLine.sigma = atan(tempLine.kb[0]) + 3.1415926; 411 412 lineSets.push_back(tempLine); 413 414 tempLine.Points.swap(std::vector<db::Point2f>()); 415 tempLine.PointsIndex.swap(std::vector<int>()); 416 tempLine.kb.swap(std::vector<double>()); 417 } 418 else 419 { 420 tempkb = kb; 421 tempLine.Points.push_back(rPoints[i]); 422 tempLine.PointsIndex.push_back(i); 423 424 if (i == (rPoints.size() - 1)) 425 { 426 tempLine.kb = tempkb; 427 if (atan(tempLine.kb[0]) >= 0) 428 tempLine.sigma = atan(tempLine.kb[0]); //0-180度 429 else 430 tempLine.sigma = atan(tempLine.kb[0]) + 3.1415926; 431 432 lineSets.push_back(tempLine); 433 } 434 } 435 } 436 437 } 438 439 // 利用ransac 进行直线拟合 440 int ransacLiner_2( 441 std::vector<db::Point2f>& pstData, 442 int minCnt, 443 double maxIterCnt, 444 int consensusCntThreshold, 445 double maxErrorThreshod, 446 double& k, double& b, 447 double& modelMeanError) 448 { 449 int dataCnt = pstData.size(); 450 if (dataCnt<2) 451 { 452 return -1; 453 } 454 455 default_random_engine rng; 456 uniform_int_distribution<unsigned> uniform(0, dataCnt - 1); 457 rng.seed(10); // 固定随机数种子 458 set<unsigned int> selectIndexs; // 选择的点的索引 459 vector<db::Point2f> selectPoints; // 选择的点 460 set<unsigned int> consensusIndexs; // 满足一致性的点的索引 461 462 double A = 0; 463 double B = 0; 464 double C = 0; 465 modelMeanError = 0; 466 int isNonFind = 1; 467 unsigned int bestConsensusCnt = 0; // 满足一致性估计的点的个数 468 int iter = 0; 469 while (iter < maxIterCnt) 470 { 471 selectIndexs.clear(); 472 selectPoints.clear(); 473 // Step1: 随机选择minCnt个点 474 while (1) 475 { 476 unsigned int index = uniform(rng); 477 selectIndexs.insert(index); 478 if (selectIndexs.size() == minCnt) 479 { 480 break; 481 } 482 } 483 // Step2: 进行模型参数估计 (y2 - y1)*x - (x2 - x1)*y + (y2 - y1)x2 - (x2 - x1)y2= 0 484 set<unsigned int>::iterator selectIter = selectIndexs.begin(); 485 while (selectIter != selectIndexs.end()) 486 { 487 unsigned int index = *selectIter; 488 selectPoints.push_back(pstData[index]); 489 selectIter++; 490 } 491 double deltaY = (selectPoints[1]).y - (selectPoints[0]).y; 492 double deltaX = (selectPoints[1]).x - (selectPoints[0]).x; 493 A = deltaY; 494 B = -deltaX; 495 C = -deltaY * (selectPoints[1]).x + deltaX * (selectPoints[1]).y; 496 // Step3: 进行模型评估: 点到直线的距离 497 int dataIter = 0; 498 double meanError = 0; 499 set<unsigned int> tmpConsensusIndexs; 500 while (dataIter < dataCnt) 501 { 502 double distance = (A * pstData[dataIter].x + B * pstData[dataIter].y + C) / sqrt(A*A + B*B); 503 distance = distance > 0 ? distance : -distance; 504 if (distance < maxErrorThreshod) 505 { 506 tmpConsensusIndexs.insert(dataIter); 507 } 508 meanError += distance; 509 dataIter++; 510 } 511 // Step4: 判断一致性: 满足一致性集合的最小元素个数条件 + 至少比上一次的好 512 if (tmpConsensusIndexs.size() >= bestConsensusCnt && tmpConsensusIndexs.size() >= consensusCntThreshold) 513 { 514 bestConsensusCnt = consensusIndexs.size(); // 更新一致性索引集合元素个数 515 modelMeanError = meanError / dataCnt; 516 consensusIndexs.clear(); 517 consensusIndexs = tmpConsensusIndexs; // 更新一致性索引集合 518 isNonFind = 0; 519 } 520 iter++; 521 522 if (B != 0) 523 { 524 k = -A / B; 525 b = -C / B; 526 } 527 else 528 { 529 k = 1000000; 530 b = -C; 531 } 532 } 533 return isNonFind; 534 } 535 // 利用ransac 进行直线拟合 获取直线 536 void getLinesFromPointset_ransac(const std::vector<db::Point2f> &rPoints, std::vector<db::line> &lineSets) 537 { 538 std::vector<db::Point2f> pointTemp; 539 int num = 0, numTemp=0; 540 double k=0.0, b=0.0; 541 std::vector<double> kb; 542 543 while(numTemp < rPoints.size()) 544 { 545 std::vector<double> kbTemp; 546 for (size_t step = 5; step < rPoints.size(); ) 547 { 548 pointTemp.swap(std::vector<db::Point2f>()); 549 //1、进行点集分段（判断条件距离d，步长10 、20、 30、...） 550 int iTemp; 551 if ((num + step)<rPoints.size()) 552 { 553 for (size_t i = num; i < (num + step); i++) 554 { 555 pointTemp.push_back(rPoints[i]); 556 iTemp = i; 557 } 558 double meanError; 559 if (!ransacLiner_2(pointTemp, 2, 50, 35, 0.1, k, b, meanError)) 560 { 561 kbTemp.push_back(k); 562 kbTemp.push_back(b); 563 double PL_Distance = getPointToline_Distance(kbTemp, pointTemp[pointTemp.size() - 1]); 564 565 if (PL_Distance>0.3) //0.3 30cm 断开 566 { 567 db::line Ltemp; 568 for (size_t j = 0; j < pointTemp.size() - 3; j++) 569 { 570 Ltemp.Points.push_back(pointTemp[j]); 571 Ltemp.PointsIndex.push_back(num + j); 572 } 573 Ltemp.kb = kbTemp; 574 lineSets.push_back(Ltemp); 575 576 num = num + step - 3; 577 break; 578 } 579 else 580 { 581 kb = kbTemp; 582 step += 5; 583 } 584 585 } 586 else 587 { 588 step += 5; 589 } 590 } 591 else { 592 numTemp = num + step; 593 break; 594 } 595 } 596 } 597 598 } 599 600 //直线段的优化处理 合并零散线段 601 int reprocessLineset(std::vector<db::line> &lineSets, std::vector<db::line>& finalLineSets) 602 { 603 if(lineSets.size()==0) 604 return -1; 605 606 std::vector<db::line> templineSets; 607 std::vector<db::line> temp; 608 bool errorline = false; 609 610 for (size_t i = 0; i < lineSets.size(); i++) 611 { 612 if (lineSets[i].Points.size() < 6) 613 { 614 temp.push_back(lineSets[i]); 615 errorline = true; 616 } 617 else 618 { 619 if (errorline && temp.size()>0) 620 { 621 db::line templine; 622 double k=0.0, b=0.0, sigma=0.0; 623 for (size_t j = 0; j < temp.size(); j++) 624 { 625 templine.Points.insert(templine.Points.end(), temp[j].Points.begin(), temp[j].Points.end()); 626 templine.PointsIndex.insert(templine.PointsIndex.end(), temp[j].PointsIndex.begin(), temp[j].PointsIndex.end()); 627 k += temp[j].kb[0]; 628 b += temp[j].kb[1]; 629 sigma += temp[j].sigma; 630 } 631 templine.kb.push_back(k/ temp.size()); 632 templine.kb.push_back(b / temp.size()); 633 templine.sigma = sigma / temp.size(); 634 635 finalLineSets.push_back(templine); 636 637 errorline = false; 638 temp.swap(std::vector<db::line>()); 639 } 640 finalLineSets.push_back(lineSets[i]); 641 } 642 } 643 } 644 int reprocessLineset2(std::vector<db::line> &lineSets, std::vector<db::line>& finalLineSets) 645 { 646 if (lineSets.size()==0) 647 { 648 return -1; 649 } 650 //合并近似平行直线 651 std::vector<db::line> lineSets_temp; 652 double distance_R = 100.0; 653 for (size_t i = 0; i < lineSets.size(); i++) 654 { 655 double distance_L=100.0; 656 if (i!= (lineSets.size()-1)) 657 { 658 distance_L = sqrt( 659 (lineSets[i].LRPoints.endPoint.x - lineSets[i + 1].LRPoints.firstPoint.x) 660 *(lineSets[i].LRPoints.endPoint.x - lineSets[i + 1].LRPoints.firstPoint.x) 661 + (lineSets[i].LRPoints.endPoint.y - lineSets[i + 1].LRPoints.firstPoint.y) 662 *(lineSets[i].LRPoints.endPoint.y - lineSets[i + 1].LRPoints.firstPoint.y)); 663 } 664 665 if ((lineSets[i].LRangle.leftangle == 0 && distance_L <= 0.5)|| 666 (lineSets[i].LRangle.rightangle == 0 && distance_R <= 0.5)) 667 { 668 lineSets_temp.push_back(lineSets[i]); 669 } 670 else 671 { 672 if (lineSets_temp.size()>0) 673 { 674 db::line templine; 675 double k = 0.0, b = 0.0, sigma = 0.0; 676 for (size_t j = 0; j < lineSets_temp.size(); j++) 677 { 678 templine.Points.insert(templine.Points.end(), lineSets_temp[j].Points.begin(), lineSets_temp[j].Points.end()); 679 templine.PointsIndex.insert(templine.PointsIndex.end(), lineSets_temp[j].PointsIndex.begin(), lineSets_temp[j].PointsIndex.end()); 680 k += lineSets_temp[j].kb[0]; 681 b += lineSets_temp[j].kb[1]; 682 sigma += lineSets_temp[j].sigma; 683 } 684 templine.kb.push_back(k / lineSets_temp.size()); 685 templine.kb.push_back(b / lineSets_temp.size()); 686 templine.sigma = sigma / lineSets_temp.size(); 687 688 finalLineSets.push_back(templine); 689 690 lineSets_temp.swap(std::vector<db::line>()); 691 } 692 693 finalLineSets.push_back(lineSets[i]); 694 } 695 696 distance_R = distance_L; 697 } 698 return 0; 699 } 700 701 // 获取直线KB参数 702 void getLinePara(float& x1, float& y1, float& x2, float& y2, db::LinePara & LP) 703 { 704 double m = 0; 705 // 计算分子 706 m = x2 - x1; 707 if (0 == m) 708 { 709 LP.k = 10000.0; 710 LP.b = y1 - LP.k * x1; 711 } 712 else 713 714 { 715 LP.k = (y2 - y1) / (x2 - x1); 716 LP.b = y1 - LP.k * x1; 717 } 718 719 } 720 721 //求解两个向量的夹角 722 void getTwoVector_angle(double (&a)[2], double (&b)[2], double & sigema) 723 { 724 double ab, a1, b1, cosr; 725 ab = a[0] * b[0] + a[1] * b[1]; 726 a1 = sqrt(a[0] * a[0] + a[1] * a[1]); 727 b1 = sqrt(b[0] * b[0] + b[1] * b[1]); 728 cosr = ab / a1 / b1; 729 sigema = acos(cosr);//* 180 / 3.1415926; 730 } 731 732 //求解两直线的交点 733 db::Point2f getTwoline_cornerPoint(db::line firstline, db::line secondline) 734 { 735 db::Point2f point; 736 point.x = (firstline.kb[1]- secondline.kb[1]) / (firstline.kb[0] - secondline.kb[0]); 737 point.y = firstline.kb[0] * point.x + firstline.kb[1]; 738 return point; 739 } 740 741 //求解一个向量与x轴正方向的逆时针夹角 742 void getVector_Xangle(double (&a)[2], double &sigema) 743 { 744 745 double b[2] = { 1,0 }; 746 if (a[0]>0 && a[1]>=0) 747 { 748 getTwoVector_angle(a, b, sigema); 749 } 750 else if (a[0]<=0 && a[1] >0) 751 { 752 getTwoVector_angle(a, b, sigema); 753 } 754 else if (a[0] < 0 && a[1] <=0) 755 { 756 getTwoVector_angle(a, b, sigema); 757 sigema += 3.1415926; 758 } 759 else if (a[0] >= 0 && a[1] < 0) 760 { 761 getTwoVector_angle(a, b, sigema); 762 sigema += 3.1415926; 763 } 764 } 765 766 // 求解2d 点云中 直线左右相邻夹角 以及左右直线的交点 767 void LRangleFromLines(std::vector<db::line> &lineSets) 768 { 769 //功能：：求解直线的左右夹角特征。 770 //定理：两个向量与X轴正方向的同向夹角，大夹角减去小夹角 一定等于这两个向量的方向夹角 或 夹角+PI。 771 //**** 在数学中斜率夹角是逆时针的，方向射线，左值空间与右值空间 来判断向量的同向性？ 772 //**** 在这里规定 绕原点逆时针 循环 前个直线段i 在后个直线段i+1的左值空间内。 773 774 db::line firstline; 775 db::line secondline; 776 777 for (size_t i = 0; i < lineSets.size(); i++) 778 { 779 if (i==0) 780 { 781 firstline = lineSets[i]; 782 db::Point2f endFootPoint = getFootPoint(firstline.kb, firstline.Points[0]); //第1条直线的第一个点 垂足 783 lineSets[i].LRPoints.firstPoint = endFootPoint; 784 continue; 785 } 786 secondline = lineSets[i]; 787 788 // 判断是否平行 求取交点 789 if (abs(firstline.kb[0]- secondline.kb[0]) > 0.5)//0.5 790 { 791 //交点 792 db::Point2f pointtemp; 793 pointtemp.x = (secondline.kb[1] - firstline.kb[1]) / (firstline.kb[0] - secondline.kb[0]); 794 pointtemp.y = firstline.kb[0] * pointtemp.x + firstline.kb[1]; 795 796 db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - 1]); //第一条直线的最后一个点 垂足 797 db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[0]); //第二条直线的第一个点 垂足 798 799 //判断第二个向量在第一个向量的左值空间 还是右值空间 800 //R L向量 x轴正方向的夹角比较 801 double R[2] = { R_endFootPoint.x - pointtemp.x, R_endFootPoint.y - pointtemp.y }, 802 L[2] = { L_endFootPoint.x - pointtemp.x, L_endFootPoint.y - pointtemp.y }; 803 double R_sigema=0.0; 804 double L_sigema=0.0; //x轴正方向的夹角 805 getVector_Xangle(R, R_sigema); 806 getVector_Xangle(L, L_sigema); 807 808 double sigema_temp=0.0; //(0-PI) 809 if ((L_sigema- R_sigema)>0 && (L_sigema - R_sigema)<3.1415926) 810 { 811 getTwoVector_angle(R, L, sigema_temp); 812 } 813 else 814 { 815 double Ltemp[2]= { pointtemp.x -L_endFootPoint.x, pointtemp.y -L_endFootPoint.y}; 816 getTwoVector_angle(R, Ltemp, sigema_temp); 817 } 818 819 lineSets[i - 1].LRangle.leftangle = sigema_temp; 820 lineSets[i].LRangle.rightangle = sigema_temp; 821 822 lineSets[i - 1].LRPoints.endPoint = pointtemp; 823 lineSets[i].LRPoints.firstPoint = pointtemp; 824 //lineSets[i - 1].LRPoints.endPoint = R_endFootPoint; 825 //lineSets[i].LRPoints.firstPoint = L_endFootPoint; 826 firstline = secondline; 827 } 828 else // 如果前后两条直线近似平行 829 { 830 db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - 1]); //第一条直线的最后一个点 垂足 831 db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[0]); //第二条直线的第一个点 垂足 832 833 lineSets[i - 1].LRPoints.endPoint = R_endFootPoint; 834 lineSets[i].LRPoints.firstPoint = L_endFootPoint; 835 836 firstline = secondline; 837 } 838 839 if (i == (lineSets.size()-1)) 840 { 841 db::Point2f endFootPoint = getFootPoint(secondline.kb, secondline.Points[secondline.Points.size() - 1]); //第1条直线的第一个点 垂足 842 lineSets[i].LRPoints.endPoint = endFootPoint; 843 } 844 } 845 846 // 847 return; 848 } 849 // 2018.11.7 修正 850 void LRangleFromLines2(std::vector<db::line> &lineSets) 851 { 852 //功能：：求解直线的左右夹角特征。 853 //定理：两个向量与X轴正方向的同向夹角，大夹角减去小夹角 一定等于这两个向量的方向夹角 或 夹角+PI。 854 //**** 在数学中斜率夹角是逆时针的，方向射线，左值空间与右值空间 来判断向量的同向性？ 855 //**** 在这里规定 绕原点逆时针 循环 前个直线段i 在后个直线段i+1的左值空间内。 856 857 db::line firstline; 858 db::line secondline; 859 860 for (size_t i = 0; i < lineSets.size(); i++) 861 { 862 if (i == 0) 863 { 864 firstline = lineSets[i]; 865 db::Point2f endFootPoint = getFootPoint(firstline.kb, firstline.Points[0]); //第1条直线的第一个点 垂足 866 lineSets[i].LRPoints.firstPoint = endFootPoint; 867 continue; 868 } 869 secondline = lineSets[i]; 870 871 double distance = getPointToPoint_Distance(firstline.Points[firstline.Points.size()-1], secondline.Points[0]); // 2018.11.7加入 872 bool have_little_line = !((firstline.Points.size() < 6) && (secondline.Points.size() < 6)); 873 // 判断是否平行 求取交点 874 if (abs(firstline.kb[0] - secondline.kb[0]) > 1.0 && distance< 0.3 && have_little_line)//0.5 875 { 876 //交点 877 db::Point2f pointtemp; 878 pointtemp.x = (secondline.kb[1] - firstline.kb[1]) / (firstline.kb[0] - secondline.kb[0]); 879 pointtemp.y = firstline.kb[0] * pointtemp.x + firstline.kb[1]; 880 881 db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - 1]); //第一条直线的最后一个点 垂足 882 db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[0]); //第二条直线的第一个点 垂足 883 884 //判断第二个向量在第一个向量的左值空间 还是右值空间 885 //R L向量 x轴正方向的夹角比较 886 double R[2] = { R_endFootPoint.x - pointtemp.x, R_endFootPoint.y - pointtemp.y }, 887 L[2] = { L_endFootPoint.x - pointtemp.x, L_endFootPoint.y - pointtemp.y }; 888 double R_sigema = 0.0; 889 double L_sigema = 0.0; //x轴正方向的夹角 890 getVector_Xangle(R, R_sigema); 891 getVector_Xangle(L, L_sigema); 892 893 double sigema_temp = 0.0; //(0-PI) 894 if ((L_sigema - R_sigema)>0 && (L_sigema - R_sigema)<3.1415926) 895 { 896 getTwoVector_angle(R, L, sigema_temp); 897 } 898 else 899 { 900 double Ltemp[2] = { pointtemp.x - L_endFootPoint.x, pointtemp.y - L_endFootPoint.y }; 901 getTwoVector_angle(R, Ltemp, sigema_temp); 902 } 903 904 lineSets[i - 1].LRangle.leftangle = sigema_temp; 905 lineSets[i].LRangle.rightangle = sigema_temp; 906 907 lineSets[i - 1].LRPoints.endPoint = pointtemp; 908 lineSets[i].LRPoints.firstPoint = pointtemp; 909 //lineSets[i - 1].LRPoints.endPoint = R_endFootPoint; 910 //lineSets[i].LRPoints.firstPoint = L_endFootPoint; 911 firstline = secondline; 912 } 913 else // 如果前后两条直线近似平行 914 { 915 db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - 1]); //第一条直线的最后一个点 垂足 916 db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[0]); //第二条直线的第一个点 垂足 917 918 lineSets[i - 1].LRPoints.endPoint = R_endFootPoint; 919 lineSets[i].LRPoints.firstPoint = L_endFootPoint; 920 921 firstline = secondline; 922 } 923 924 //db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - 1]); //第一条直线的最后一个点 垂足 925 //db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[0]); //第二条直线的第一个点 垂足 926 //lineSets[i - 1].LRPoints.endPoint = R_endFootPoint; 927 //lineSets[i].LRPoints.firstPoint = L_endFootPoint; 928 //firstline = secondline; 929 930 if (i == (lineSets.size() - 1)) 931 { 932 db::Point2f endFootPoint = getFootPoint(secondline.kb, secondline.Points[secondline.Points.size() - 1]); //第1条直线的第一个点 垂足 933 lineSets[i].LRPoints.endPoint = endFootPoint; 934 } 935 } 936 937 // 938 return; 939 } 940 941 //求解两个平面两向量的旋转角 942 /* 943 * 两个向量之间的旋转角 944 * 首先明确几个数学概念： 945 * 1. 极轴沿逆时针转动的方向是正方向 946 * 2. 两个向量之间的夹角theta， 是指(A^B)/(|A|*|B|) = cos(theta)，0<=theta<=180 度， 而且没有方向之分 947 * 3. 两个向量的旋转角，是指从向量p1开始，逆时针旋转，转到向量p2时，所转过的角度， 范围是 0 ~ 360度 948 * 计算向量p1到p2的旋转角，算法如下： 949 * 首先通过点乘和arccosine的得到两个向量之间的夹角 950 * 然后判断通过差乘来判断两个向量之间的位置关系 951 * 如果p2在p1的逆时针方向, 返回arccose的角度值, 范围0 ~ 180.0(根据右手定理,可以构成正的面积) 952 * 否则返回 360.0 - arecose的值, 返回180到360(根据右手定理,面积为负) 953 */ 954 double getRotateAngle(double x1, double y1, double x2, double y2) 955 956 { 957 const double epsilon = 1.0e-6; 958 const double nyPI = acos(-1.0); 959 double dist, dot, degree, angle; 960 961 // normalize 962 dist = sqrt(x1 * x1 + y1 * y1); 963 x1 /= dist; 964 y1 /= dist; 965 dist = sqrt(x2 * x2 + y2 * y2); 966 x2 /= dist; 967 y2 /= dist; 968 969 // dot product 970 dot = x1 * x2 + y1 * y2; 971 if (fabs(dot - 1.0) <= epsilon) 972 angle = 0.0; 973 else if (fabs(dot + 1.0) <= epsilon) 974 angle = nyPI; 975 else { 976 double cross; 977 angle = acos(dot); 978 //cross product 979 cross = x1 * y2 - x2 * y1; 980 // vector p2 is clockwise from vector p1 981 // with respect to the origin (0.0) 982 if (cross < 0) { 983 angle = 2 * nyPI - angle; //p2在p1的顺时针方向 984 } 985 } 986 //degree = angle * 180.0 / nyPI; 987 return angle; 988 } 989 990 // 变换矩阵估计 2dlidar 匹配 991 void registration_2dLidar( 992 std::vector<db::Point2f>& src_points, 993 std::vector<db::Point2f>& targ_points, 994 Eigen::Matrix<float, 2, 3>& RT, 995 pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, 996 pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_targ) 997 { 998 if (src_points.size()==0|| targ_points.size()==0) 999 { 1000 return; 1001 } 1002 std::vector<db::line> src_lineSets; 1003 std::vector<db::line> src_templineSets; 1004 1005 getLinesFromPointset2(src_points, src_templineSets); 1006 LRangleFromLines(src_templineSets); 1007 //reprocessLineset2(src_lineSets, src_templineSets); 1008 //LRangleFromLines(src_templineSets); 1009 1010 std::vector<db::line> targ_lineSets; 1011 std::vector<db::line> targ_templineSets; 1012 1013 getLinesFromPointset2(targ_points, targ_templineSets); 1014 LRangleFromLines(targ_templineSets); 1015 //reprocessLineset2(targ_lineSets, targ_templineSets); 1016 //LRangleFromLines(targ_templineSets); 1017 1018 //1、寻找最优匹配线段 1019 struct paraLines 1020 { 1021 int src_line; 1022 int targ_line; 1023 int score=0; //if one match 1 two match 2 1024 }; 1025 std::vector<paraLines> matchLines; 1026 bool bestFind = false; 1027 1028 for (size_t i = 0; i < src_templineSets.size(); i++) 1029 { 1030 for (size_t j = 0; j < targ_templineSets.size(); j++) 1031 { 1032 if (src_templineSets[i].LRangle.leftangle!=0.0 && targ_templineSets[j].LRangle.leftangle!=0.0 1033 && abs(src_templineSets[i].LRangle.leftangle - targ_templineSets[j].LRangle.leftangle) < 0.05) //0-PI 1034 { 1035 paraLines ml; 1036 ml.src_line = i; 1037 ml.targ_line = j; 1038 ml.score = 1; 1039 if (src_templineSets[i].LRangle.rightangle !=0.0 && targ_templineSets[j].LRangle.rightangle !=0.0 && 1040 abs(src_templineSets[i].LRangle.rightangle - targ_templineSets[j].LRangle.rightangle) < 0.2) 1041 { 1042 ml.score = 2; 1043 1044 //line corner 1045 db::Point2f firstPoint; 1046 db::Point2f secondPoint; 1047 firstPoint = src_templineSets[i].LRPoints.firstPoint; 1048 secondPoint = src_templineSets[i].LRPoints.endPoint; 1049 float src_distance = sqrt((firstPoint.x - secondPoint.x)*(firstPoint.x - secondPoint.x) + 1050 (firstPoint.y - secondPoint.y)*(firstPoint.y - secondPoint.y)); 1051 1052 firstPoint = src_templineSets[i].LRPoints.firstPoint; 1053 secondPoint = src_templineSets[i].LRPoints.endPoint; 1054 float targ_distance = sqrt((firstPoint.x - secondPoint.x)*(firstPoint.x - secondPoint.x) + 1055 (firstPoint.y - secondPoint.y)*(firstPoint.y - secondPoint.y)); 1056 if ((src_distance- targ_distance)<0.1) //线段长度对比 1057 { 1058 ml.score = 3; 1059 } 1060 } 1061 matchLines.push_back(ml); 1062 } 1063 } 1064 } 1065 1066 if (matchLines.size()==0) 1067 { 1068 std::cout << "cannot find match lines" << std::endl; 1069 return; 1070 } 1071 1072 int numTemp=0; 1073 size_t i_temp; 1074 1075 for (size_t i = 0; i < matchLines.size(); i++) 1076 { 1077 //1、变换矩阵 估计 1078 //db::Point2f src_firtpoint = src_templineSets[matchLines[i].src_line].LRPoints.firstPoint; 1079 db::Point2f src_firtpoint = getFootPoint(src_templineSets[matchLines[i].src_line].kb, src_templineSets[matchLines[i].src_line].Points[0]); 1080 db::Point2f src_secondpoint = src_templineSets[matchLines[i].src_line].LRPoints.endPoint; 1081 db::Point2f firstvector; 1082 firstvector.x = src_firtpoint.x - src_secondpoint.x; 1083 firstvector.y = src_firtpoint.y - src_secondpoint.y; 1084 1085 //db::Point2f targ_firtpoint = targ_templineSets[matchLines[i].targ_line].LRPoints.firstPoint; 1086 db::Point2f targ_firtpoint = getFootPoint(targ_templineSets[matchLines[i].targ_line].kb, targ_templineSets[matchLines[i].targ_line].Points[0]); 1087 db::Point2f targ_secondpoint = targ_templineSets[matchLines[i].targ_line].LRPoints.endPoint; 1088 db::Point2f secondvector; 1089 secondvector.x = targ_firtpoint.x - targ_secondpoint.x; 1090 secondvector.y = targ_firtpoint.y - targ_secondpoint.y; 1091 1092 Eigen::Matrix<float, 2, 3> RT_temp; 1093 double theta=getRotateAngle(firstvector.x, firstvector.y, secondvector.x, secondvector.y); 1094 RT_temp(0, 0) = cos(theta); 1095 RT_temp(0, 1) = -sin(theta); 1096 RT_temp(1, 0) = sin(theta); 1097 RT_temp(1, 1) = cos(theta); 1098 1099 float x0 = src_secondpoint.x*cos(theta) - src_secondpoint.y*sin(theta); 1100 float y0 = src_secondpoint.x*sin(theta) + src_secondpoint.y*cos(theta); 1101 1102 RT_temp(0, 2) = targ_secondpoint.x- x0; 1103 RT_temp(1, 2) = targ_secondpoint.y- y0; 1104 1105 1106 //2、匹配精度结果验证 1107 struct Tnode * root = NULL; 1108 root = build_kdtree(targ_points); //创建kdtree 1109 1110 int num = 0; 1111 std::vector<double> distanceVector; 1112 for (size_t j = 0; j < src_points.size(); j++) 1113 { 1114 db::Point2f target; 1115 target.x = RT_temp(0, 0)*src_points[j].x + RT_temp(0, 1)*src_points[j].y + RT_temp(0, 2); 1116 target.y = RT_temp(1, 0)*src_points[j].x + RT_temp(1, 1)*src_points[j].y + RT_temp(1, 2); 1117 1118 db::Point2f nearestpoint; 1119 double distance = 0.0; 1120 searchNearest(root, target, nearestpoint, distance); //临近搜索 最近点 1121 1122 if (distance<0.3) //统计距离小于30cm的点个数 1123 { 1124 num++; 1125 } 1126 distanceVector.push_back(distance); 1127 } 1128 1129 if (numTemp < num) 1130 { 1131 numTemp = num; 1132 i_temp = i; 1133 RT = RT_temp; 1134 } 1135 } 1136 //i_temp = 1; 1137 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_001(new pcl::PointCloud<pcl::PointXYZ>); 1138 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_002(new pcl::PointCloud<pcl::PointXYZ>); 1139 for (size_t i = 0; i < src_templineSets[matchLines[i_temp].src_line].PointsIndex.size(); i++) 1140 { 1141 pcl::PointXYZ point; 1142 point.x = src_points[src_templineSets[matchLines[i_temp].src_line].PointsIndex[i]].x; 1143 point.y = src_points[src_templineSets[matchLines[i_temp].src_line].PointsIndex[i]].y; 1144 point.z = 1.5; 1145 cloud_001->points.push_back(point); 1146 } 1147 for (size_t i = 0; i < targ_templineSets[matchLines[i_temp].targ_line].PointsIndex.size(); i++) 1148 { 1149 pcl::PointXYZ point; 1150 point.x = targ_points[targ_templineSets[matchLines[i_temp].targ_line].PointsIndex[i]].x; 1151 point.y = targ_points[targ_templineSets[matchLines[i_temp].targ_line].PointsIndex[i]].y; 1152 point.z = 1.5; 1153 cloud_002->points.push_back(point); 1154 } 1155 1156 //显示匹配的直线的点 1157 pcl::visualization::PCLVisualizer viewer; 1158 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> inColorHandler(cloud_in, 255, 255, 255);// 白色 1159 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> targColorHandler(cloud_in, 0, 0, 255);// 白色 1160 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_001Handler(cloud_001, 255, 0, 0); // 红色 1161 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_002Handler(cloud_002, 0, 255, 0); // 红色 1162 viewer.addPointCloud(cloud_targ, targColorHandler, "targ"); 1163 viewer.addPointCloud(cloud_in, inColorHandler, "In"); 1164 viewer.addPointCloud(cloud_001, cloud_001Handler, "001"); 1165 viewer.addPointCloud(cloud_002, cloud_002Handler, "002"); 1166 1167 viewer.addCoordinateSystem(1.0, "cloud", 0); 1168 1169 while (!viewer.wasStopped()) 1170 { 1171 viewer.spinOnce(); 1172 } 1173 return; 1174 } 1175 1176 pcl::PointXYZ _2dpointTo3dPoint(db::Point2f& temp2dpoint) 1177 { 1178 pcl::PointXYZ temp3dpoint; 1179 temp3dpoint.x = temp2dpoint.x; 1180 temp3dpoint.y = temp2dpoint.y; 1181 temp3dpoint.z = 1.5; 1182 return temp3dpoint; 1183 } 1184 1185 void viewLines(std::vector<db::line> &lineSets, pcl::visualization::PCLVisualizer& viewer) 1186 { 1187 for (size_t i = 0; i < lineSets.size(); i++) 1188 { 1189 std::stringstream ss_line; 1190 ss_line << "line_" << i; 1191 pcl::PointXYZ first_point= _2dpointTo3dPoint(lineSets[i].LRPoints.firstPoint); 1192 pcl::PointXYZ end_point = _2dpointTo3dPoint(lineSets[i].LRPoints.endPoint); 1193 1194 viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(first_point, end_point, 0, 255, 0, ss_line.str()); 1195 }; 1196 1197 } 1198 1199 int main() 1200 { 1201 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>); // 原始点云 1202 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_targ(new pcl::PointCloud<pcl::PointXYZ>); // 原始点云 1203 if (pcl::io::loadPCDFile("C:/Users/18148/Desktop/room_test/020.pcd", *cloud_in) == -1) 1204 { 1205 return -1; 1206 } 1207 if (pcl::io::loadPCDFile("C:/Users/18148/Desktop/angelcomm/r2.pcd", *cloud_targ) == -1) 1208 { 1209 return -1; 1210 } 1211 1212 std::vector<db::Point2f> src_points; 1213 std::vector<db::Point2f> targ_points; 1214 Eigen::Matrix<float, 2, 3> RT= Eigen::Matrix<float, 2, 3>::Zero(); 1215 //MatrixXf RT(2,3) 1216 for (size_t i = 0; i < cloud_in->size();) 1217 { 1218 db::Point2f Point2f; 1219 Point2f.x = cloud_in->points[i].x; 1220 Point2f.y = cloud_in->points[i].y; 1221 src_points.push_back(Point2f); 1222 i += 1; 1223 } 1224 for (size_t i = 0; i < cloud_targ->size(); ) 1225 { 1226 db::Point2f Point2f; 1227 Point2f.x = cloud_targ->points[i].x; 1228 Point2f.y = cloud_targ->points[i].y; 1229 targ_points.push_back(Point2f); 1230 i += 1; 1231 } 1232 1233 std::vector<db::line> src_templineSets; 1234 std::vector<db::line> templineSets; 1235 1236 //getLinesFromPointset_ransac(src_points, templineSets); 1237 1238 1239 getLinesFromPointset(src_points, templineSets); 1240 LRangleFromLines2(templineSets); 1241 //reprocessLineset2(templineSets, src_templineSets); 1242 //LRangleFromLines(src_templineSets); 1243 1244 //registration_2dLidar(src_points, targ_points, RT, cloud_in, cloud_targ); 1245 1246 //Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // 单位阵 1247 //transform_1(0, 0) = RT(0, 0); 1248 //transform_1(0, 1) = RT(0, 1); 1249 //transform_1(0, 3) = RT(0, 2); 1250 1251 //transform_1(1, 0) = RT(1, 0); 1252 //transform_1(1, 1) = RT(1, 1); 1253 //transform_1(1, 3) = RT(1, 2); 1254 1255 //transform_1(2, 2) = 1; 1256 //transform_1(3, 3) = 1; 1257 1258 //cout << transform_1 << endl; 1259 // // 执行变换 1260 //pcl::PointCloud<pcl::PointXYZ>::Ptr pPointCloudOut(new pcl::PointCloud<pcl::PointXYZ>()); 1261 //pcl::transformPointCloud(*cloud_in, *pPointCloudOut, transform_1); 1262 1263 //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>()); 1264 1265 1266 //pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; 1267 //icp.setMaxCorrespondenceDistance(30); //设置对应点对之间的最大距离（此值对配准结果影响较大） 1268 //icp.setTransformationEpsilon(1e-10); //设置两次变化矩阵之间的差值（一般设置为1e-10即可）； 1269 //icp.setEuclideanFitnessEpsilon(0.00001); //设置收敛条件是均方误差和小于阈值， 停止迭代 1270 //icp.setMaximumIterations(1000); //设置最大迭代次数iterations=true 1271 //icp.setInputSource(cloud_in); //设置输入的点云 1272 //icp.setInputTarget(cloud_targ); //目标点云 1273 //icp.align(*cloud_icp, transform_1); //匹配后源点云 1274 //Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); //初始化 1275 //if (icp.hasConverged()) //icp.hasConverged ()=1（true）输出变换矩阵的适合性评估 1276 //{ 1277 // std::cout << " ICP has converged, score is " << icp.getFitnessScore() << std::endl; 1278 // transformation_matrix = icp.getFinalTransformation(); //.cast<double>(); 1279 //} 1280 1281 // 3. 可视窗口初始化 点云可视化 1282 pcl::visualization::PCLVisualizer viewer; 1283 viewLines(templineSets, viewer); 1284 1285 //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> inColorHandler(cloud_in, 255, 255, 255);// 白色 1286 //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> targColorHandler(cloud_targ, 0, 0, 255);// 白色 1287 //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outColorHandler(cloud_icp, 230, 20, 20); // 红色 1288 //viewer.addPointCloud(cloud_targ, targColorHandler, "targ"); 1289 //viewer.addPointCloud(cloud_in, inColorHandler, "In"); 1290 //viewer.addPointCloud(cloud_icp, outColorHandler, "Out"); 1291 //viewer.addCoordinateSystem(1.0, "In", 0); 1292 while (!viewer.wasStopped()) 1293 { // 显示，直到‘q’键被按下 1294 viewer.spinOnce(); 1295 } 1296 return 0; 1297 }
总结：实现单线2dlidar 的匹配算法。