GICP

530人阅读 评论(0)

 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 /** \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& cloud,                                   const pcl::PointCloud& normals,                                   std::vector >& 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;       }     }     }

0
0

* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
个人资料
• 访问：526690次
• 积分：6265
• 等级：
• 排名：第3956名
• 原创：11篇
• 转载：575篇
• 译文：0篇
• 评论：59条
最新评论