├── README ├── image-20250613153500037.png ├── image-20250613153743410.png └── image-20250613154052218.png ├── README.md ├── superloc_icp_integration.h ├── utils.h └── superloc_icp_integration.cpp /README/image-20250613153500037.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/SuperOdom-M/HEAD/README/image-20250613153500037.png -------------------------------------------------------------------------------- /README/image-20250613153743410.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/SuperOdom-M/HEAD/README/image-20250613153743410.png -------------------------------------------------------------------------------- /README/image-20250613154052218.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/SuperOdom-M/HEAD/README/image-20250613154052218.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SuperOdom-M 2 | Minmal ICP case of SuperOdom for degeneracy detetcion, only depending on Ceres and Eigen. We only consider the plane features. I use this to test my degeneracy registration methods [DCReg](https://github.com/JokerJohn/DCReg) . 3 | 4 | Related package: https://github.com/superxslam/SuperOdom 5 | 6 | ```c++ 7 | #include "superloc_icp_integration.h" 8 | 9 | // 运行SuperLoc ICP 10 | SuperLocICPIntegration::SuperLocResult superloc_result; 11 | auto start_total = std::chrono::high_resolution_clock::now(); 12 | 13 | // 调用SuperLoc ICP，使用默认的plane_resolution = 0.1 14 | bool success = SuperLocICPIntegration::runSuperLocICP( 15 | source_cloud_, 16 | target_cloud_, 17 | initial_matrix, 18 | config_.max_iterations, 19 | config_.search_radius, 20 | superloc_result, 21 | result.final_transform, 22 | result.iteration_data, // 直接使用result的iteration_data 23 | 0.1 // plane_resolution参数，对应原始SuperLoc的localMap.planeRes_ 24 | ); 25 | 26 | auto end_total = std::chrono::high_resolution_clock::now(); 27 | 28 | ``` 29 | 30 | ```cmake 31 | // cmakelist 32 | find_package(Ceres REQUIRED) 33 | find_package(TBB REQUIRED) 34 | target_link_libraries(icp_test_runner2 35 | ${CERES_LIBRARIES} 36 | TBB::tbb 37 | ) 38 | ``` 39 | 40 | You can use the Autodiff function of Ceres to verify the Jacobian is corrected derived. Other configs are keep the same with SuperOdom. 41 | 42 | ![image-20250613153500037](./README/image-20250613153500037.png) 43 | 44 | On my desktop (12th Gen Intel® Core™ i7-12700K × 20), here is a result for self-registration of a cylinder point cloud with initial pose (0.2m, 0.25m, 1.25m, 0 ,0, -50deg) , search radius 1.0m. the evaluation resluts ofd CD and P2P error are based on a given trunced distance of 0.2m. 45 | 46 | ![image-20250613153743410](./README/image-20250613153743410.png) 47 | 48 | Output log shows Z and yaw degenerate: 49 | 50 | ```bash 51 | === Method: SuperLoc === 52 | Detection: SUPERLOC (Feature Observability + Covariance) 53 | Handling: SUPERLOC (Ceres-based SE3 optimization) 54 | ============================ 55 | [ICPContext::setTargetCloud] KdTree built for target cloud with 7562 points. 56 | 57 | [SuperLoc] Starting SuperLoc ICP method... 58 | [SuperLoc] Using Ceres-based SE(3) optimization 59 | [SuperLoc] Parameters: max_iter=30, search_radius=1 60 | [SuperLoc Debug] Initial state - t: [ 0.2 0.25 1.25], q: [ 0 0 -0.422618 0.906308] 61 | Failed to find match for field 'intensity'. 62 | Failed to find match for field 'intensity'. 63 | [SuperLoc Debug] Valid constraints: 3372, avg residual: 0.207057 64 | [SuperLoc Debug] Iter 0, cost change: 94.0026 -> 18.0748, successful steps: 2 65 | [SuperLoc Debug] Parameter change - trans: 1.03507, rot: 0.350189 deg 66 | info Eigen values: 4.61058e-06 5.01252e-06 0.0001393 0.00215127 0.00240307 0.121005 67 | info Eigen values: 0 0 0 0 0 0 68 | 69 | [SuperLoc] === Final Results === 70 | [SuperLoc] Converged: No (after 30 iterations) 71 | [SuperLoc] Final RMSE: 0.10354, Fitness: 0.879004 72 | [SuperLoc] Transform error: trans=0.257706m, rot=50.1974deg 73 | [SuperLoc] P2P RMSE: 0.677702, Chamfer: 0.612426 74 | 75 | [SuperLoc] Feature Observability Analysis: 76 | Translation uncertainty - X: 1.000000, Y: 1.000000, Z: 0.003559 77 | Rotation uncertainty - Roll: 1.000000, Pitch: 1.000000, Yaw: 0.012456 78 | 79 | [SuperLoc] Degeneracy Detection: 80 | Condition numbers - Full: 1.000000, Rot: 1.000000, Trans: 1.000000 81 | Is degenerate: Yes 82 | 83 | [SuperLoc] Iteration History: 84 | Iter RMSE Fitness Corr# TransErr RotErr Time(ms) 85 | 0 0.1035 0.8790 3372 0.2577 50.1974 16.0674 86 | 87 | [SuperLoc] Total time: 25.4843 ms 88 | [SuperLoc] ======================== 89 | ``` 90 | 91 | the aligned cloud 92 | 93 | ![image-20250613154052218](./README/image-20250613154052218.png) 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /superloc_icp_integration.h: -------------------------------------------------------------------------------- 1 | #ifndef CLOUD_MAP_EVALUATION_SUPERLOC_ICP_INTEGRATION_H 2 | #define CLOUD_MAP_EVALUATION_SUPERLOC_ICP_INTEGRATION_H 3 | 4 | #pragma once 5 | 6 | // SuperLoc ICP Integration for icp_so3_test_runner 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | // 添加TBB支持以提升性能 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "utils.h" 23 | 24 | namespace ICPRunner { 25 | 26 | // SuperLoc特有的参数化 - 与原始SuperLoc的PoseLocalParameterization保持一致 27 | class SuperLocPoseParameterization : public ceres::LocalParameterization { 28 | public: 29 | virtual ~SuperLocPoseParameterization() {} 30 | 31 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 32 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 33 | virtual int GlobalSize() const { return 7; } 34 | virtual int LocalSize() const { return 6; } 35 | }; 36 | 37 | // SuperLoc点到平面代价函数 - 与原始SuperLoc的SurfNormAnalyticCostFunction对应 38 | class SuperLocPlaneResidual : public ceres::SizedCostFunction<1, 7> { 39 | public: 40 | SuperLocPlaneResidual(const Eigen::Vector3d &src_point, 41 | const Eigen::Vector3d &tgt_normal, 42 | double d) 43 | : src_point_(src_point), tgt_normal_(tgt_normal), d_(d) {} 44 | 45 | virtual bool Evaluate(double const *const *parameters, 46 | double *residuals, 47 | double **jacobians) const; 48 | 49 | private: 50 | Eigen::Vector3d src_point_; 51 | Eigen::Vector3d tgt_normal_; 52 | double d_; 53 | }; 54 | 55 | // 使用自动微分的版本，用于对比测试 56 | struct SuperLocPlaneResidualAuto { 57 | SuperLocPlaneResidualAuto(const Eigen::Vector3d& src_point, 58 | const Eigen::Vector3d& tgt_normal, 59 | double d) 60 | : src_point_(src_point), tgt_normal_(tgt_normal), d_(d) {} 61 | 62 | template 63 | bool operator()(const T* const pose, T* residuals) const { 64 | // 位姿参数: [x, y, z, qx, qy, qz, qw] 65 | Eigen::Map> t(pose); 66 | Eigen::Quaternion q(pose[6], pose[3], pose[4], pose[5]); 67 | 68 | // 变换源点 69 | Eigen::Matrix src_pt_T = src_point_.cast(); 70 | Eigen::Matrix p_trans = q * src_pt_T + t; 71 | 72 | // 计算残差 73 | Eigen::Matrix normal_T = tgt_normal_.cast(); 74 | residuals[0] = normal_T.dot(p_trans) + T(d_); 75 | 76 | return true; 77 | } 78 | 79 | static ceres::CostFunction* Create(const Eigen::Vector3d& src_point, 80 | const Eigen::Vector3d& tgt_normal, 81 | double d) { 82 | return new ceres::AutoDiffCostFunction( 83 | new SuperLocPlaneResidualAuto(src_point, tgt_normal, d)); 84 | } 85 | 86 | private: 87 | Eigen::Vector3d src_point_; 88 | Eigen::Vector3d tgt_normal_; 89 | double d_; 90 | }; 91 | 92 | // SuperLoc ICP集成类 93 | class SuperLocICPIntegration { 94 | public: 95 | // 特征可观测性枚举 - 与原始SuperLoc保持一致 96 | enum class FeatureObservability { 97 | rx_cross = 0, 98 | neg_rx_cross = 1, 99 | ry_cross = 2, 100 | neg_ry_cross = 3, 101 | rz_cross = 4, 102 | neg_rz_cross = 5, 103 | tx_dot = 6, 104 | ty_dot = 7, 105 | tz_dot = 8 106 | }; 107 | 108 | struct SuperLocResult { 109 | bool converged; 110 | int iterations; 111 | double final_rmse; 112 | double final_fitness; 113 | 114 | // 退化信息 115 | Eigen::Matrix covariance; 116 | Eigen::Matrix degeneracy_mask; 117 | 118 | // 特征可观测性 119 | double uncertainty_x, uncertainty_y, uncertainty_z; 120 | double uncertainty_roll, uncertainty_pitch, uncertainty_yaw; 121 | 122 | // 特征可观测性直方图（用于深入分析） 123 | std::array feature_histogram; 124 | 125 | // 条件数 126 | double cond_full; 127 | double cond_rot; 128 | double cond_trans; 129 | 130 | bool isDegenerate; 131 | }; 132 | 133 | // 运行SuperLoc ICP 134 | static bool runSuperLocICP( 135 | const pcl::PointCloud::Ptr &source, 136 | const pcl::PointCloud::Ptr &target, 137 | const Eigen::Matrix4d &initial_guess, 138 | int max_iterations, 139 | double correspondence_distance, 140 | SuperLocResult &result, 141 | Eigen::Matrix4d &final_transform, 142 | std::vector &iteration_logs, 143 | double plane_resolution = 0.1 // 新增：平面分辨率参数，用于损失函数 144 | ); 145 | 146 | private: 147 | // 内部辅助函数 148 | static void findCorrespondences( 149 | const pcl::PointCloud::Ptr &source, 150 | const pcl::PointCloud::Ptr &target, 151 | const Eigen::Matrix4d &transform, 152 | double max_distance, 153 | std::vector> &correspondences, 154 | std::vector &normals 155 | ); 156 | 157 | // 新增：扩展的对应点查找函数，包含平面参数 158 | static void findCorrespondencesWithNormals( 159 | const pcl::PointCloud::Ptr &source, 160 | const pcl::PointCloud::Ptr &target, 161 | const Eigen::Matrix4d &transform, 162 | double max_distance, 163 | std::vector> &correspondences, 164 | std::vector &normals, 165 | std::vector &plane_d_values 166 | ); 167 | 168 | static void analyzeFeatureObservability( 169 | const pcl::PointCloud::Ptr &source, 170 | const std::vector> &correspondences, 171 | const std::vector &normals, 172 | const Eigen::Matrix4d &transform, 173 | std::array &histogram 174 | ); 175 | 176 | // 新增：详细的特征可观测性分析 - 与原始SuperLoc的FeatureObservabilityAnalysis对应 177 | static void analyzeFeatureObservabilityDetailed( 178 | const pcl::PointCloud::Ptr &source, 179 | const std::vector> &correspondences, 180 | const std::vector &normals, 181 | const Eigen::Matrix4d &transform, 182 | std::array &histogram 183 | ); 184 | 185 | static void computeUncertainties( 186 | const std::array &histogram, 187 | SuperLocResult &result 188 | ); 189 | 190 | // 新增：基于直方图计算不确定性 - 与原始SuperLoc的EstimateLidarUncertainty对应 191 | static void computeUncertaintiesFromHistogram( 192 | const std::array &histogram, 193 | SuperLocResult &result 194 | ); 195 | 196 | static void computeDegeneracyFromCovariance( 197 | const Eigen::Matrix &covariance, 198 | double threshold, 199 | SuperLocResult &result 200 | ); 201 | 202 | // 新增：估计配准误差 - 与原始SuperLoc的EstimateRegistrationError对应 203 | static void EstimateRegistrationError( 204 | ceres::Problem &problem, 205 | const double *pose_parameters, 206 | SuperLocResult &result 207 | ); 208 | 209 | // 新增：检查退化状态 210 | static void checkDegeneracy(SuperLocResult &result); 211 | 212 | static void computePlaneParameters( 213 | const pcl::PointCloud::Ptr &cloud, 214 | const std::vector &indices, 215 | Eigen::Vector3d &normal, 216 | double &d 217 | ); 218 | }; 219 | 220 | } // namespace ICPRunner 221 | 222 | #endif //CLOUD_MAP_EVALUATION_SUPERLOC_ICP_INTEGRATION_H -------------------------------------------------------------------------------- /utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xchu on 12/6/2025. 3 | // 4 | 5 | #ifndef CLOUD_MAP_EVALUATION_UTILS_H 6 | #define CLOUD_MAP_EVALUATION_UTILS_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include // for pcl::rad2deg, pcl::deg2rad 31 | 32 | // Define PointT if not defined elsewhere 33 | // typedef pcl::PointXYZI PointT; // Example: Use PointXYZI 34 | // Or use the one from the original code if it's different and accessible 35 | using PointT = pcl::PointXYZI; // Using PointXYZ as a common default 36 | using namespace Eigen; 37 | namespace fs = std::filesystem; 38 | 39 | namespace ICPRunner { 40 | // Simple Timer (same as before) 41 | class TicToc { 42 | public: 43 | TicToc() { tic(); } 44 | 45 | void tic() { start = std::chrono::system_clock::now(); } 46 | 47 | double toc() { 48 | end = std::chrono::system_clock::now(); 49 | std::chrono::duration elapsed_seconds = end - start; 50 | return elapsed_seconds.count() * 1000; // return ms 51 | } 52 | 53 | private: 54 | std::chrono::time_point start, end; 55 | }; 56 | 57 | // Pose Representation (same as before) 58 | struct Pose6D { 59 | double roll = 0.0, pitch = 0.0, yaw = 0.0; 60 | double x = 0.0, y = 0.0, z = 0.0; 61 | 62 | Pose6D operator+(const Pose6D &other) const { 63 | Pose6D result; 64 | result.roll = roll + other.roll; 65 | result.pitch = pitch + other.pitch; 66 | result.yaw = yaw + other.yaw; 67 | result.x = x + other.x; 68 | result.y = y + other.y; 69 | result.z = z + other.z; 70 | return result; 71 | } 72 | }; 73 | 74 | // Tunable Parameters - will be updated from config 75 | struct ICPParameters { 76 | double DEGENERACY_THRES_COND = 10.0; 77 | double DEGENERACY_THRES_EIG = 120.0; 78 | double KAPPA_TARGET = 1.0; 79 | double PCG_TOLERANCE = 1e-6; 80 | int PCG_MAX_ITER = 10; 81 | double ADAPTIVE_REG_ALPHA = 10.0; 82 | double STD_REG_GAMMA = 0.01; 83 | double LOAM_EIGEN_THRESH = 120.0; 84 | double TSVD_SINGULAR_THRESH = 120.0; 85 | }; 86 | 87 | // Degeneracy Detection Methods 88 | enum class DegeneracyDetectionMethod { 89 | NONE_DETE, SCHUR_CONDITION_NUMBER, FULL_EVD_MIN_EIGENVALUE, 90 | EVD_SUB_CONDITION, FULL_SVD_CONDITION, O3D, SUPERLOC 91 | }; 92 | 93 | // Degeneracy Handling Methods 94 | enum class DegeneracyHandlingMethod { 95 | NONE_HAND, STANDARD_REGULARIZATION, ADAPTIVE_REGULARIZATION, 96 | PRECONDITIONED_CG, SOLUTION_REMAPPING, TRUNCATED_SVD, O3D, SUPERLOC 97 | }; 98 | 99 | 100 | enum class ICPEngine { 101 | CUSTOM_EULER, // 自定义欧拉角实现 102 | CUSTOM_SO3, // 自定义SO(3)实现 103 | OPEN3D // Open3D实现 104 | }; 105 | 106 | 107 | // Configuration structure 108 | struct Config { 109 | // Test configuration 110 | int num_runs = 1; 111 | bool save_pcd = true; 112 | bool save_error_pcd = true; 113 | bool visualize = false; 114 | 115 | // File paths 116 | std::string folder_path; 117 | std::string source_pcd; 118 | std::string target_pcd; 119 | std::string output_folder; 120 | 121 | // ICP parameters 122 | double search_radius = 1.0; 123 | int max_iterations = 30; 124 | double error_threshold = 0.05; // for visualization 125 | 126 | // Initial noise 127 | Pose6D initial_noise; 128 | 129 | // Ground truth 130 | bool use_identity_gt = true; 131 | Pose6D ground_truth; 132 | 133 | // ICP parameter values 134 | ICPParameters icp_params; 135 | 136 | ICPEngine icp_engine = ICPEngine::CUSTOM_EULER; 137 | 138 | // Test methods 139 | std::map > test_methods; 140 | 141 | // Engine selection 142 | bool use_custom_engine = true; 143 | 144 | // Use SO(3) parameterization 145 | bool use_so3_parameterization = true; 146 | }; 147 | 148 | // --- CSV Logging Data Structure --- 149 | struct IterationLogData { 150 | int iter_count = 0; 151 | int effective_points = 0; 152 | double rmse = 0.0; 153 | double fitness = 0.0; 154 | int corr_num = 0; 155 | 156 | double iter_time_ms = 0.0; 157 | double cond_schur_rot = NAN; 158 | double cond_schur_trans = NAN; 159 | double cond_diag_rot = NAN; 160 | double cond_diag_trans = NAN; 161 | double cond_full_evd_sub_rot = NAN; 162 | double cond_full_evd_sub_trans = NAN; 163 | double cond_full_svd = NAN; 164 | double cond_full = NAN; // 直接从H = J^T * J计算的条件数 165 | Eigen::Vector3d lambda_schur_rot = Eigen::Vector3d::Constant(NAN); 166 | Eigen::Vector3d lambda_schur_trans = Eigen::Vector3d::Constant(NAN); 167 | Eigen::Matrix eigenvalues_full = Eigen::Matrix::Constant(NAN); 168 | Eigen::Matrix singular_values_full = Eigen::Matrix::Constant(NAN); 169 | Eigen::Matrix update_dx = Eigen::Matrix::Constant(NAN); 170 | bool is_degenerate = false; 171 | std::vector degenerate_mask = std::vector(6, false); 172 | 173 | Eigen::Matrix4d transform_matrix = Eigen::Matrix4d::Identity(); // 添加变换矩阵 174 | double trans_error_vs_gt = 0.0; // 相对于真值的平移误差 175 | double rot_error_vs_gt = 0.0; // 相对于真值的旋转误差 176 | 177 | // 新增：对角块特征值 178 | Eigen::Vector3d lambda_diag_rot = Eigen::Vector3d::Constant(NAN); 179 | Eigen::Vector3d lambda_diag_trans = Eigen::Vector3d::Constant(NAN); 180 | 181 | // 新增：预处理矩阵 182 | Eigen::Matrix P_preconditioner = Eigen::Matrix::Identity(); 183 | Eigen::Matrix W_adaptive = Eigen::Matrix::Zero(); 184 | 185 | 186 | // 新增：对于Schur+PCG方法的特殊信息 187 | Eigen::Matrix3d aligned_V_rot = Eigen::Matrix3d::Identity(); 188 | Eigen::Matrix3d aligned_V_trans = Eigen::Matrix3d::Identity(); 189 | std::vector rot_indices = {0, 1, 2}; 190 | std::vector trans_indices = {0, 1, 2}; 191 | 192 | // 新增：梯度信息（-J^T * r） 193 | Eigen::Matrix gradient = Eigen::Matrix::Constant(NAN); 194 | 195 | // 新增：目标函数值（0.5 * r^T * r） 196 | double objective_value = NAN; 197 | 198 | }; 199 | 200 | 201 | // Test result structure 202 | struct TestResult { 203 | std::string method_name; 204 | bool converged = false; 205 | int iterations = 0; 206 | double time_ms = 0.0; 207 | double trans_error_m = 0.0; 208 | double rot_error_deg = 0.0; 209 | double final_rmse = 0.0; 210 | double final_fitness = 0.0; 211 | double p2p_rmse = 0.0; 212 | double p2p_fitness = 0.0; 213 | double chamfer_distance = 0.0; 214 | int corr_num = 0; 215 | Eigen::Matrix4d final_transform = Eigen::Matrix4d::Identity(); 216 | 217 | // Degeneracy analysis (for single run) 218 | std::vector condition_numbers; 219 | std::vector eigenvalues; 220 | std::vector singular_values; 221 | std::vector degenerate_mask; 222 | 223 | // Iteration history for plotting 224 | std::vector iter_rmse_history; 225 | std::vector iter_fitness_history; 226 | std::vector iter_corr_num_history; 227 | std::vector iter_trans_error_history; 228 | std::vector iter_rot_error_history; 229 | 230 | // 新增：保存每次迭代的完整数据 231 | std::vector iteration_data; 232 | 233 | // 新增：保存每次迭代的变换矩阵 234 | std::vector iter_transform_history; 235 | 236 | // SuperLoc特有的数据 237 | struct SuperLocData { 238 | bool has_data = false; 239 | double uncertainty_x = 0.0; 240 | double uncertainty_y = 0.0; 241 | double uncertainty_z = 0.0; 242 | double uncertainty_roll = 0.0; 243 | double uncertainty_pitch = 0.0; 244 | double uncertainty_yaw = 0.0; 245 | double cond_full = 0.0; 246 | double cond_rot = 0.0; 247 | double cond_trans = 0.0; 248 | bool is_degenerate = false; 249 | Eigen::Matrix covariance = Eigen::Matrix::Identity(); 250 | std::array feature_histogram = {0, 0, 0, 0, 0, 0, 0, 0, 0}; // 新增：特征可观测性直方图 251 | } superloc_data; 252 | }; 253 | 254 | // Statistics for multiple runs 255 | struct MethodStatistics { 256 | std::string method_name; 257 | int total_runs = 0; 258 | int converged_runs = 0; 259 | int corr_num = 0; 260 | 261 | // Mean values 262 | double mean_trans_error = 0.0; 263 | double mean_rot_error = 0.0; 264 | double mean_time_ms = 0.0; 265 | double mean_iterations = 0.0; 266 | double mean_rmse = 0.0; 267 | double mean_fitness = 0.0; 268 | double mean_p2p_rmse = 0.0; 269 | double mean_p2p_fitness = 0.0; 270 | double mean_chamfer = 0.0; 271 | 272 | // Standard deviations 273 | double std_trans_error = 0.0; 274 | double std_rot_error = 0.0; 275 | double std_time_ms = 0.0; 276 | 277 | // Min/Max values 278 | double min_trans_error = std::numeric_limits::max(); 279 | double max_trans_error = 0.0; 280 | double min_rot_error = std::numeric_limits::max(); 281 | double max_rot_error = 0.0; 282 | 283 | // Success rate 284 | double success_rate = 0.0; 285 | }; 286 | 287 | 288 | // --- ICP State Context --- 289 | // Holds variables that persist across calls or are needed internally by ICP 290 | struct ICPContext { 291 | // Pointers to clouds used internally 292 | pcl::PointCloud::Ptr laserCloudEffective; 293 | pcl::PointCloud::Ptr coeffSel; // Stores weighted normal (xyz) and residual (intensity) 294 | 295 | // Internal state vectors (resized based on input cloud) 296 | std::vector laserCloudOriSurfVec; 297 | std::vector coeffSelSurfVec; 298 | std::vector laserCloudOriSurfFlag; // Use uint8_t for bool efficiency 299 | 300 | // KdTree for the target map 301 | pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap; 302 | 303 | // Result Storage 304 | Eigen::Matrix icp_cov; // Final computed covariance 305 | std::vector iteration_log_data_; // Log data per iteration 306 | double total_icp_time_ms_ = 0.0; 307 | Pose6D final_pose_; // The final optimized pose 308 | bool final_convergence_flag_ = false; 309 | int final_iterations_ = 0; // Store the number of iterations performed 310 | 311 | // Constructor to initialize pointers and default covariance 312 | ICPContext() : 313 | laserCloudEffective(new pcl::PointCloud()), 314 | coeffSel(new pcl::PointCloud()), 315 | kdtreeSurfFromMap(new pcl::KdTreeFLANN()), 316 | icp_cov(Eigen::Matrix::Identity()) { 317 | icp_cov *= 1e6; // Default high covariance 318 | } 319 | 320 | // Prevent copying (pointers would be shallow copied) 321 | ICPContext(const ICPContext &) = delete; 322 | 323 | ICPContext &operator=(const ICPContext &) = delete; 324 | 325 | // Setup method (optional, can be done in runSingleICPTest) 326 | void setTargetCloud(pcl::PointCloud::Ptr target_cloud_ptr) { 327 | if (!target_cloud_ptr || target_cloud_ptr->empty()) { 328 | std::cerr << "[ICPContext::setTargetCloud] Error: Target cloud is null or empty." << std::endl; 329 | return; 330 | } 331 | kdtreeSurfFromMap->setInputCloud(target_cloud_ptr); 332 | std::cout << "[ICPContext::setTargetCloud] KdTree built for target cloud with " 333 | << target_cloud_ptr->size() << " points." << std::endl; 334 | } 335 | }; 336 | 337 | struct DegeneracyAnalysisResult { 338 | bool isDegenerate = false; 339 | std::vector degenerate_mask; 340 | double cond_schur_rot = NAN, cond_schur_trans = NAN; 341 | double cond_diag_rot = NAN, cond_diag_trans = NAN; 342 | double cond_full = NAN; 343 | double cond_full_sub_rot = NAN, cond_full_sub_trans = NAN; 344 | Eigen::Matrix eigenvalues_full; 345 | Eigen::Matrix singular_values; 346 | Eigen::Vector3d lambda_schur_rot = Eigen::Vector3d::Constant(NAN); 347 | Eigen::Vector3d lambda_schur_trans = Eigen::Vector3d::Constant(NAN); 348 | Eigen::Vector3d lambda_sub_rot = Eigen::Vector3d::Constant(NAN); 349 | Eigen::Vector3d lambda_sub_trans = Eigen::Vector3d::Constant(NAN); 350 | Eigen::Matrix3d aligned_V_rot = Eigen::Matrix3d::Identity(); 351 | Eigen::Matrix3d aligned_V_trans = Eigen::Matrix3d::Identity(); 352 | Eigen::Matrix3d schur_V_rot = Eigen::Matrix3d::Identity(); 353 | Eigen::Matrix3d schur_V_trans = Eigen::Matrix3d::Identity(); 354 | std::vector rot_indices = {0, 1, 2}; 355 | std::vector trans_indices = {0, 1, 2}; 356 | Eigen::Matrix W_adaptive = Eigen::Matrix::Zero(); 357 | Eigen::Matrix P_preconditioner = Eigen::Matrix::Identity(); 358 | }; 359 | 360 | } 361 | 362 | #endif //CLOUD_MAP_EVALUATION_UTILS_H 363 | -------------------------------------------------------------------------------- /superloc_icp_integration.cpp: -------------------------------------------------------------------------------- 1 | #include "superloc_icp_integration.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace ICPRunner { 7 | 8 | // SuperLocPoseParameterization实现 9 | bool SuperLocPoseParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const { 10 | // 位置更新 11 | x_plus_delta[0] = x[0] + delta[0]; 12 | x_plus_delta[1] = x[1] + delta[1]; 13 | x_plus_delta[2] = x[2] + delta[2]; 14 | 15 | // 旋转更新 - 与原始SuperLoc保持一致 16 | const double norm_delta = sqrt(delta[3] * delta[3] + delta[4] * delta[4] + delta[5] * delta[5]); 17 | if (norm_delta > 0.0) { 18 | const double sin_delta_by_delta = sin(norm_delta) / norm_delta; 19 | double q_delta[4]; 20 | q_delta[0] = cos(norm_delta); 21 | q_delta[1] = sin_delta_by_delta * delta[3]; 22 | q_delta[2] = sin_delta_by_delta * delta[4]; 23 | q_delta[3] = sin_delta_by_delta * delta[5]; 24 | 25 | // 四元数乘法: q_delta * q_current 26 | ceres::QuaternionProduct(q_delta, x + 3, x_plus_delta + 3); 27 | } else { 28 | x_plus_delta[3] = x[3]; 29 | x_plus_delta[4] = x[4]; 30 | x_plus_delta[5] = x[5]; 31 | x_plus_delta[6] = x[6]; 32 | } 33 | 34 | return true; 35 | } 36 | 37 | bool SuperLocPoseParameterization::ComputeJacobian(const double *x, double *jacobian) const { 38 | Eigen::Map > J(jacobian); 39 | J.setZero(); 40 | 41 | // 位置部分的雅可比 42 | J.topLeftCorner<3, 3>().setIdentity(); 43 | 44 | // 旋转部分的雅可比 45 | const double q_w = x[6]; 46 | const double q_x = x[3]; 47 | const double q_y = x[4]; 48 | const double q_z = x[5]; 49 | 50 | J(3, 3) = 0.5 * q_w; 51 | J(3, 4) = 0.5 * q_z; 52 | J(3, 5) = -0.5 * q_y; 53 | J(4, 3) = -0.5 * q_z; 54 | J(4, 4) = 0.5 * q_w; 55 | J(4, 5) = 0.5 * q_x; 56 | J(5, 3) = 0.5 * q_y; 57 | J(5, 4) = -0.5 * q_x; 58 | J(5, 5) = 0.5 * q_w; 59 | J(6, 3) = -0.5 * q_x; 60 | J(6, 4) = -0.5 * q_y; 61 | J(6, 5) = -0.5 * q_z; 62 | 63 | return true; 64 | } 65 | 66 | // SuperLoc点到平面代价函数 67 | bool SuperLocPlaneResidual::Evaluate(double const *const *parameters, 68 | double *residuals, 69 | double **jacobians) const { 70 | // 获取位姿参数 71 | Eigen::Map t(parameters[0]); 72 | Eigen::Map q(parameters[0] + 3); 73 | 74 | // 变换源点 75 | Eigen::Vector3d p_trans = q * src_point_ + t; 76 | 77 | // 计算残差：点到平面距离 78 | // 注意：这里的d_已经是negative_OA_dot_norm 79 | residuals[0] = tgt_normal_.dot(p_trans) + d_; 80 | 81 | if (jacobians != NULL && jacobians[0] != NULL) { 82 | Eigen::Map > J(jacobians[0]); 83 | 84 | // 对平移的导数 85 | J(0, 0) = tgt_normal_(0); 86 | J(0, 1) = tgt_normal_(1); 87 | J(0, 2) = tgt_normal_(2); 88 | 89 | // 对旋转的导数 - 简化版本 90 | // 使用链式法则: dr/dq = n^T * d(R*p)/dq 91 | // const Eigen::Matrix3d R = q.toRotationMatrix(); 92 | // const Eigen::Vector3d Rp = R * src_point_; 93 | 94 | // 构造[Rp]×矩阵 95 | // Eigen::Matrix3d skew_Rp; 96 | // skew_Rp << 0, -Rp(2), Rp(1), 97 | // Rp(2), 0, -Rp(0), 98 | // -Rp(1), Rp(0), 0; 99 | // 100 | // // 计算 n^T * [Rp]× 101 | // Eigen::RowVector3d nT_skew = tgt_normal_.transpose() * skew_Rp; 102 | // 103 | // // 四元数导数（这是一个近似，但在小角度下足够准确） 104 | // J(0, 3) = nT_skew(0); 105 | // J(0, 4) = nT_skew(1); 106 | // J(0, 5) = nT_skew(2); 107 | // J(0, 6) = 0.0; 108 | 109 | // 四元数旋转公式: q * p * q^(-1) 110 | // 展开后对各个四元数分量求导 111 | 112 | double qw = q.w(); 113 | double qx = q.x(); 114 | double qy = q.y(); 115 | double qz = q.z(); 116 | 117 | double px = src_point_(0); 118 | double py = src_point_(1); 119 | double pz = src_point_(2); 120 | 121 | // 四元数旋转公式: q * p * q^(-1) 122 | // 展开后对各个四元数分量求导 123 | 124 | // ∂(q*p)/∂qx - 修正后的公式 125 | Eigen::Vector3d dp_dqx; 126 | dp_dqx(0) = 2 * (qy * py + qz * pz); 127 | dp_dqx(1) = 2 * (qy * px - 2 * qx * py - qw * pz); 128 | dp_dqx(2) = 2 * (qz * px + qw * py - 2 * qx * pz); 129 | 130 | // ∂(q*p)/∂qy - 修正后的公式 131 | Eigen::Vector3d dp_dqy; 132 | dp_dqy(0) = 2 * (qx * py - 2 * qy * px + qw * pz); 133 | dp_dqy(1) = 2 * (qx * px + qz * pz); 134 | dp_dqy(2) = 2 * (qz * py - qw * px - 2 * qy * pz); 135 | 136 | // ∂(q*p)/∂qz - 修正后的公式 137 | Eigen::Vector3d dp_dqz; 138 | dp_dqz(0) = 2 * (qx * pz - qw * py - 2 * qz * px); 139 | dp_dqz(1) = 2 * (qy * pz + qw * px - 2 * qz * py); 140 | dp_dqz(2) = 2 * (qx * px + qy * py); 141 | 142 | // ∂(q*p)/∂qw - 修正后的公式 143 | Eigen::Vector3d dp_dqw; 144 | dp_dqw(0) = 2 * (qy * pz - qz * py); 145 | dp_dqw(1) = 2 * (qz * px - qx * pz); 146 | dp_dqw(2) = 2 * (qx * py - qy * px); 147 | 148 | // 最终雅可比 = n^T * ∂(q*p)/∂q 149 | J(0, 3) = tgt_normal_.dot(dp_dqx); 150 | J(0, 4) = tgt_normal_.dot(dp_dqy); 151 | J(0, 5) = tgt_normal_.dot(dp_dqz); 152 | J(0, 6) = tgt_normal_.dot(dp_dqw); 153 | } 154 | 155 | return true; 156 | } 157 | 158 | bool SuperLocICPIntegration::runSuperLocICP( 159 | const pcl::PointCloud::Ptr &source, 160 | const pcl::PointCloud::Ptr &target, 161 | const Eigen::Matrix4d &initial_guess, 162 | int max_iterations, 163 | double correspondence_distance, 164 | SuperLocResult &result, 165 | Eigen::Matrix4d &final_transform, 166 | std::vector &iteration_logs, 167 | double plane_resolution) { 168 | 169 | // 初始化结果 170 | result.converged = false; 171 | result.iterations = 0; 172 | result.isDegenerate = false; 173 | final_transform = initial_guess; 174 | 175 | // 初始化位姿参数（7维：x,y,z,qx,qy,qz,qw） 176 | double pose_parameters[7]; 177 | Eigen::Map pose_t(pose_parameters); 178 | Eigen::Map pose_q(pose_parameters + 3); 179 | 180 | // 从初始猜测设置参数 181 | Eigen::Vector3d t = initial_guess.block<3, 1>(0, 3); 182 | Eigen::Quaterniond q(initial_guess.block<3, 3>(0, 0)); 183 | q.normalize(); 184 | 185 | pose_t = t; 186 | pose_q = q; 187 | 188 | // 构建目标点云KD树 189 | pcl::KdTreeFLANN kdtree; 190 | kdtree.setInputCloud(target); 191 | 192 | // 特征可观测性直方图 - 与原始SuperLoc保持一致 193 | std::array PlaneFeatureHistogramObs; 194 | 195 | // ICP主循环 196 | for (int iter = 0; iter < max_iterations; ++iter) { 197 | IterationLogData iter_log; 198 | iter_log.iter_count = iter; 199 | 200 | auto iter_start = std::chrono::high_resolution_clock::now(); 201 | 202 | // 重置特征可观测性直方图 203 | PlaneFeatureHistogramObs.fill(0); 204 | 205 | // 1. 寻找对应点和法向量 206 | std::vector > correspondences; 207 | std::vector normals; 208 | std::vector plane_d_values; 209 | 210 | findCorrespondencesWithNormals(source, target, final_transform, 211 | correspondence_distance, correspondences, 212 | normals, plane_d_values); 213 | 214 | if (correspondences.size() < 10) { 215 | std::cout << "[SuperLoc] Not enough correspondences: " 216 | << correspondences.size() << std::endl; 217 | break; 218 | } 219 | 220 | iter_log.corr_num = correspondences.size(); 221 | 222 | // 2. 分析特征可观测性 - 使用原始SuperLoc的方法 223 | analyzeFeatureObservabilityDetailed(source, correspondences, normals, 224 | final_transform, PlaneFeatureHistogramObs); 225 | 226 | // 3. 构建优化问题 227 | ceres::Problem problem; 228 | ceres::LocalParameterization *pose_parameterization = 229 | new SuperLocPoseParameterization(); 230 | problem.AddParameterBlock(pose_parameters, 7, pose_parameterization); 231 | 232 | // 添加点到平面约束 233 | double total_residual = 0.0; 234 | int valid_constraints = 0; 235 | 236 | for (size_t i = 0; i < correspondences.size(); ++i) { 237 | const auto &corr = correspondences[i]; 238 | Eigen::Vector3d src_pt(source->points[corr.first].x, 239 | source->points[corr.first].y, 240 | source->points[corr.first].z); 241 | 242 | // 使用自动微分版本进行测试 243 | bool use_autodiff = false; // 可以切换测试 244 | ceres::CostFunction *cost_function = nullptr; 245 | 246 | if (use_autodiff) { 247 | cost_function = SuperLocPlaneResidualAuto::Create(src_pt, normals[i], plane_d_values[i]); 248 | } else { 249 | cost_function = new SuperLocPlaneResidual(src_pt, normals[i], plane_d_values[i]); 250 | } 251 | 252 | // 损失函数策略 253 | ceres::LossFunction *loss_function = nullptr; 254 | if (iter > 2) { // 前几次迭代不使用鲁棒核 255 | loss_function = new ceres::TukeyLoss(std::sqrt(3 * plane_resolution)); 256 | } 257 | 258 | problem.AddResidualBlock(cost_function, loss_function, pose_parameters); 259 | valid_constraints++; 260 | 261 | // 计算初始残差用于调试 262 | if (iter == 0 && i < 100) { 263 | Eigen::Vector3d p_trans = pose_q * src_pt + pose_t; 264 | double residual = normals[i].dot(p_trans) + plane_d_values[i]; 265 | total_residual += residual * residual; 266 | } 267 | } 268 | 269 | // 调试信息 270 | if (iter == 0) { 271 | double avg_residual = std::sqrt(total_residual / std::min(100, (int) correspondences.size())); 272 | std::cout << "[SuperLoc Debug] Initial state - t: [" << pose_t.transpose() 273 | << "], q: [" << pose_q.coeffs().transpose() << "]" << std::endl; 274 | std::cout << "[SuperLoc Debug] Valid constraints: " << valid_constraints 275 | << ", avg residual: " << avg_residual << std::endl; 276 | } 277 | 278 | // 4. 求解优化问题 - 与原始SuperLoc完全一致 279 | ceres::Solver::Options options; 280 | options.max_num_iterations = 4; 281 | options.linear_solver_type = ceres::DENSE_QR; 282 | options.minimizer_progress_to_stdout = false; 283 | options.check_gradients = false; 284 | options.gradient_check_relative_precision = 1e-4; 285 | 286 | // 保存优化前的参数用于调试 287 | Eigen::Vector3d t_before = t; 288 | Eigen::Quaterniond q_before = q; 289 | 290 | ceres::Solver::Summary summary; 291 | ceres::Solve(options, &problem, &summary); 292 | 293 | // 调试信息 294 | if (iter < 3 || (iter % 10 == 0)) { 295 | std::cout << "[SuperLoc Debug] Iter " << iter 296 | << ", cost change: " << summary.initial_cost << " -> " << summary.final_cost 297 | << ", successful steps: " << summary.num_successful_steps << std::endl; 298 | } 299 | 300 | // 5. 更新变换矩阵 301 | // 直接从Map获取更新后的值 302 | t = pose_t; 303 | q = pose_q; 304 | q.normalize(); 305 | 306 | final_transform.setIdentity(); 307 | final_transform.block<3, 3>(0, 0) = q.toRotationMatrix(); 308 | final_transform.block<3, 1>(0, 3) = t; 309 | 310 | // 调试：检查参数是否更新 311 | if (iter < 3 || (iter % 10 == 0)) { 312 | Eigen::Vector3d t_change = t - t_before; 313 | double angle_change = q.angularDistance(q_before); 314 | std::cout << "[SuperLoc Debug] Parameter change - trans: " << t_change.norm() 315 | << ", rot: " << pcl::rad2deg(angle_change) << " deg" << std::endl; 316 | } 317 | 318 | // 6. 计算误差指标 319 | double rmse = 0.0; 320 | int inlier_count = 0; 321 | for (size_t i = 0; i < correspondences.size(); ++i) { 322 | const auto &corr = correspondences[i]; 323 | Eigen::Vector3d src_pt(source->points[corr.first].x, 324 | source->points[corr.first].y, 325 | source->points[corr.first].z); 326 | Eigen::Vector3d transformed_pt = q * src_pt + t; 327 | 328 | double residual = normals[i].dot(transformed_pt) + plane_d_values[i]; 329 | rmse += residual * residual; 330 | 331 | if (std::abs(residual) < 0.1) { 332 | inlier_count++; 333 | } 334 | } 335 | 336 | rmse = std::sqrt(rmse / correspondences.size()); 337 | double fitness = static_cast(inlier_count) / correspondences.size(); 338 | 339 | // 记录迭代日志 340 | iter_log.rmse = rmse; 341 | iter_log.fitness = fitness; 342 | iter_log.transform_matrix = final_transform; 343 | 344 | auto iter_end = std::chrono::high_resolution_clock::now(); 345 | iter_log.iter_time_ms = std::chrono::duration( 346 | iter_end - iter_start).count(); 347 | 348 | iteration_logs.push_back(iter_log); 349 | 350 | // 检查收敛条件 - 与原始SuperLoc保持一致 351 | if ((summary.num_successful_steps > 0) || (iter == max_iterations - 1)) { 352 | result.converged = (summary.num_successful_steps > 0) && (rmse < 0.01); 353 | result.iterations = iter + 1; 354 | result.final_rmse = rmse; 355 | result.final_fitness = fitness; 356 | 357 | break; 358 | } 359 | } 360 | 361 | if (!result.converged) { 362 | result.iterations = max_iterations; 363 | result.final_rmse = iteration_logs.back().rmse; 364 | result.final_fitness = iteration_logs.back().fitness; 365 | 366 | // 即使没有收敛，也进行最后的分析 367 | // 估计配准误差和协方差 368 | ceres::Problem final_problem; 369 | ceres::LocalParameterization *pose_parameterization = 370 | new SuperLocPoseParameterization(); 371 | final_problem.AddParameterBlock(pose_parameters, 7, pose_parameterization); 372 | 373 | // 使用最后一次迭代的对应点重建问题（简化版） 374 | EstimateRegistrationError(final_problem, pose_parameters, result); 375 | 376 | // 计算不确定性 377 | computeUncertaintiesFromHistogram(PlaneFeatureHistogramObs, result); 378 | result.feature_histogram = PlaneFeatureHistogramObs; 379 | 380 | // 检查退化 381 | checkDegeneracy(result); 382 | } 383 | 384 | return result.converged; 385 | } 386 | 387 | // 扩展的对应点查找函数，包含法向量计算 388 | void SuperLocICPIntegration::findCorrespondencesWithNormals( 389 | const pcl::PointCloud::Ptr &source, 390 | const pcl::PointCloud::Ptr &target, 391 | const Eigen::Matrix4d &transform, 392 | double max_distance, 393 | std::vector > &correspondences, 394 | std::vector &normals, 395 | std::vector &plane_d_values) { 396 | 397 | correspondences.clear(); 398 | normals.clear(); 399 | plane_d_values.clear(); 400 | 401 | pcl::KdTreeFLANN kdtree; 402 | kdtree.setInputCloud(target); 403 | 404 | // 对每个源点查找最近邻 405 | for (size_t i = 0; i < source->size(); ++i) { 406 | Eigen::Vector4d src_pt(source->points[i].x, source->points[i].y, 407 | source->points[i].z, 1.0); 408 | Eigen::Vector4d transformed_pt = transform * src_pt; 409 | 410 | pcl::PointXYZI query_pt; 411 | query_pt.x = transformed_pt(0); 412 | query_pt.y = transformed_pt(1); 413 | query_pt.z = transformed_pt(2); 414 | 415 | // 查找k个最近邻用于平面拟合 416 | std::vector k_indices; 417 | std::vector k_distances; 418 | if (kdtree.nearestKSearch(query_pt, 5, k_indices, k_distances) >= 5) { 419 | if (k_distances[0] <= max_distance * max_distance) { 420 | // 计算平面参数 - 与原始SuperLoc保持一致 421 | Eigen::MatrixXd A(5, 3); 422 | Eigen::VectorXd b = -Eigen::VectorXd::Ones(5); 423 | 424 | for (int j = 0; j < 5; ++j) { 425 | A(j, 0) = target->points[k_indices[j]].x; 426 | A(j, 1) = target->points[k_indices[j]].y; 427 | A(j, 2) = target->points[k_indices[j]].z; 428 | } 429 | 430 | // 使用最小二乘拟合平面: Ax + By + Cz + 1 = 0 431 | // 求解 [A B C]^T，使得 ||[x y z][A B C]^T + 1||^2 最小 432 | Eigen::Vector3d plane_coeffs = A.colPivHouseholderQr().solve(b); 433 | 434 | // plane_coeffs = [A, B, C]，平面方程为 Ax + By + Cz + 1 = 0 435 | // 转换为标准形式 n·p + d = 0，其中|n| = 1 436 | double norm = plane_coeffs.norm(); 437 | if (norm < 1e-6) continue; // 退化平面 438 | 439 | Eigen::Vector3d plane_normal = plane_coeffs / norm; 440 | double negative_OA_dot_norm = 1.0 / norm; // 这是原始SuperLoc中的d 441 | 442 | // 确保法向量朝向视点（与原始SuperLoc一致） 443 | Eigen::Vector3d viewpoint_direction(query_pt.x, query_pt.y, query_pt.z); 444 | if (viewpoint_direction.dot(plane_normal) < 0) { 445 | plane_normal = -plane_normal; 446 | negative_OA_dot_norm = -negative_OA_dot_norm; 447 | } 448 | 449 | correspondences.push_back(std::make_pair(i, k_indices[0])); 450 | normals.push_back(plane_normal); 451 | plane_d_values.push_back(negative_OA_dot_norm); 452 | } 453 | } 454 | } 455 | } 456 | 457 | // 详细的特征可观测性分析 - 与原始SuperLoc保持一致 458 | void SuperLocICPIntegration::analyzeFeatureObservabilityDetailed( 459 | const pcl::PointCloud::Ptr &source, 460 | const std::vector > &correspondences, 461 | const std::vector &normals, 462 | const Eigen::Matrix4d &transform, 463 | std::array &histogram) { 464 | 465 | // 获取当前旋转矩阵 466 | Eigen::Matrix3d R = transform.block<3, 3>(0, 0); 467 | 468 | // 旋转后的坐标轴 469 | Eigen::Vector3d x_axis = R * Eigen::Vector3d(1, 0, 0); 470 | Eigen::Vector3d y_axis = R * Eigen::Vector3d(0, 1, 0); 471 | Eigen::Vector3d z_axis = R * Eigen::Vector3d(0, 0, 1); 472 | 473 | for (size_t i = 0; i < correspondences.size(); ++i) { 474 | const auto &corr = correspondences[i]; 475 | Eigen::Vector3d src_pt(source->points[corr.first].x, 476 | source->points[corr.first].y, 477 | source->points[corr.first].z); 478 | 479 | // 变换点到世界坐标系 480 | Eigen::Vector3d transformed_pt = transform.block<3, 3>(0, 0) * src_pt + 481 | transform.block<3, 1>(0, 3); 482 | 483 | const Eigen::Vector3d &normal = normals[i]; 484 | 485 | // 计算叉积（用于旋转可观测性） 486 | Eigen::Vector3d cross = transformed_pt.cross(normal); 487 | 488 | // 旋转可观测性分析 489 | std::vector > rotation_quality; 490 | rotation_quality.push_back({std::abs(cross.dot(x_axis)), 0}); // rx_cross 491 | rotation_quality.push_back({std::abs(cross.dot(-x_axis)), 1}); // neg_rx_cross 492 | rotation_quality.push_back({std::abs(cross.dot(y_axis)), 2}); // ry_cross 493 | rotation_quality.push_back({std::abs(cross.dot(-y_axis)), 3}); // neg_ry_cross 494 | rotation_quality.push_back({std::abs(cross.dot(z_axis)), 4}); // rz_cross 495 | rotation_quality.push_back({std::abs(cross.dot(-z_axis)), 5}); // neg_rz_cross 496 | 497 | // 平移可观测性分析 498 | std::vector > trans_quality; 499 | trans_quality.push_back({std::abs(normal.dot(x_axis)), 6}); // tx_dot 500 | trans_quality.push_back({std::abs(normal.dot(y_axis)), 7}); // ty_dot 501 | trans_quality.push_back({std::abs(normal.dot(z_axis)), 8}); // tz_dot 502 | 503 | // 选择最佳的旋转和平移可观测性 504 | std::sort(rotation_quality.begin(), rotation_quality.end(), 505 | [](const auto &a, const auto &b) { return a.first > b.first; }); 506 | std::sort(trans_quality.begin(), trans_quality.end(), 507 | [](const auto &a, const auto &b) { return a.first > b.first; }); 508 | 509 | // 更新直方图 510 | histogram[rotation_quality[0].second]++; 511 | histogram[rotation_quality[1].second]++; 512 | histogram[trans_quality[0].second]++; 513 | } 514 | } 515 | 516 | // 估计配准误差 - 与原始SuperLoc的EstimateRegistrationError保持一致 517 | void SuperLocICPIntegration::EstimateRegistrationError( 518 | ceres::Problem &problem, 519 | const double *pose_parameters, 520 | SuperLocResult &result) { 521 | 522 | // 协方差计算选项 - 与原始SuperLoc保持一致 523 | ceres::Covariance::Options covOptions; 524 | covOptions.apply_loss_function = true; 525 | covOptions.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD; 526 | covOptions.null_space_rank = -1; 527 | covOptions.num_threads = 2; 528 | 529 | ceres::Covariance covarianceSolver(covOptions); 530 | std::vector > covarianceBlocks; 531 | covarianceBlocks.emplace_back(pose_parameters, pose_parameters); 532 | 533 | if (covarianceSolver.Compute(covarianceBlocks, &problem)) { 534 | // 获取6x6协方差矩阵（在切空间中） 535 | result.covariance.setZero(); 536 | covarianceSolver.GetCovarianceBlockInTangentSpace( 537 | pose_parameters, pose_parameters, result.covariance.data()); 538 | 539 | // 计算条件数 540 | Eigen::SelfAdjointEigenSolver > solver_full(result.covariance); 541 | Eigen::VectorXd eigenvalues = solver_full.eigenvalues(); 542 | std::cout << "info Eigen values: " << eigenvalues.transpose() << std::endl; 543 | 544 | // 避免除零 545 | double min_eigenvalue = std::max(eigenvalues(0), 1e-10); 546 | double max_eigenvalue = std::max(eigenvalues(5), 1e-10); 547 | result.cond_full = std::sqrt(max_eigenvalue / min_eigenvalue); 548 | 549 | // 位置部分条件数 550 | Eigen::SelfAdjointEigenSolver solver_trans( 551 | result.covariance.topLeftCorner<3, 3>()); 552 | double min_trans = std::max(solver_trans.eigenvalues()(0), 1e-10); 553 | double max_trans = std::max(solver_trans.eigenvalues()(2), 1e-10); 554 | result.cond_trans = std::sqrt(max_trans / min_trans); 555 | 556 | // 旋转部分条件数 557 | Eigen::SelfAdjointEigenSolver solver_rot( 558 | result.covariance.bottomRightCorner<3, 3>()); 559 | double min_rot = std::max(solver_rot.eigenvalues()(0), 1e-10); 560 | double max_rot = std::max(solver_rot.eigenvalues()(2), 1e-10); 561 | result.cond_rot = std::sqrt(max_rot / min_rot); 562 | } else { 563 | // 如果协方差计算失败，设置默认值 564 | result.covariance.setIdentity(); 565 | result.cond_full = 1.0; 566 | result.cond_trans = 1.0; 567 | result.cond_rot = 1.0; 568 | } 569 | } 570 | 571 | // 基于特征直方图计算不确定性 - 与原始SuperLoc的EstimateLidarUncertainty保持一致 572 | void SuperLocICPIntegration::computeUncertaintiesFromHistogram( 573 | const std::array &histogram, 574 | SuperLocResult &result) { 575 | 576 | // 平移特征总数 577 | double TotalTransFeature = histogram[6] + histogram[7] + histogram[8]; 578 | 579 | if (TotalTransFeature > 0) { 580 | // X方向不确定性 581 | double uncertaintyX = (histogram[6] / TotalTransFeature) * 3; 582 | result.uncertainty_x = std::min(uncertaintyX, 1.0); 583 | 584 | // Y方向不确定性 585 | double uncertaintyY = (histogram[7] / TotalTransFeature) * 3; 586 | result.uncertainty_y = std::min(uncertaintyY, 1.0); 587 | 588 | // Z方向不确定性 589 | double uncertaintyZ = (histogram[8] / TotalTransFeature) * 3; 590 | result.uncertainty_z = std::min(uncertaintyZ, 1.0); 591 | } else { 592 | result.uncertainty_x = 0.0; 593 | result.uncertainty_y = 0.0; 594 | result.uncertainty_z = 0.0; 595 | } 596 | 597 | // 旋转特征总数 598 | double TotalRotationFeature = histogram[0] + histogram[1] + histogram[2] + 599 | histogram[3] + histogram[4] + histogram[5]; 600 | 601 | if (TotalRotationFeature > 0) { 602 | // Roll不确定性 603 | double uncertaintyRoll = ((histogram[0] + histogram[1]) / TotalRotationFeature) * 3; 604 | result.uncertainty_roll = std::min(uncertaintyRoll, 1.0); 605 | 606 | // Pitch不确定性 607 | double uncertaintyPitch = ((histogram[2] + histogram[3]) / TotalRotationFeature) * 3; 608 | result.uncertainty_pitch = std::min(uncertaintyPitch, 1.0); 609 | 610 | // Yaw不确定性 611 | double uncertaintyYaw = ((histogram[4] + histogram[5]) / TotalRotationFeature) * 3; 612 | result.uncertainty_yaw = std::min(uncertaintyYaw, 1.0); 613 | } else { 614 | result.uncertainty_roll = 0.0; 615 | result.uncertainty_pitch = 0.0; 616 | result.uncertainty_yaw = 0.0; 617 | } 618 | } 619 | 620 | // 检查退化 - 基于不确定性和特征数量 621 | void SuperLocICPIntegration::checkDegeneracy(SuperLocResult &result) { 622 | // 使用与原始SuperLoc完全一致的退化判断条件 623 | // 原始代码中的判断逻辑包括不确定性阈值和特征数量检查 624 | 625 | // 基于不确定性的退化判断 626 | if (result.uncertainty_x < 0.2 || result.uncertainty_y < 0.1 || result.uncertainty_z < 0.2) { 627 | result.isDegenerate = true; 628 | } 629 | // 基于条件数的退化判断（作为额外的安全检查） 630 | // else if (result.cond_full > 100.0 || result.cond_trans > 100.0 || result.cond_rot > 100.0) { 631 | // result.isDegenerate = true; 632 | // } 633 | else { 634 | result.isDegenerate = false; 635 | } 636 | 637 | // 注：原始SuperLoc还检查了特征数量(PlaneFeatureHistogramObs.at(6/7/8) < 20/10/10) 638 | // 但在我们的实现中，这个检查已经隐含在不确定性计算中 639 | // 因为当特征数量少时，不确定性会自动变高 640 | } 641 | 642 | // 保持原有的辅助函数 643 | void SuperLocICPIntegration::findCorrespondences( 644 | const pcl::PointCloud::Ptr &source, 645 | const pcl::PointCloud::Ptr &target, 646 | const Eigen::Matrix4d &transform, 647 | double max_distance, 648 | std::vector > &correspondences, 649 | std::vector &normals) { 650 | 651 | // 调用新的函数，忽略plane_d_values 652 | std::vector plane_d_values; 653 | findCorrespondencesWithNormals(source, target, transform, max_distance, 654 | correspondences, normals, plane_d_values); 655 | } 656 | 657 | void SuperLocICPIntegration::analyzeFeatureObservability( 658 | const pcl::PointCloud::Ptr &source, 659 | const std::vector > &correspondences, 660 | const std::vector &normals, 661 | const Eigen::Matrix4d &transform, 662 | std::array &histogram) { 663 | 664 | // 调用新的详细分析函数 665 | analyzeFeatureObservabilityDetailed(source, correspondences, normals, 666 | transform, histogram); 667 | } 668 | 669 | void SuperLocICPIntegration::computeUncertainties( 670 | const std::array &histogram, 671 | SuperLocResult &result) { 672 | 673 | // 调用新的函数 674 | computeUncertaintiesFromHistogram(histogram, result); 675 | } 676 | 677 | void SuperLocICPIntegration::computeDegeneracyFromCovariance( 678 | const Eigen::Matrix &covariance, 679 | double threshold, 680 | SuperLocResult &result) { 681 | 682 | // 该函数已经集成到EstimateRegistrationError中 683 | // 保留以维持接口兼容性 684 | } 685 | 686 | void SuperLocICPIntegration::computePlaneParameters( 687 | const pcl::PointCloud::Ptr &cloud, 688 | const std::vector &indices, 689 | Eigen::Vector3d &normal, 690 | double &d) { 691 | 692 | if (indices.size() < 3) { 693 | normal = Eigen::Vector3d(0, 0, 1); 694 | d = 0; 695 | return; 696 | } 697 | 698 | // 使用最小二乘拟合平面 699 | Eigen::MatrixXd A(indices.size(), 3); 700 | Eigen::VectorXd b = -Eigen::VectorXd::Ones(indices.size()); 701 | 702 | for (size_t i = 0; i < indices.size(); ++i) { 703 | A(i, 0) = cloud->points[indices[i]].x; 704 | A(i, 1) = cloud->points[indices[i]].y; 705 | A(i, 2) = cloud->points[indices[i]].z; 706 | } 707 | 708 | normal = A.colPivHouseholderQr().solve(b); 709 | d = 1.0 / normal.norm(); 710 | normal.normalize(); 711 | } 712 | 713 | } // namespace ICPRunner 714 | --------------------------------------------------------------------------------