I am trying to load two .pcd files and match them using ICP , given the tutorial from the PCL website. After Compiling and running I get the following error, Previously I was using Python , I am beginner in C++, Detailed help appreciated. Thanks!
/usr/include/boost/smart_ptr/shared_ptr.hpp:728: typename boost::detail::sp_dereference<t>::type boost::shared_ptr<t>::operator*() const [with T = pcl::PointCloud<pcl::pointxyz>; typename boost::detail::sp_dereference<t>::type = pcl::PointCloud<pcl::pointxyz>&]: Assertion `px != 0' failed. Aborted (core dumped)
:: Here is the link to the dataset.
https://github.com/PointCloudLibrary/...
int main(int argc, char **argv) {
ros::init(argc, argv, "pcl_registration");
ros::NodeHandle nh;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_matched",1);
ros::Publisher cloud_A = nh.advertise<sensor_msgs::PointCloud2>("cloud_1",1);
ros::Publisher cloud_B = nh.advertise<sensor_msgs::PointCloud2>("cloud_2",1);
sensor_msgs::PointCloud2 output;
sensor_msgs::PointCloud2 cloud_1_out;
sensor_msgs::PointCloud2 cloud_2_out;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt;
PointCloud::Ptr src (new PointCloud);
PointCloud::Ptr tgt (new PointCloud);
pcl::io::loadPCDFile("capture0002.pcd",*cloud_tgt);
pcl::io::loadPCDFile("capture0003.pcd",*cloud_src);
// Implements filtering & downsampling of Cloud 1
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
statFilter.setInputCloud(cloud_src);
statFilter.setMeanK(10);
statFilter.setStddevMulThresh(0.2);
statFilter.filter(*cloud_src);
pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
voxelSampler.setInputCloud(cloud_src);
voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
voxelSampler.filter(*src);
// Implements filtering & downsampling of Cloud 2
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter2;
statFilter2.setInputCloud(cloud_tgt);
statFilter2.setMeanK(10);
statFilter2.setStddevMulThresh(0.2);
statFilter2.filter(*cloud_tgt);
pcl::VoxelGrid<pcl::PointXYZ> voxelSampler2;
voxelSampler2.setInputCloud(cloud_tgt);
voxelSampler2.setLeafSize(0.01f, 0.01f, 0.01f);
voxelSampler2.filter(*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);
// Implements IPC Algorithm for Registration 1 & 2
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> icp;
icp.setInputSource(points_with_normals_src);
icp.setInputTarget(points_with_normals_tgt);
icp.setMaxCorrespondenceDistance(0.1);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon (1e-6);
icp.setEuclideanFitnessEpsilon(0.1);
Eigen::Matrix4f final_transform;
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
for (int i = 0; i < 30; ++i) {
ROS_INFO ("Iteration Nr. %d.\n", i);
// save cloud for visualization purpose
points_with_normals_src = reg_result;
// Estimate
icp.setInputSource (points_with_normals_src);
icp.align (*reg_result);
//accumulate transformation between each Iteration
Ti = icp.getFinalTransformation () * Ti;
if (std::abs ((icp.getLastIncrementalTransformation () - prev).sum ()) < icp.getTransformationEpsilon ())
icp.setMaxCorrespondenceDistance (icp.getMaxCorrespondenceDistance () - 0.001);
prev = icp.getLastIncrementalTransformation ();}
// Get the transformation from target to source
targetToSource = Ti.inverse();
// Transform target back in source frame
pcl::transformPointCloud (*cloud_tgt, *cloud_aligned, targetToSource);
//add the source to the transformed target
*cloud_aligned += *cloud_src;
final_transform = targetToSource;
pcl::toROSMsg(*cloud_aligned, output);
output.header.frame_id = "odom";
pcl::toROSMsg(*cloud_src, cloud_1_out);
cloud_1_out.header.frame_id = "odom";
pcl::toROSMsg(*cloud_tgt, cloud_2_out);
cloud_2_out.header.frame_id = "odom";
while(ros::ok){
pcl_pub.publish(output);
cloud_A.publish(cloud_1_out);
cloud_B.publish(cloud_2_out);
ros::spinOnce();
}
return 0;
}
As @Grimmyshini has pointed out that you need to initialize your pointers which is causing the problem. Therefore, I am adding a reference for you to help. Please check here. You can get a complete picture of using
pcl::PointCloud
. Please let me know if the issue persists.Thanks. Appreciate the help guys.