# GICP

 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;       }     }     }

• 本文已收录于以下专栏：

### 相关文章推荐

举报原因： 您举报文章：GICP 色情 政治 抄袭 广告 招聘 骂人 其他 (最多只允许输入30个字)