| 1432 | } |
| 1433 | |
| 1434 | void STDescManager::BuildMapCovSTD( |
| 1435 | const pcl::PointCloud<pcl::PointXYZLNormal>::Ptr &instance_pc, |
| 1436 | const std::vector<Eigen::Matrix3d, |
| 1437 | Eigen::aligned_allocator<Eigen::Matrix3d>> |
| 1438 | &cluster_cov_vec, |
| 1439 | std::vector<STDesc> &stds_vec) { |
| 1440 | stds_vec.clear(); |
| 1441 | double scale = 1.0 / config_setting_.std_side_resolution_; |
| 1442 | int near_num = config_setting_.descriptor_near_num_; |
| 1443 | double max_dis_threshold = config_setting_.descriptor_max_len_; |
| 1444 | double min_dis_threshold = config_setting_.descriptor_min_len_; |
| 1445 | std::unordered_map<VOXEL_LOC, bool> feat_map; |
| 1446 | pcl::KdTreeFLANN<pcl::PointXYZLNormal>::Ptr kd_tree( |
| 1447 | new pcl::KdTreeFLANN<pcl::PointXYZLNormal>); |
| 1448 | kd_tree->setInputCloud(instance_pc); |
| 1449 | std::vector<int> pointIdxNKNSearch(near_num); |
| 1450 | std::vector<float> pointNKNSquaredDistance(near_num); |
| 1451 | // Search N nearest corner points to form stds. |
| 1452 | for (size_t i = 0; i < instance_pc->size(); i++) { |
| 1453 | pcl::PointXYZLNormal searchPoint = instance_pc->points[i]; |
| 1454 | Eigen::Matrix3d searchCov = cluster_cov_vec[i]; |
| 1455 | if (kd_tree->nearestKSearch(searchPoint, near_num, pointIdxNKNSearch, |
| 1456 | pointNKNSquaredDistance) > 0) { |
| 1457 | for (int m = 1; m < near_num - 1; m++) { |
| 1458 | for (int n = m + 1; n < near_num; n++) { |
| 1459 | pcl::PointXYZLNormal p1 = searchPoint; |
| 1460 | Eigen::Matrix3d cov1 = searchCov; |
| 1461 | pcl::PointXYZLNormal p2 = instance_pc->points[pointIdxNKNSearch[m]]; |
| 1462 | Eigen::Matrix3d cov2 = cluster_cov_vec[pointIdxNKNSearch[m]]; |
| 1463 | pcl::PointXYZLNormal p3 = instance_pc->points[pointIdxNKNSearch[n]]; |
| 1464 | Eigen::Matrix3d cov3 = cluster_cov_vec[pointIdxNKNSearch[n]]; |
| 1465 | Eigen::Vector3d normal_inc1(p1.normal_x - p2.normal_x, |
| 1466 | p1.normal_y - p2.normal_y, |
| 1467 | p1.normal_z - p2.normal_z); |
| 1468 | Eigen::Vector3d normal_inc2(p3.normal_x - p2.normal_x, |
| 1469 | p3.normal_y - p2.normal_y, |
| 1470 | p3.normal_z - p2.normal_z); |
| 1471 | Eigen::Vector3d normal_add1(p1.normal_x + p2.normal_x, |
| 1472 | p1.normal_y + p2.normal_y, |
| 1473 | p1.normal_z + p2.normal_z); |
| 1474 | Eigen::Vector3d normal_add2(p3.normal_x + p2.normal_x, |
| 1475 | p3.normal_y + p2.normal_y, |
| 1476 | p3.normal_z + p2.normal_z); |
| 1477 | double a = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + |
| 1478 | pow(p1.z - p2.z, 2)); |
| 1479 | double b = sqrt(pow(p1.x - p3.x, 2) + pow(p1.y - p3.y, 2) + |
| 1480 | pow(p1.z - p3.z, 2)); |
| 1481 | double c = sqrt(pow(p3.x - p2.x, 2) + pow(p3.y - p2.y, 2) + |
| 1482 | pow(p3.z - p2.z, 2)); |
| 1483 | if (a > max_dis_threshold || b > max_dis_threshold || |
| 1484 | c > max_dis_threshold || a < min_dis_threshold || |
| 1485 | b < min_dis_threshold || c < min_dis_threshold) { |
| 1486 | continue; |
| 1487 | } |
| 1488 | // re-range the vertex by the side length |
| 1489 | double temp; |
| 1490 | Eigen::Vector3d A, B, C; |
| 1491 | Eigen::Matrix3d cA, cB, cC; |
no test coverage detected