[PointCloud] GICP

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

CC中的GICP插件

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

void qLxPluginPCL::doSpraseRegistration()
{
    assert(m_app);
    if (!m_app)
        return;
 
    const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
    size_t selNum = selectedEntities.size();
    if (selNum!=2)
    {
        m_app->dispToConsole("Please select two cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
        return;
    }
 
    ccHObject* ent = selectedEntities[0];
    assert(ent);
    if (!ent || !ent->isA(CC_TYPES::POINT_CLOUD))
    {
        m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
        return;
    }
    ccHObject* ent2 = selectedEntities[1];
    assert(ent2);
    if (!ent2 || !ent2->isA(CC_TYPES::POINT_CLOUD))
    {
        m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
        return;
    }
 
    ccPointCloud* m_target_cloud = static_cast<ccPointCloud*>(ent);
    ccPointCloud* m_Source_cloud = static_cast<ccPointCloud*>(ent2);
 
    //input cloud
    unsigned count = m_target_cloud->size();
    bool hasNorms = m_target_cloud->hasNormals();
    CCVector3 bbMin, bbMax;
    m_target_cloud->getBoundingBox(bbMin,bbMax);
    const CCVector3d& globalShift = m_target_cloud->getGlobalShift();
    double globalScale = m_target_cloud->getGlobalScale();
 
    cc3DNdtdlg dlg;
    if (!dlg.exec())
        return;
 
    double maxCorrDis =1;
    maxCorrDis= dlg.getVoxelGridsize();
    pcl::PointCloud<PointXYZ>::Ptr pcl_t_cloud (new pcl::PointCloud<PointXYZ>);
    pcl::PointCloud<PointXYZ>::Ptr pcl_s_cloud (new pcl::PointCloud<PointXYZ>);
    try
    {
        //
        unsigned pointCount = m_target_cloud->size();
        pcl_t_cloud->resize(pointCount);
 
        for (unsigned i = 0; i < pointCount; ++i)
        {
            const CCVector3* P = m_target_cloud->getPoint(i);
            pcl_t_cloud->at(i).x = static_cast<float>(P->x);
            pcl_t_cloud->at(i).y = static_cast<float>(P->y);
            pcl_t_cloud->at(i).z = static_cast<float>(P->z);
        }
        //
        pointCount = m_Source_cloud->size();
        pcl_s_cloud->resize(pointCount);
 
        for (unsigned i = 0; i < pointCount; ++i)
        {
            const CCVector3* P = m_Source_cloud->getPoint(i);
            pcl_s_cloud->at(i).x = static_cast<float>(P->x);
            pcl_s_cloud->at(i).y = static_cast<float>(P->y);
            pcl_s_cloud->at(i).z = static_cast<float>(P->z);
        }
    }
    catch(...)
    {
        //any error (memory, etc.)
        pcl_t_cloud.reset();
        pcl_s_cloud.reset();
    }
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ>);
    tree1->setInputCloud (pcl_t_cloud); 
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ>);
    tree2->setInputCloud (pcl_s_cloud);  
    pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
    // reconstruct meshes for source and target
    pcl::OrganizedFastMesh<pcl::PointXYZ> fast_mesh;
    fast_mesh.setInputCloud(pcl_t_cloud);     //设置输入点云
/*  ofm.setIndices(cloudInd);*/        //设置输入点云索引
    fast_mesh.setMaxEdgeLength(0.1);        //设置多边形网格最大边长
    fast_mesh.setTriangulationType(pcl::OrganizedFastMesh<pcl::PointXYZ>::TRIANGLE_ADAPTIVE_CUT);//设置网格类型
    fast_mesh.setTrianglePixelSize(1);        //设置三角形边长 1表示链接相邻点作为边长
    //fast_mesh.storeShadowedFaces(false);        //设置是否存储阴影面
    fast_mesh.setSearchMethod(tree1);
    fast_mesh.reconstruct(*mesh);
        // compute normals and covariances for source and target
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    boost::shared_ptr<std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >> target_covariances(new std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >);
    pcl::features::computeApproximateNormals(*pcl_t_cloud, mesh->polygons, *normals);
    pcl::features::computeApproximateCovariances(*pcl_t_cloud, *normals, *target_covariances);
     
 
    fast_mesh.setInputCloud(pcl_s_cloud);
    /*  ofm.setIndices(cloudInd);*/        //设置输入点云索引
    fast_mesh.setMaxEdgeLength(1.0);        //设置多边形网格最大边长
    fast_mesh.setTriangulationType(pcl::OrganizedFastMesh<pcl::PointXYZ>::TRIANGLE_ADAPTIVE_CUT);//设置网格类型
    fast_mesh.setTrianglePixelSize(1);        //设置三角形边长 1表示链接相邻点作为边长
    fast_mesh.storeShadowedFaces(false);        //设置是否存储阴影面
    fast_mesh.setSearchMethod(tree2);
    fast_mesh.reconstruct(*mesh);
    // compute normals and covariances for source and target
    pcl::PointCloud<pcl::Normal>::Ptr normalst(new pcl::PointCloud<pcl::Normal>);
    boost::shared_ptr<std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>> source_covariances(new std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >);
    pcl::features::computeApproximateNormals(*pcl_s_cloud, mesh->polygons, *normalst);
    pcl::features::computeApproximateCovariances(*pcl_s_cloud, *normalst, *source_covariances);
        // setup Generalized-ICP
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;
    gicp.setMaxCorrespondenceDistance(maxCorrDis);
    gicp.setInputSource(pcl_s_cloud);
    gicp.setInputTarget(pcl_t_cloud);
    /*gicp.setSourceCovariances(source_covariances);
    gicp.setTargetCovariances(target_covariances);*/
    // run registration and get transformation
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
    gicp.align(*cloud_out);
    Eigen::Matrix4f transform = gicp.getFinalTransformation();
    int pointCount = cloud_out->size();
 
    //static_cast<size_t>(sm_cloud ? sm_cloud->width * sm_cloud->height : 0);
 
    ccPointCloud* ccCloud =new ccPointCloud();
    if (!ccCloud->reserve(static_cast<unsigned>(pointCount)))
        return ;
    for (size_t i = 0; i < pointCount; ++i)
    {
        CCVector3 P(cloud_out->at(i).x,cloud_out->at(i).y,cloud_out->at(i).z);
        ccCloud->addPoint(P);
    }
    ccCloud->setName(QString("GICP"));
    ccColor::Rgb col = ccColor::Generator::Random();
    ccCloud->setRGBColor(col);
    ccCloud->showColors(true);
    ccCloud->setPointSize(1);
    ccHObject* group = 0;
    if (!group)
        group = new ccHObject(QString("GICP(%1)").arg(ent2->getName()));
    group->addChild(ccCloud);
    group->setVisible(true);
    m_app->addToDB(group);
}

近似特征值计算

这个的原理被想复杂了，就是特征值分解的逆步骤，形成了三个正交的向量，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;
      }
    }
 
  }