GICP

/** \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 (() == ());

      int  nr_points =  static_cast < int >(());
      (nr_points);
      for  ( int  i = 0; i < nr_points; ++i)
      {
        Eigen::Vector3d normal([i].normal_x,
                               [i].normal_y,
                               [i].normal_z);

        // compute rotation matrix
        Eigen::Matrix3d rot;
        Eigen::Vector3d y;
        y << 0, 1, 0;
        (2) = normal;
        y = y - normal(1) * normal;
        ();
        (1) = y;
        (0) = ((1));

        // comnpute approximate covariance
        Eigen::Matrix3d cov;
        cov << 1, 0, 0,
               0, 1, 0,
               0, 0, epsilon;
        covariances[i] = ()*cov*rot;
      }
    }

  }