| 1254 | } |
| 1255 | |
| 1256 | void STDescManager::BuildMapSTD( |
| 1257 | const pcl::PointCloud<pcl::PointXYZLNormal>::Ptr &instance_pc, |
| 1258 | std::vector<STDesc> &stds_vec) { |
| 1259 | stds_vec.clear(); |
| 1260 | double scale = 1.0 / config_setting_.std_side_resolution_; |
| 1261 | int near_num = config_setting_.descriptor_near_num_; |
| 1262 | double max_dis_threshold = config_setting_.descriptor_max_len_; |
| 1263 | double min_dis_threshold = config_setting_.descriptor_min_len_; |
| 1264 | std::unordered_map<VOXEL_LOC, bool> feat_map; |
| 1265 | pcl::KdTreeFLANN<pcl::PointXYZLNormal>::Ptr kd_tree( |
| 1266 | new pcl::KdTreeFLANN<pcl::PointXYZLNormal>); |
| 1267 | kd_tree->setInputCloud(instance_pc); |
| 1268 | std::vector<int> pointIdxNKNSearch(near_num); |
| 1269 | std::vector<float> pointNKNSquaredDistance(near_num); |
| 1270 | // Search N nearest corner points to form stds. |
| 1271 | for (size_t i = 0; i < instance_pc->size(); i++) { |
| 1272 | pcl::PointXYZLNormal searchPoint = instance_pc->points[i]; |
| 1273 | if (kd_tree->nearestKSearch(searchPoint, near_num, pointIdxNKNSearch, |
| 1274 | pointNKNSquaredDistance) > 0) { |
| 1275 | for (int m = 1; m < near_num - 1; m++) { |
| 1276 | for (int n = m + 1; n < near_num; n++) { |
| 1277 | pcl::PointXYZLNormal p1 = searchPoint; |
| 1278 | pcl::PointXYZLNormal p2 = instance_pc->points[pointIdxNKNSearch[m]]; |
| 1279 | pcl::PointXYZLNormal p3 = instance_pc->points[pointIdxNKNSearch[n]]; |
| 1280 | Eigen::Vector3d normal_inc1(p1.normal_x - p2.normal_x, |
| 1281 | p1.normal_y - p2.normal_y, |
| 1282 | p1.normal_z - p2.normal_z); |
| 1283 | Eigen::Vector3d normal_inc2(p3.normal_x - p2.normal_x, |
| 1284 | p3.normal_y - p2.normal_y, |
| 1285 | p3.normal_z - p2.normal_z); |
| 1286 | Eigen::Vector3d normal_add1(p1.normal_x + p2.normal_x, |
| 1287 | p1.normal_y + p2.normal_y, |
| 1288 | p1.normal_z + p2.normal_z); |
| 1289 | Eigen::Vector3d normal_add2(p3.normal_x + p2.normal_x, |
| 1290 | p3.normal_y + p2.normal_y, |
| 1291 | p3.normal_z + p2.normal_z); |
| 1292 | double a = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + |
| 1293 | pow(p1.z - p2.z, 2)); |
| 1294 | double b = sqrt(pow(p1.x - p3.x, 2) + pow(p1.y - p3.y, 2) + |
| 1295 | pow(p1.z - p3.z, 2)); |
| 1296 | double c = sqrt(pow(p3.x - p2.x, 2) + pow(p3.y - p2.y, 2) + |
| 1297 | pow(p3.z - p2.z, 2)); |
| 1298 | if (a > max_dis_threshold || b > max_dis_threshold || |
| 1299 | c > max_dis_threshold || a < min_dis_threshold || |
| 1300 | b < min_dis_threshold || c < min_dis_threshold) { |
| 1301 | continue; |
| 1302 | } |
| 1303 | // re-range the vertex by the side length |
| 1304 | double temp; |
| 1305 | Eigen::Vector3d A, B, C; |
| 1306 | Eigen::Vector3i l1, l2, l3; |
| 1307 | Eigen::Vector3i l_temp; |
| 1308 | l1 << 1, 2, 0; |
| 1309 | l2 << 1, 0, 3; |
| 1310 | l3 << 0, 2, 3; |
| 1311 | // a < b < c |
| 1312 | if (a > b) { |
| 1313 | temp = a; |
no test coverage detected