ICP in VTK
提要
今天要研究的是关于图像配准的问题，图像配准是图像处理研究领域中的一个典型问题和技术难点，其目的在于比较或融合针对同一对象在不同条件下获取的图像，例如图像会来自不同的采集设备，取自不同的时间，不同的拍摄视角等等，有时也需要用到针对不同对象的图像配准问题。具体地说，对于一组图像数据集中的两幅图像，通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。
一个经典的应用是场景的重建，比如说一张茶几上摆了很多杯具，用深度摄像机进行场景的扫描，通常不可能通过一次采集就将场景中的物体全部扫描完成，只能是获取场景不同角度的点云，然后将这些点云融合在一起，获得一个完整的场景。
对于点云的配准，给定一个源点云和一个目标点云，配准可以简单地分为三个步骤：
● 找配准对（correspondence pairs）；
● 计算配准对之间的变换矩阵；
● 将对应的变换施加到源点云上；
* crrespondences 可以是点，特征等等。
ICP算法是图像配准极其重要的算法之一。
ICP算法简介
ICP算法最初由Besl和Mckey提出，是一种基于轮廓特征的点配准方法。基准点在CT图像坐标系及世界坐标系下的坐标点集P = {Pi, i = 0,1, 2,…,k}及U = {Ui,i=0,1,2,…,n}。其中，U与P元素间不必存在一一对应关系，元素数目亦不必相同，设k≥n。配准过程就是求取2个坐标系间的旋转和平移变换矩阵，使得来自U与P的同源点间距离最小。其过程如下：
(1)计算最近点，即对于集合U中的每一个点，在集合P中都找出距该点最近的对应点，设集合P中由这些对应点组成的新点集为Q = {qi,i = 0,1,2,…,n}。
(2)采用最小均方根法，计算点集U与Q之间的配准，使得到配准变换矩阵R，T，其中R是3×3的旋转矩阵，T是3×1的平移矩阵。
(3)计算坐标变换，即对于集合U，用配准变换矩阵R，T进行坐标变换，得到新的点集U1，即U1 = RU + T
(4)计算U1与Q之间的均方根误差，如小于预设的极限值ε，则结束，否则，以点集U1替换U，重复上述步骤。
数学描述（感觉更好理解一些）
三维空间中两个3D点，
，他们的欧式距离表示为：
，他们的欧式距离表示为：
三维点云匹配问题的目的是找到P和Q变化的矩阵R和T，对于
，
，利用最小二乘法求解最优解使：
，
，利用最小二乘法求解最优解使：
最小时的R和T。
VTK中有一个类vtkIterativeClosestPointTransform实现了ICP算法，并将ICP算法保存在一个4×4的齐次矩阵中。下面就跟着官方demo来实践一下。
安装库
升级cmake
编译VTK6.1需要cmake2.8.8以上。
下载cmake2.8.12.2
解压终端cd进目录
sudo ./bootstrap
make
sudo make install
编译VTK6.1
官网下载解压终端cd进目录
mkdir build
cd build
cmake ..
make
sudo make install
实战
ICP的输入是两个点云，这两个点云必须是针对同一个场景，而且必须有重叠部分。
这里关乎格式转换、读取的问题的。，对新手来说，xyz是做好的读取文件了，只含有坐标信息，而且是文本信息。如果不是.xyz格式，用meshlab导出一个ply，把文件头部的说明去掉，扩展名改成xyz就可以了。
代码：
#include <vtkVersion.h>
#include <vtkSmartPointer.h>
#include <vtkTransform.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkIterativeClosestPointTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkLandmarkTransform.h>
#include <vtkMath.h>
#include <vtkMatrix4x4.h>
#include <vtkXMLPolyDataWriter.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkXMLPolyDataReader.h>
#include <vtkProperty.h>
#include <vtkPLYReader.h>
#include <sstream>
#include <iostream>
int main(int argc, char *argv[])
{
vtkSmartPointer<vtkPolyData> sourceTmp =
vtkSmartPointer<vtkPolyData>::New();
vtkSmartPointer<vtkPolyData> targetTmp =
vtkSmartPointer<vtkPolyData>::New();
vtkSmartPointer<vtkPolyData> source =
vtkSmartPointer<vtkPolyData>::New();
vtkSmartPointer<vtkPolyData> target =
vtkSmartPointer<vtkPolyData>::New();
if(argc == 3)
{
// Get all data from the file
std::string strSource = argv[1];
std::string strTarget = argv[2];
std::ifstream fSource(strSource.c_str());
std::ifstream fTarget(strTarget.c_str());
std::string line;
vtkSmartPointer<vtkPoints> sourcePoints =
vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkPoints> targetPoints =
vtkSmartPointer<vtkPoints>::New();
while(std::getline(fSource, line))
{
double x,y,z;
std::stringstream linestream;
linestream << line;
linestream >> x >> y >> z;
sourcePoints->InsertNextPoint(x, y, z);
}
sourceTmp->SetPoints(sourcePoints);
vtkSmartPointer<vtkVertexGlyphFilter> vertexFilter1 =
vtkSmartPointer<vtkVertexGlyphFilter>::New();
#if VTK_MAJOR_VERSION <= 5
vertexFilter1->SetInputConnection(sourceTmp->GetProducerPort());
#else
vertexFilter1->SetInputData(sourceTmp);
#endif
vertexFilter1->Update();
source->ShallowCopy(vertexFilter1->GetOutput());
while(std::getline(fTarget, line))
{
double x,y,z;
std::stringstream linestream;
linestream << line;
linestream >> x >> y >> z;
targetPoints->InsertNextPoint(x, y, z);
}
targetTmp->SetPoints(targetPoints);
vtkSmartPointer<vtkVertexGlyphFilter> vertexFilter2 =
vtkSmartPointer<vtkVertexGlyphFilter>::New();
#if VTK_MAJOR_VERSION <= 5
vertexFilter2->SetInputConnection(targetTmp->GetProducerPort());
#else
vertexFilter2->SetInputData(targetTmp);
#endif
vertexFilter2->Update();
target->ShallowCopy(vertexFilter2->GetOutput());
}
else
{
std::cout << "Error data..." << std::endl;
}
// Setup ICP transform
vtkSmartPointer<vtkIterativeClosestPointTransform> icp =
vtkSmartPointer<vtkIterativeClosestPointTransform>::New();
icp->SetSource(source);
icp->SetTarget(target);
icp->GetLandmarkTransform()->SetModeToRigidBody();
icp->SetMaximumNumberOfIterations(20);
//icp->StartByMatchingCentroidsOn();
icp->Modified();
icp->Update();
cout<<"bitch"<<endl;
// Get the resulting transformation matrix (this matrix takes the source points to the target points)
vtkSmartPointer<vtkMatrix4x4> m = icp->GetMatrix();
std::cout << "The resulting matrix is: " << *m << std::endl;
// Transform the source points by the ICP solution
vtkSmartPointer<vtkTransformPolyDataFilter> icpTransformFilter =
vtkSmartPointer<vtkTransformPolyDataFilter>::New();
#if VTK_MAJOR_VERSION <= 5
icpTransformFilter->SetInput(source);
#else
icpTransformFilter->SetInputData(source);
#endif
icpTransformFilter->SetTransform(icp);
icpTransformFilter->Update();
/*
// If you need to take the target points to the source points, the matrix is:
icp->Inverse();
vtkSmartPointer<vtkMatrix4x4> minv = icp->GetMatrix();
std::cout << "The resulting inverse matrix is: " << *minv << std::cout;
*/
// Visualize
vtkSmartPointer<vtkPolyDataMapper> sourceMapper =
vtkSmartPointer<vtkPolyDataMapper>::New();
#if VTK_MAJOR_VERSION <= 5
sourceMapper->SetInputConnection(source->GetProducerPort());
#else
sourceMapper->SetInputData(source);
#endif
vtkSmartPointer<vtkActor> sourceActor =
vtkSmartPointer<vtkActor>::New();
sourceActor->SetMapper(sourceMapper);
sourceActor->GetProperty()->SetColor(1,0,0);
sourceActor->GetProperty()->SetPointSize(4);
vtkSmartPointer<vtkPolyDataMapper> targetMapper =
vtkSmartPointer<vtkPolyDataMapper>::New();
#if VTK_MAJOR_VERSION <= 5
targetMapper->SetInputConnection(target->GetProducerPort());
#else
targetMapper->SetInputData(target);
#endif
vtkSmartPointer<vtkActor> targetActor =
vtkSmartPointer<vtkActor>::New();
targetActor->SetMapper(targetMapper);
targetActor->GetProperty()->SetColor(0,1,0);
targetActor->GetProperty()->SetPointSize(4);
vtkSmartPointer<vtkPolyDataMapper> solutionMapper =
vtkSmartPointer<vtkPolyDataMapper>::New();
solutionMapper->SetInputConnection(icpTransformFilter->GetOutputPort());
vtkSmartPointer<vtkActor> solutionActor =
vtkSmartPointer<vtkActor>::New();
solutionActor->SetMapper(solutionMapper);
solutionActor->GetProperty()->SetColor(0,0,1);
solutionActor->GetProperty()->SetPointSize(3);
// Create a renderer, render window, and interactor
vtkSmartPointer<vtkRenderer> renderer =
vtkSmartPointer<vtkRenderer>::New();
vtkSmartPointer<vtkRenderWindow> renderWindow =
vtkSmartPointer<vtkRenderWindow>::New();
renderWindow->AddRenderer(renderer);
vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
vtkSmartPointer<vtkRenderWindowInteractor>::New();
renderWindowInteractor->SetRenderWindow(renderWindow);
// Add the actor to the scene
renderer->AddActor(sourceActor);
renderer->AddActor(targetActor);
renderer->AddActor(solutionActor);
renderer->SetBackground(.3, .6, .3); // Background color green
// Render and interact
renderWindow->Render();
renderWindowInteractor->Start();
return EXIT_SUCCESS;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
PROJECT(IterativeClosestPointsTransform)
find_package(VTK REQUIRED)
include(${VTK_USE_FILE})
add_executable(IterativeClosestPointsTransform MACOSX_BUNDLE IterativeClosestPointsTransform)
if(VTK_LIBRARIES)
target_link_libraries(IterativeClosestPointsTransform ${VTK_LIBRARIES})
else()
target_link_libraries(IterativeClosestPointsTransform vtkHybrid)
endif()
编译运行一下，用两片点云来测试，得到的结果：
微小的点云平移：
稍微大一些的平移
加入旋转量
绿色是target，红色是source，蓝色是solution。
结论和思考
和同学一起试用了几种ICP的方法，包括PCL的和VTK的，得到的结果都差不多。并不是很理想，感觉最好的Registration适用情况应该是从不同方位扫描一个物体，然后将点云进行配准，而且点云的算法的初始状态也有要求，一是要有点云的重合，二是不能分开得太远。
难道就这样结束了？
答案是No... 难道传说中的ICP这点配准都搞不定！？那也太弱了吧。
继续看论文和尝试.
这次改用PCL的库来实现。
用blender基于stanford bunny来做一组测试数据
按照PCL的pipeline，首先采用的是进行一个初始化操作，将点云进行一次预处理，得到一个稍微好一点的结果，这里用到的是SAC-IA的算法，流程如下：
SAC-IA: Sampled Consesus-Initial Alignment
1. Draw n points di from the source cloud
(with a minimum distance d in between).
2. For each drawn di :
2.1 get k closest matches, and
2.2 draw one of the k closest matches as mi
(instead of taking closest match)
3. Estimate transformation (R, t) for these samples
4. Determine inlier pairs with ((Rdi + t) − mi )2 <
5. Repeat N times, and use (R, t) having most inliers
想搞懂算法的自己扒论文，只想知道怎么用的和我来看代码：
template_alignment.cpp
#include <iostream>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/PolygonMesh.h>
#include <pcl/visualization/histogram_visualizer.h>
#include <boost/thread/thread.hpp>
class FeatureCloud
{
public:
// A bit of shorthand
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
typedef pcl::search::KdTree<pcl::PointXYZ> SearchMethod;
FeatureCloud () :
search_method_xyz_ (new SearchMethod),
normal_radius_ (0.06f),
feature_radius_ (0.06f)
{}
~FeatureCloud () {}
// Process the given cloud
void
setInputCloud (PointCloud::Ptr xyz)
{
xyz_ = xyz;
processInput ();
}
// Load and process the cloud in the given PCD file
void
loadInputCloud (const std::string &pcd_file)
{
xyz_ = PointCloud::Ptr (new PointCloud);
pcl::io::loadPCDFile (pcd_file, *xyz_);
processInput ();
}
// Get a pointer to the cloud 3D points
PointCloud::Ptr
getPointCloud () const
{
return (xyz_);
}
// Get a pointer to the cloud of 3D surface normals
SurfaceNormals::Ptr
getSurfaceNormals () const
{
return (normals_);
}
// Get a pointer to the cloud of feature descriptors
LocalFeatures::Ptr
getLocalFeatures () const
{
return (features_);
}
protected:
// Compute the surface normals and local features
void
processInput ()
{
computeSurfaceNormals ();
computeLocalFeatures ();
}
// Compute the surface normals
void
computeSurfaceNormals ()
{
normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
norm_est.setInputCloud (xyz_);
norm_est.setSearchMethod (search_method_xyz_);
norm_est.setRadiusSearch (normal_radius_);
norm_est.compute (*normals_);
}
// Compute the local feature descriptors
void
computeLocalFeatures ()
{
features_ = LocalFeatures::Ptr (new LocalFeatures);
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
fpfh_est.setInputCloud (xyz_);
fpfh_est.setInputNormals (normals_);
fpfh_est.setSearchMethod (search_method_xyz_);
fpfh_est.setRadiusSearch (feature_radius_);
fpfh_est.compute (*features_);
}
private:
// Point cloud data
PointCloud::Ptr xyz_;
SurfaceNormals::Ptr normals_;
LocalFeatures::Ptr features_;
SearchMethod::Ptr search_method_xyz_;
// Parameters
float normal_radius_;
float feature_radius_;
};
class TemplateAlignment
{
public:
// A struct for storing alignment results
struct Result
{
float fitness_score;
Eigen::Matrix4f final_transformation;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
TemplateAlignment () :
min_sample_distance_ (0.02f),
max_correspondence_distance_ (0.001f*0.001f),
nr_iterations_ (1000)
{
// Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
sac_ia_.setMinSampleDistance (min_sample_distance_);
sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
sac_ia_.setMaximumIterations (nr_iterations_);
}
~TemplateAlignment () {}
// Set the given cloud as the target to which the templates will be aligned
void
setTargetCloud (FeatureCloud &target_cloud)
{
target_ = target_cloud;
sac_ia_.setInputTarget (target_cloud.getPointCloud ());
sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
}
// Add the given cloud to the list of template clouds
void
addTemplateCloud (FeatureCloud &template_cloud)
{
templates_.push_back (template_cloud);
}
// Align the given template cloud to the target specified by setTargetCloud ()
void
align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
{
sac_ia_.setInputCloud (template_cloud.getPointCloud ());
sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());
pcl::PointCloud<pcl::PointXYZ> registration_output;
sac_ia_.align (registration_output);
result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);
result.final_transformation = sac_ia_.getFinalTransformation ();
}
// Align all of template clouds set by addTemplateCloud to the target specified by setTargetCloud ()
void
alignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results)
{
results.resize (templates_.size ());
for (size_t i = 0; i < templates_.size (); ++i)
{
align (templates_[i], results[i]);
}
}
// Align all of template clouds to the target cloud to find the one with best alignment score
int
findBestAlignment (TemplateAlignment::Result &result)
{
// Align all of the templates to the target cloud
std::vector<Result, Eigen::aligned_allocator<Result> > results;
alignAll (results);
// Find the template with the best (lowest) fitness score
float lowest_score = std::numeric_limits<float>::infinity ();
int best_template = 0;
for (size_t i = 0; i < results.size (); ++i)
{
const Result &r = results[i];
if (r.fitness_score < lowest_score)
{
lowest_score = r.fitness_score;
best_template = (int) i;
}
}
// Output the best alignment
result = results[best_template];
return (best_template);
}
private:
// A list of template clouds and the target to which they will be aligned
std::vector<FeatureCloud> templates_;
FeatureCloud target_;
// The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
float min_sample_distance_;
float max_correspondence_distance_;
int nr_iterations_;
};
int main()
{
// pcl::PolygonMesh::Ptr obj_in (new pcl::PolygonMesh);
// //Read obj file.
// if(pcl::io::loadPolygonFileOBJ("tree/tarotemplate.obj",*obj_in)==-1)
// {
// PCL_ERROR("Couldn't read file template.obj");
// return -1;
// }
// std::cout<<"Loaded "
// <<obj_in->cloud.width * obj_in->cloud.height
// << " data points: "
// << std::endl;
//Transform obj to source PCD.
pcl::PointCloud<pcl::PointXYZ>::Ptr tree_template(new pcl::PointCloud<pcl::PointXYZ>);
//pcl::fromROSMsg(obj_in->cloud, *tree_template);
pcl::io::loadPCDFile("source.pcd",*tree_template);
FeatureCloud object_template;
object_template.setInputCloud(tree_template);
//Load taget point cloud.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("target.pcd",*cloud);
FeatureCloud target_cloud;
target_cloud.setInputCloud(cloud);
TemplateAlignment template_align;
template_align.addTemplateCloud(object_template);
template_align.setTargetCloud(target_cloud);
TemplateAlignment::Result best_alignment;
template_align.align(object_template, best_alignment);
// Print the alignment fitness score (values less than 0.00002 are good)
printf ("fitness score: %f\n", best_alignment.fitness_score);
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf ("R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
// Save the aligned template for visualization
pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
pcl::transformPointCloud (*object_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);
pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud);
pcl::visualization::PCLHistogramVisualizer hViewer;
hViewer.addFeatureHistogram(*target_cloud.getLocalFeatures(),"fpfh",0);
hViewer.addFeatureHistogram(*object_template.getLocalFeatures(),"fpfh",0,"cloud1");
while(1)
{
hViewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
CMakeList.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(template_alignment)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (template_alignment template_alignment.cpp)
target_link_libraries (template_alignment ${PCL_LIBRARIES})
编译运行，得到结果：
参考
【3D】迭代最近点算法 Iterative Closest Points
ICP算法(Iterative Closest Point)及VTK实现
ICCV2011-registration 下载
ICCV2011-initial_registration 下载