#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//簡單型別定義
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
//這是一個輔助教程，因此我們可以負擔全域變數
//創建可視化工具
pcl::visualization::PCLVisualizer *p;
//定義左右視點
int vp_1, vp_2;
//處理點云的方便的結構定義
struct PCD
{
PointCloud::Ptr cloud;
std::string f_name;
PCD() : cloud (new PointCloud) {};
};
struct PCDComparator
{
bool operator () (const PCD& p1, const PCD& p2)
{
return (p1.f_name < p2.f_name);
}
};
//以< x, y, z, curvature >形式定義一個新的點
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
MyPointRepresentation ()
{
//定義尺寸值
nr_dimensions_ = 4;
}
//覆寫copyToFloatArray方法來定義我們的特征矢量
virtual void copyToFloatArray (const PointNormalT &p, float * out) const
{
// < x, y, z, curvature >
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
out[3] = p.curvature;
}
};
////////////////////////////////////////////////////////////////////////////////
/** 在可視化視窗的第一視點顯示源點云和目標點云
*
*/
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
p->removePointCloud ("vp1_target");
p->removePointCloud ("vp1_source");
PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0);
p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1);
p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1);
PCL_INFO ("Press q to begin the registration.\n");
p-> spin();
}
////////////////////////////////////////////////////////////////////////////////
/**在可視化視窗的第二視點顯示源點云和目標點云
*
*/
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{
p->removePointCloud ("source");
p->removePointCloud ("target");
PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature");
if (!tgt_color_handler.isCapable ())
PCL_WARN ("Cannot create curvature color handler!");
PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature");
if (!src_color_handler.isCapable ())
PCL_WARN ("Cannot create curvature color handler!");
p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2);
p->addPointCloud (cloud_source, src_color_handler, "source", vp_2);
p->spinOnce();
}
////////////////////////////////////////////////////////////////////////////////
/**加載一組我們想要匹配在一起的PCD檔案
* 引數argc是引數的數量 (pass from main ())
*引數 argv 實際的命令列引數 (pass from main ())
*引數models點云資料集的合成矢量
*/
void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models)
{
std::string extension (".pcd");
//假定第一個引數是實際測驗模型
for (int i = 1; i < argc; i++)
{
std::string fname = std::string (argv[i]);
// 至少需要5個字符長（因為.plot就有 5個字符）
if (fname.size () <= extension.size ())
continue;
std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);
//檢查引數是一個pcd檔案
if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0)
{
//加載點云并保存在總體的模型串列中
PCD m;
m.f_name = argv[i];
pcl::io::loadPCDFile (argv[i], *m.cloud);
//從點云中移除NAN點
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);
models.push_back (m);
}
}
}
////////////////////////////////////////////////////////////////////////////////
/**匹配一對點云資料集并且返還結果
*引數 cloud_src 是源點云
*引數 cloud_src 是目標點云
*引數output輸出的配準結果的源點云
*引數final_transform是在來源和目標之間的轉換
*/
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
//
//為了一致性和高速的下采樣
//注意：為了大資料集需要允許這項
PointCloud::Ptr src (new PointCloud);
PointCloud::Ptr tgt (new PointCloud);
pcl::VoxelGrid<PointT> grid;
if (downsample)
{
grid.setLeafSize (0.05, 0.05, 0.05);
grid.setInputCloud (cloud_src);
grid.filter (*src);
grid.setInputCloud (cloud_tgt);
grid.filter (*tgt);
}
else
{
src = cloud_src;
tgt = cloud_tgt;
}
//計算曲面法線和曲率
PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);
pcl::NormalEstimation<PointT, PointNormalT> norm_est;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
norm_est.setSearchMethod (tree);
norm_est.setKSearch (30);
norm_est.setInputCloud (src);
norm_est.compute (*points_with_normals_src);
pcl::copyPointCloud (*src, *points_with_normals_src);
norm_est.setInputCloud (tgt);
norm_est.compute (*points_with_normals_tgt);
pcl::copyPointCloud (*tgt, *points_with_normals_tgt);
//
//舉例說明我們自定義點的表示（以上定義）
MyPointRepresentation point_representation;
//調整'curvature'尺寸權重以便使它和x, y, z平衡
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues (alpha);
//
// 配準
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
reg.setTransformationEpsilon (1e-6);
//將兩個對應關系之間的(src<->tgt)最大距離設定為10厘米
//注意：根據你的資料集大小來調整
reg.setMaxCorrespondenceDistance (0.1);
//設定點表示
reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputCloud (points_with_normals_src);
reg.setInputTarget (points_with_normals_tgt);
//
//在一個回圈中運行相同的最優化并且使結果可視化
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations (2);
for (int i = 0; i < 30; ++i)
{
PCL_INFO ("Iteration Nr. %d.\n", i);
//為了可視化的目的保存點云
points_with_normals_src = reg_result;
//估計
reg.setInputCloud (points_with_normals_src);
reg.align (*reg_result);
//在每一個迭代之間累積轉換
Ti = reg.getFinalTransformation () * Ti;
//如果這次轉換和之前轉換之間的差異小于閾值
//則通過減小最大對應距離來改善程式
if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
prev = reg.getLastIncrementalTransformation ();
//可視化當前狀態
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}
//
// 得到目標點云到源點云的變換
targetToSource = Ti.inverse();
//
//把目標點云轉換回源框架
pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
p->removePointCloud ("source");
p->removePointCloud ("target");
PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);
p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
p->spin ();
p->removePointCloud ("source");
p->removePointCloud ("target");
//添加源點云到轉換目標
*output += *cloud_src;
final_transform = targetToSource;
}
/* [ */
int main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr target1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> output;
pcl::io::loadPCDFile (argv[1], *target1);
pcl::io::loadPCDFile (argv[2], output);
// 加載資料
std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
PCD m;
std::vector<int> indices;
m.cloud = target1;
//從點云中移除NAN點
pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);
data.push_back (m);
m.cloud = output.makeShared();
pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);
data.push_back (m);
//檢查用戶輸入
if (data.empty ())
{
return (-1);
}
PCL_INFO ("Loaded %d datasets.", (int)data.size ());
PointCloud::Ptr result (new PointCloud), source, target;
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
for (size_t i = 1; i < data.size (); ++i)
{
source = data[i-1].cloud;
target = data[i].cloud;
PointCloud::Ptr temp (new PointCloud);
PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
pairAlign (source, target, temp, pairTransform, true);
//把當前的兩兩配對轉換到全域變換
pcl::transformPointCloud (*temp, *result, GlobalTransform);
//update the global transform更新全域變換
GlobalTransform = pairTransform * GlobalTransform;
//保存配準對，轉換到第一個點云框架中
std::stringstream ss;
ss << i << ".pcd";
pcl::io::savePCDFile (ss.str (), *result, true);
}
}
轉載請註明出處，本文鏈接：https://www.uj5u.com/houduan/69118.html
標籤：數據庫及相關技術
上一篇：動態規劃：最大子串和