fork download
copy 
  1. #include "stdafx.h"
  2. #undef max
  3. #undef min
  4.  
  5. #define _SCL_SECURE_NO_WARNINGS
  6. #include <boost/make_shared.hpp>
  7. #include <pcl/point_types.h>
  8. #include <pcl/point_cloud.h>
  9. #include <pcl/point_representation.h>
  10.  
  11. #include <pcl/io/pcd_io.h>
  12.  
  13. #include <pcl/filters/voxel_grid.h>
  14. #include <pcl/filters/filter.h>
  15.  
  16. #include <pcl/features/normal_3d.h>
  17.  
  18. #include <pcl/registration/icp.h>
  19. #include <pcl/registration/icp_nl.h>
  20. #include <pcl/registration/transforms.h>
  21.  
  22. #include <pcl/visualization/pcl_visualizer.h>
  23.  
  24. using pcl::visualization::PointCloudColorHandlerGenericField;
  25. using pcl::visualization::PointCloudColorHandlerCustom;
  26.  
  27. //convenient typedefs
  28. typedef pcl::PointXYZ PointT;
  29. typedef pcl::PointCloud<PointT> PointCloud;
  30. typedef pcl::PointNormal PointNormalT;
  31. typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
  32.  
  33. // This is a tutorial so we can afford having global variables 
  34. //our visualizer
  35. pcl::visualization::PCLVisualizer *p;
  36. //its left and right viewports
  37. int vp_1, vp_2;
  38.  
  39. //convenient structure to handle our pointclouds
  40. struct PCD
  41. {
  42. 	PointCloud::Ptr cloud;
  43. 	std::string f_name;
  44.  
  45. 	PCD() : cloud(new PointCloud) {};
  46. };
  47.  
  48. struct PCDComparator
  49. {
  50. 	bool operator () (const PCD& p1, const PCD& p2)
  51. 	{
  52. 		return (p1.f_name < p2.f_name);
  53. 	}
  54. };
  55.  
  56.  
  57. // Define a new point representation for < x, y, z, curvature >
  58. class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
  59. {
  60. 	using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
  61. public:
  62. 	MyPointRepresentation()
  63. 	{
  64. 		// Define the number of dimensions
  65. 		nr_dimensions_ = 4;
  66. 	}
  67.  
  68. 	// Override the copyToFloatArray method to define our feature vector
  69. 	virtual void copyToFloatArray(const PointNormalT &p, float * out) const
  70. 	{
  71. 		// < x, y, z, curvature >
  72. 		out[0] = p.x;
  73. 		out[1] = p.y;
  74. 		out[2] = p.z;
  75. 		out[3] = p.curvature;
  76. 	}
  77. };
  78.  
  79.  
  80. void pairAlignShort2(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
  81. {
  82. 	//
  83. 	// Downsample for consistency and speed
  84. 	// \note enable this for large datasets
  85. 	PointCloud::Ptr src(new PointCloud);
  86. 	PointCloud::Ptr tgt(new PointCloud);
  87. 	pcl::VoxelGrid<PointT> grid;
  88. 	if (downsample)
  89. 	{
  90. 		grid.setLeafSize(10, 10, 10);
  91. 		grid.setInputCloud(cloud_src);
  92. 		grid.filter(*src);
  93.  
  94. 		grid.setInputCloud(cloud_tgt);
  95. 		grid.filter(*tgt);
  96. 	}
  97. 	else
  98. 	{
  99. 		src = cloud_src;
  100. 		tgt = cloud_tgt;
  101. 	}
  102.  
  103.  
  104. 	// Compute surface normals and curvature
  105. 	PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
  106. 	PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);
  107.  
  108. 	pcl::NormalEstimation<PointT, PointNormalT> norm_est;
  109. 	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
  110. 	norm_est.setSearchMethod(tree);
  111. 	norm_est.setKSearch(30);
  112.  
  113. 	norm_est.setInputCloud(src);
  114. 	norm_est.compute(*points_with_normals_src);
  115. 	pcl::copyPointCloud(*src, *points_with_normals_src);
  116.  
  117. 	norm_est.setInputCloud(tgt);
  118. 	norm_est.compute(*points_with_normals_tgt);
  119. 	pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
  120.  
  121. 	//
  122. 	// Instantiate our custom point representation (defined above) ...
  123. 	MyPointRepresentation point_representation;
  124. 	// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
  125. 	float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
  126. 	point_representation.setRescaleValues(alpha);
  127.  
  128. 	//
  129. 	// Align
  130. 	pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
  131. 	reg.setTransformationEpsilon(1e-6);
  132. 	// Set the maximum distance between two correspondences (src<->tgt) to 10cm
  133. 	// Note: adjust this based on the size of your datasets
  134. 	reg.setMaxCorrespondenceDistance(50000);
  135. 	// Set the point representation
  136. 	reg.setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_representation));
  137.  
  138. 	reg.setInputSource(points_with_normals_src);
  139. 	reg.setInputTarget(points_with_normals_tgt);
  140.  
  141.  
  142.  
  143. 	//
  144. 	// Run the same optimization in a loop and visualize the results
  145. 	Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
  146. 	PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
  147. 	reg.setMaximumIterations(100);
  148. 	for (int i = 0; i < 100; ++i)
  149. 	{
  150. 		//PCL_INFO("Iteration Nr. %d.\n", i);
  151.  
  152. 		// save cloud for visualization purpose
  153. 		points_with_normals_src = reg_result;
  154.  
  155. 		// Estimate
  156. 		reg.setInputSource(points_with_normals_src);
  157. 		reg.align(*reg_result);
  158.  
  159. 		//accumulate transformation between each Iteration
  160. 		Ti = reg.getFinalTransformation() * Ti;
  161.  
  162. 		//if the difference between this transformation and the previous one
  163. 		//is smaller than the threshold, refine the process by reducing
  164. 		//the maximal correspondence distance
  165. 		if (fabs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())
  166. 			reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - reg.getMaxCorrespondenceDistance() / 100);
  167.  
  168. 		prev = reg.getLastIncrementalTransformation();
  169.  
  170. 		// visualize current state
  171. 		//showCloudsRight(points_with_normals_tgt, points_with_normals_src);
  172. 	}
  173.  
  174. 	//
  175. 	// Get the transformation from target to source
  176. 	targetToSource = Ti.inverse();
  177. 	/*
  178. 	//
  179. 	// Transform target back in source frame
  180. 	pcl::transformPointCloud(*cloud_tgt, *output, targetToSource);
  181.  
  182. 	p->removePointCloud("source");
  183. 	p->removePointCloud("target");
  184.  
  185. 	PointCloudColorHandlerCustom<PointT> cloud_tgt_h(output, 0, 255, 0);
  186. 	PointCloudColorHandlerCustom<PointT> cloud_src_h(cloud_src, 255, 0, 0);
  187. 	p->addPointCloud(output, cloud_tgt_h, "target", vp_2);
  188. 	p->addPointCloud(cloud_src, cloud_src_h, "source", vp_2);
  189.  
  190. 	PCL_INFO("Press q to continue the registration.\n");
  191. 	p->spin();
  192.  
  193. 	p->removePointCloud("source");
  194. 	p->removePointCloud("target");*/
  195.  
  196. 	//add the source to the transformed target
  197. 	*output += *cloud_src;
  198.  
  199. 	final_transform = targetToSource;
  200. }
  201.  
  202.  
  203.  
  204. void doTest2()
  205. {
  206. 	for (int t = 0; t < 100; t++)
  207. 	{
  208. 		PointCloud::Ptr source(new PointCloud);
  209. 		PointCloud::Ptr target(new PointCloud);
  210.  
  211. 		source->resize(10000);
  212. 		target->resize(10000);
  213. 		for (int i = 0; i < 10000; i++)
  214. 		{
  215. 			source->points[i].x = rand() % 500;
  216. 			source->points[i].y = rand() % 500;
  217. 			source->points[i].z = rand() % 500;
  218. 			target->points[i].x = rand() % 500;
  219. 			target->points[i].y = rand() % 500;
  220. 			target->points[i].z = rand() % 500;
  221. 		}
  222. 		source->resize(9002);
  223. 		target->resize(9022);
  224. 		PointCloud::Ptr temp(new PointCloud);
  225. 		Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity(), pairTransform;
  226. 		pairAlignShort2(source, target, temp, pairTransform, false);
  227. 	}
  228.  
  229. }
  230.  
  231. int main(int argc, char** argv)
  232. {
  233. 	doTest2();
  234.  
  235. }
Compilation error #stdin compilation error #stdout 0s 0KB
comments (?)
stdin
copy 
Standard input is empty
compilation info 
prog.cpp:1:20: fatal error: stdafx.h: No such file or directory
 #include "stdafx.h"
                    ^
compilation terminated.
stdout
copy 
Standard output is empty
https://ideone.com/MyxABM
language:
C++ (gcc 8.3)
created:
7 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!