GICP

最新推荐文章于 2024-08-02 06:00:00 发布

查志强

最新推荐文章于 2024-08-02 06:00:00 发布

阅读量6.8k

点赞数 1

分类专栏：【三维】PCL 文章标签： PCL ICP

【三维】PCL 专栏收录该内容

20 篇文章 5 订阅

订阅专栏

【原文：http://www.cnblogs.com/yhlx125/p/5709157.html】

泛化的ICP算法，通过协方差矩阵起到类似于权重的作用，消除某些不好的对应点在求解过程中的作用。不过可以囊括Point to Point，Point to Plane的ICP算法，真正的是泛化的ICP。牛！

近似特征值计算

这个的原理被想复杂了，就是特征值分解的逆步骤，形成了三个正交的向量，epsilon是最小的特征值，法向量是最小的特征向量。

本来求法向量的过程就是根据近邻的k个点，利用主成分分析PCA进行计算得到特征值最小的那个特征向量作为法向量。

 
         /** \brief Compute GICP-style covariance matrices given a point cloud and 
        
         * the corresponding surface normals. 
        
         * \param[in] cloud Point cloud containing the XYZ coordinates, 
        
         * \param[in] normals Point cloud containing the corresponding surface normals. 
        
         * \param[out] covariances Vector of computed covariances. 
        
         * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001) 
        
         */ 
        
         template  
         < 
         typename  
         PointT,  
         typename  
         PointNT>  
         inline  
         void 
        
         computeApproximateCovariances( 
         const  
         pcl::PointCloud<PointT>& cloud, 
        
         const  
         pcl::PointCloud<PointNT>& normals, 
        
         std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >& covariances, 
        
         double  
         epsilon = 0.001) 
        
         { 
        
         assert 
         (cloud.points.size() == normals.points.size()); 
        
         int  
         nr_points =  
         static_cast 
         < 
         int 
         >(cloud.points.size()); 
        
         covariances.resize(nr_points); 
        
         for  
         ( 
         int  
         i = 0; i < nr_points; ++i) 
        
         { 
        
         Eigen::Vector3d normal(normals.points[i].normal_x, 
        
         normals.points[i].normal_y, 
        
         normals.points[i].normal_z); 
        
         // compute rotation matrix 
        
         Eigen::Matrix3d rot; 
        
         Eigen::Vector3d y; 
        
         y << 0, 1, 0; 
        
         rot.row(2) = normal; 
        
         y = y - normal(1) * normal; 
        
         y.normalize(); 
        
         rot.row(1) = y; 
        
         rot.row(0) = normal.cross(rot.row(1)); 
        
         // comnpute approximate covariance 
        
         Eigen::Matrix3d cov; 
        
         cov << 1, 0, 0, 
        
         0, 1, 0, 
        
         0, 0, epsilon; 
        
         covariances[i] = rot.transpose()*cov*rot; 
        
         } 
        
         } 
        
         }