泛化的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;
}
}
}