[PointCloud] GICP

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

CC中的GICP插件

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进行计算得到特征值最小的那个特征向量作为法向量。

/** rief 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;
      }
    }

  }