| 1955 | } |
| 1956 | |
| 1957 | void STDescManager::build_stdesc( |
| 1958 | const pcl::PointCloud<pcl::PointXYZINormal>::Ptr &corner_points, |
| 1959 | std::vector<STDesc> &stds_vec) { |
| 1960 | stds_vec.clear(); |
| 1961 | double scale = 1.0 / config_setting_.std_side_resolution_; |
| 1962 | int near_num = config_setting_.descriptor_near_num_; |
| 1963 | double max_dis_threshold = config_setting_.descriptor_max_len_; |
| 1964 | double min_dis_threshold = config_setting_.descriptor_min_len_; |
| 1965 | std::unordered_map<VOXEL_LOC, bool> feat_map; |
| 1966 | pcl::KdTreeFLANN<pcl::PointXYZINormal>::Ptr kd_tree( |
| 1967 | new pcl::KdTreeFLANN<pcl::PointXYZINormal>); |
| 1968 | kd_tree->setInputCloud(corner_points); |
| 1969 | std::vector<int> pointIdxNKNSearch(near_num); |
| 1970 | std::vector<float> pointNKNSquaredDistance(near_num); |
| 1971 | // Search N nearest corner points to form stds. |
| 1972 | for (size_t i = 0; i < corner_points->size(); i++) { |
| 1973 | pcl::PointXYZINormal searchPoint = corner_points->points[i]; |
| 1974 | if (kd_tree->nearestKSearch(searchPoint, near_num, pointIdxNKNSearch, |
| 1975 | pointNKNSquaredDistance) > 0) { |
| 1976 | for (int m = 1; m < near_num - 1; m++) { |
| 1977 | for (int n = m + 1; n < near_num; n++) { |
| 1978 | pcl::PointXYZINormal p1 = searchPoint; |
| 1979 | pcl::PointXYZINormal p2 = corner_points->points[pointIdxNKNSearch[m]]; |
| 1980 | pcl::PointXYZINormal p3 = corner_points->points[pointIdxNKNSearch[n]]; |
| 1981 | Eigen::Vector3d normal_inc1(p1.normal_x - p2.normal_x, |
| 1982 | p1.normal_y - p2.normal_y, |
| 1983 | p1.normal_z - p2.normal_z); |
| 1984 | Eigen::Vector3d normal_inc2(p3.normal_x - p2.normal_x, |
| 1985 | p3.normal_y - p2.normal_y, |
| 1986 | p3.normal_z - p2.normal_z); |
| 1987 | Eigen::Vector3d normal_add1(p1.normal_x + p2.normal_x, |
| 1988 | p1.normal_y + p2.normal_y, |
| 1989 | p1.normal_z + p2.normal_z); |
| 1990 | Eigen::Vector3d normal_add2(p3.normal_x + p2.normal_x, |
| 1991 | p3.normal_y + p2.normal_y, |
| 1992 | p3.normal_z + p2.normal_z); |
| 1993 | double a = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + |
| 1994 | pow(p1.z - p2.z, 2)); |
| 1995 | double b = sqrt(pow(p1.x - p3.x, 2) + pow(p1.y - p3.y, 2) + |
| 1996 | pow(p1.z - p3.z, 2)); |
| 1997 | double c = sqrt(pow(p3.x - p2.x, 2) + pow(p3.y - p2.y, 2) + |
| 1998 | pow(p3.z - p2.z, 2)); |
| 1999 | if (a > max_dis_threshold || b > max_dis_threshold || |
| 2000 | c > max_dis_threshold || a < min_dis_threshold || |
| 2001 | b < min_dis_threshold || c < min_dis_threshold) { |
| 2002 | continue; |
| 2003 | } |
| 2004 | // re-range the vertex by the side length |
| 2005 | double temp; |
| 2006 | Eigen::Vector3d A, B, C; |
| 2007 | Eigen::Vector3i l1, l2, l3; |
| 2008 | Eigen::Vector3i l_temp; |
| 2009 | l1 << 1, 2, 0; |
| 2010 | l2 << 1, 0, 3; |
| 2011 | l3 << 0, 2, 3; |
| 2012 | if (a > b) { |
| 2013 | temp = a; |
| 2014 | a = b; |
nothing calls this directly
no test coverage detected