| 992 | } |
| 993 | |
| 994 | void STDescManager::extract_corner( |
| 995 | const Eigen::Vector3d &proj_center, const Eigen::Vector3d proj_normal, |
| 996 | const std::vector<Eigen::Vector3d> proj_points, |
| 997 | pcl::PointCloud<pcl::PointXYZINormal>::Ptr &corner_points) { |
| 998 | double resolution = config_setting_.proj_image_resolution_; |
| 999 | double dis_threshold_min = config_setting_.proj_dis_min_; |
| 1000 | double dis_threshold_max = config_setting_.proj_dis_max_; |
| 1001 | double A = proj_normal[0]; |
| 1002 | double B = proj_normal[1]; |
| 1003 | double C = proj_normal[2]; |
| 1004 | double D = -(A * proj_center[0] + B * proj_center[1] + C * proj_center[2]); |
| 1005 | Eigen::Vector3d x_axis(1, 1, 0); |
| 1006 | if (C != 0) { |
| 1007 | x_axis[2] = -(A + B) / C; |
| 1008 | } else if (B != 0) { |
| 1009 | x_axis[1] = -A / B; |
| 1010 | } else { |
| 1011 | x_axis[0] = 0; |
| 1012 | x_axis[1] = 1; |
| 1013 | } |
| 1014 | x_axis.normalize(); |
| 1015 | Eigen::Vector3d y_axis = proj_normal.cross(x_axis); |
| 1016 | y_axis.normalize(); |
| 1017 | double ax = x_axis[0]; |
| 1018 | double bx = x_axis[1]; |
| 1019 | double cx = x_axis[2]; |
| 1020 | double dx = |
| 1021 | -(ax * proj_center[0] + bx * proj_center[1] + cx * proj_center[2]); |
| 1022 | double ay = y_axis[0]; |
| 1023 | double by = y_axis[1]; |
| 1024 | double cy = y_axis[2]; |
| 1025 | double dy = |
| 1026 | -(ay * proj_center[0] + by * proj_center[1] + cy * proj_center[2]); |
| 1027 | std::vector<Eigen::Vector2d> point_list_2d; |
| 1028 | for (size_t i = 0; i < proj_points.size(); i++) { |
| 1029 | double x = proj_points[i][0]; |
| 1030 | double y = proj_points[i][1]; |
| 1031 | double z = proj_points[i][2]; |
| 1032 | double dis = fabs(x * A + y * B + z * C + D); |
| 1033 | if (dis < dis_threshold_min || dis > dis_threshold_max) { |
| 1034 | continue; |
| 1035 | } |
| 1036 | Eigen::Vector3d cur_project; |
| 1037 | |
| 1038 | cur_project[0] = (-A * (B * y + C * z + D) + x * (B * B + C * C)) / |
| 1039 | (A * A + B * B + C * C); |
| 1040 | cur_project[1] = (-B * (A * x + C * z + D) + y * (A * A + C * C)) / |
| 1041 | (A * A + B * B + C * C); |
| 1042 | cur_project[2] = (-C * (A * x + B * y + D) + z * (A * A + B * B)) / |
| 1043 | (A * A + B * B + C * C); |
| 1044 | pcl::PointXYZ p; |
| 1045 | p.x = cur_project[0]; |
| 1046 | p.y = cur_project[1]; |
| 1047 | p.z = cur_project[2]; |
| 1048 | double project_x = |
| 1049 | cur_project[0] * ay + cur_project[1] * by + cur_project[2] * cy + dy; |
| 1050 | double project_y = |
| 1051 | cur_project[0] * ax + cur_project[1] * bx + cur_project[2] * cx + dx; |