ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

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!

asked 2020-04-08 10:57:56 -0500

Aadi gravatar image

updated 2020-04-08 11:41:34 -0500

/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;

}

edit retag flag offensive close merge delete

Comments

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.

ravijoshi gravatar image ravijoshi  ( 2020-04-19 09:16:40 -0500 )edit

Thanks. Appreciate the help guys.

Aadi gravatar image Aadi  ( 2020-04-23 15:25:16 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-04-18 13:10:49 -0500

Grimmyshini gravatar image

Hey! I suppose that assertion is failing due to the use of some uninitialized boost shared_ptr. Now, PCL internally uses boost shared_ptrs so, you might want to make sure all your PCL pointers are initialized too, like these

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned( new pcl::PointCloud<pcl::PointXYZ>() ); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src( new pcl::PointCloud<pcl::PointXYZ>() ); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt( new pcl::PointCloud<pcl::PointXYZ>() );

So try initializing these pointers using new and try recompiling your project and running again. It's always a good practice to initialize smart pointers right on declaration as specified in the following guide Boost Shared_ptr best practices

hope the advice helps!

edit flag offensive delete link more

Comments

Thanks for your answer. I figured the same. The problem was with the boost shared_ptrs. As they have discontinued using <pcl memory.h=""> library which was used handle them. Thanks again.

Aadi gravatar image Aadi  ( 2020-04-23 15:22:03 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-04-08 10:57:56 -0500

Seen: 643 times

Last updated: Apr 18 '20