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

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

      }

    }

  }

巴特西

[PointCloud] GICP

最新文章

热门文章