GICP

转载 2016年08月31日 11:34:53

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

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

近似特征值计算

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

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

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

  


相关文章推荐

内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:GICP
举报原因:
原因补充:

(最多只允许输入30个字)