| 1803 | } |
| 1804 | |
| 1805 | void STDescManager::BuildSingleScanSTD( |
| 1806 | const pcl::PointCloud<pcl::PointXYZLNormal>::Ptr &instance_pc, |
| 1807 | std::vector<STDesc> &stds_vec) { |
| 1808 | stds_vec.clear(); |
| 1809 | double scale = 1.0 / config_setting_.std_side_resolution_; |
| 1810 | int near_num = config_setting_.descriptor_near_num_; |
| 1811 | double max_dis_threshold = config_setting_.descriptor_max_len_; |
| 1812 | double min_dis_threshold = config_setting_.descriptor_min_len_; |
| 1813 | std::unordered_map<VOXEL_LOC, bool> feat_map; |
| 1814 | pcl::KdTreeFLANN<pcl::PointXYZLNormal>::Ptr kd_tree( |
| 1815 | new pcl::KdTreeFLANN<pcl::PointXYZLNormal>); |
| 1816 | kd_tree->setInputCloud(instance_pc); |
| 1817 | std::vector<int> pointIdxNKNSearch(near_num); |
| 1818 | std::vector<float> pointNKNSquaredDistance(near_num); |
| 1819 | // Search N nearest corner points to form stds. |
| 1820 | for (size_t i = 0; i < instance_pc->size(); i++) { |
| 1821 | pcl::PointXYZLNormal searchPoint = instance_pc->points[i]; |
| 1822 | if (kd_tree->nearestKSearch(searchPoint, near_num, pointIdxNKNSearch, |
| 1823 | pointNKNSquaredDistance) > 0) { |
| 1824 | for (int m = 1; m < near_num - 1; m++) { |
| 1825 | for (int n = m + 1; n < near_num; n++) { |
| 1826 | pcl::PointXYZLNormal p1 = searchPoint; |
| 1827 | pcl::PointXYZLNormal p2 = instance_pc->points[pointIdxNKNSearch[m]]; |
| 1828 | pcl::PointXYZLNormal p3 = instance_pc->points[pointIdxNKNSearch[n]]; |
| 1829 | Eigen::Vector3d normal_inc1(p1.normal_x - p2.normal_x, |
| 1830 | p1.normal_y - p2.normal_y, |
| 1831 | p1.normal_z - p2.normal_z); |
| 1832 | Eigen::Vector3d normal_inc2(p3.normal_x - p2.normal_x, |
| 1833 | p3.normal_y - p2.normal_y, |
| 1834 | p3.normal_z - p2.normal_z); |
| 1835 | Eigen::Vector3d normal_add1(p1.normal_x + p2.normal_x, |
| 1836 | p1.normal_y + p2.normal_y, |
| 1837 | p1.normal_z + p2.normal_z); |
| 1838 | Eigen::Vector3d normal_add2(p3.normal_x + p2.normal_x, |
| 1839 | p3.normal_y + p2.normal_y, |
| 1840 | p3.normal_z + p2.normal_z); |
| 1841 | double a = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + |
| 1842 | pow(p1.z - p2.z, 2)); |
| 1843 | double b = sqrt(pow(p1.x - p3.x, 2) + pow(p1.y - p3.y, 2) + |
| 1844 | pow(p1.z - p3.z, 2)); |
| 1845 | double c = sqrt(pow(p3.x - p2.x, 2) + pow(p3.y - p2.y, 2) + |
| 1846 | pow(p3.z - p2.z, 2)); |
| 1847 | if (a > max_dis_threshold || b > max_dis_threshold || |
| 1848 | c > max_dis_threshold || a < min_dis_threshold || |
| 1849 | b < min_dis_threshold || c < min_dis_threshold) { |
| 1850 | continue; |
| 1851 | } |
| 1852 | // re-range the vertex by the side length |
| 1853 | double temp; |
| 1854 | Eigen::Vector3d A, B, C; |
| 1855 | Eigen::Vector3i l1, l2, l3; |
| 1856 | Eigen::Vector3i l_temp; |
| 1857 | l1 << 1, 2, 0; |
| 1858 | l2 << 1, 0, 3; |
| 1859 | l3 << 0, 2, 3; |
| 1860 | // a < b < c |
| 1861 | if (a > b) { |
| 1862 | temp = a; |
no test coverage detected