[PointCloud] GICP |
您所在的位置：网站首页 › vue地图选点 › [PointCloud] GICP |
|
[PointCloud] GICP
转载
mb5fcdf2add9b6a 2016-07-26 22:14:00 文章标签 Point Cloud 3d 特征值 点云 泛化 文章分类 其它 其它 泛化的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(ent); ccPointCloud* m_Source_cloud = static_cast(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::Ptr pcl_t_cloud (new pcl::PointCloud); pcl::PointCloud::Ptr pcl_s_cloud (new pcl::PointCloud); 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(P->x); pcl_t_cloud->at(i).y = static_cast(P->y); pcl_t_cloud->at(i).z = static_cast(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(P->x); pcl_s_cloud->at(i).y = static_cast(P->y); pcl_s_cloud->at(i).z = static_cast(P->z); } } catch(...) { //any error (memory, etc.) pcl_t_cloud.reset(); pcl_s_cloud.reset(); } pcl::search::KdTree::Ptr tree1 (new pcl::search::KdTree); tree1->setInputCloud (pcl_t_cloud); pcl::search::KdTree::Ptr tree2 (new pcl::search::KdTree); tree2->setInputCloud (pcl_s_cloud); pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh); // reconstruct meshes for source and target pcl::OrganizedFastMesh fast_mesh; fast_mesh.setInputCloud(pcl_t_cloud); //设置输入点云/* ofm.setIndices(cloudInd);*/ //设置输入点云索引 fast_mesh.setMaxEdgeLength(0.1); //设置多边形网格最大边长 fast_mesh.setTriangulationType(pcl::OrganizedFastMesh::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::Ptr normals(new pcl::PointCloud); boost::shared_ptr target_covariances(new std::vector); 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::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::Ptr normalst(new pcl::PointCloud); boost::shared_ptr source_covariances(new std::vector); pcl::features::computeApproximateNormals(*pcl_s_cloud, mesh->polygons, *normalst); pcl::features::computeApproximateCovariances(*pcl_s_cloud, *normalst, *source_covariances); // setup Generalized-ICP pcl::GeneralizedIterativeClosestPoint 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::Ptr cloud_out (new pcl::PointCloud); gicp.align(*cloud_out); Eigen::Matrix4f transform = gicp.getFinalTransformation(); int pointCount = cloud_out->size(); //static_cast(sm_cloud ? sm_cloud->width * sm_cloud->height : 0); ccPointCloud* ccCloud =new ccPointCloud(); if (!ccCloud->reserve(static_cast(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 inline void computeApproximateCovariances(const pcl::PointCloud& cloud, const pcl::PointCloud& normals, std::vector& covariances, double epsilon = 0.001) { assert(cloud.points.size() == normals.points.size()); int nr_points = static_cast(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 赞 收藏 评论 分享 举报上一篇：流媒体技术学习笔记之（三）Nginx-Rtmp-Module统计某频道在线观看流的客户数 下一篇：memset()函数 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |