Transform issue in buiding point cloud
I've configuring a transform for accumulating clouds over time. In the code snippet below, temp_cloud is a local accumulator, cloud the final cloud accumulator(but not yet transformed), cloud2 the final cloud accumulator that should have been transformed.
It's compiled successfully, but when I run it, the node is killed. I suspect this is because I wrongly assigned my transform listener in the pcl_ros::transformPointCloud call.
The pcl_ros::transformPointCloud have just been introduced as I only obtained 2D point cloud by which it should be in 3D point cloud (Kindly refer here).
How do I resolve this?
Thanks in advance.
The code snippet:
//Class member variables
sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2 cloud2;
laser_geometry::LaserProjection projector_;
tf::TransformListener tfListener_;
//Resides in a callback function
{
sensor_msgs::PointCloud2 c_tmp;
sensor_msgs::PointCloud2 temp_cloud;
cloud.header.frame_id = "/cloud";
cloud.header.stamp = pose3D_LDD.header.stamp;
c_tmp.header.frame_id = "/c_tmp";
c_tmp.header.stamp = pose3D_LDD.header.stamp;
temp_cloud.header.frame_id = "/temp_cloud";
temp_cloud.header.stamp = pose3D_LDD.header.stamp;
static tf::TransformBroadcaster tfb;
tf::Transform xform;
xform.setOrigin(tf::Vector3(pose3D_LDD.pose.position.x, pose3D_LDD.pose.position.y, pose3D_LDD.pose.position.z));
xform.setRotation(tf::Quaternion(pose3D_LDD.pose.orientation.x, pose3D_LDD.pose.orientation.y, pose3D_LDD.pose.orientation.z, pose3D_LDD.pose.orientation.w));
tfb.sendTransform(tf::StampedTransform(xform, pose3D_LDD.header.stamp, "/world", "/base_link"));
tfListener_.waitForTransform("/world", "/base_link", pose3D_LDD.header.stamp, ros::Duration(1.0));
try
{
projector_.transformLaserScanToPointCloud("/laser", previous_scan_, c_tmp, tfListener_);
if (!initialized_cloud_)
{
cloud = c_tmp;
initialized_cloud_ = true;
}
else
{
pcl::concatenatePointCloud(cloud, c_tmp, temp_cloud);
cloud = temp_cloud;
pcl_ros::transformPointCloud("/laser", cloud, cloud2, tfListener_);
ROS_INFO("From c_tmp: Got cloud with %u of width and %u of height", c_tmp.width, c_tmp.height);
ROS_INFO("From temp_cloud: Got cloud with %u of width and %u of height", temp_cloud.width, temp_cloud.height);
ROS_INFO("From cloud: Got cloud with %u of width and %u of height", cloud.width, cloud.height);
ROS_INFO("########################################################################");
}
}
catch (tf::TransformException ex)
{
ROS_WARN("Warning: %s", ex.what());
}
}
EDIT
Error message:
pointcloud_builder_node: /usr/include/eigen3/Eigen/src/Core/CwiseBinaryOp.h:144: Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::CwiseBinaryOp(const Lhs&, const Rhs&, const BinaryOp&) [with BinaryOp = Eigen::internal::scalar_product_op<double, double>, Lhs = const Eigen::Array<double, -0x00000000000000001, -0x00000000000000001, 0, -0x00000000000000001, -0x00000000000000001>, Rhs = const Eigen::Array<double, -0x00000000000000001, -0x00000000000000001, 0, -0x00000000000000001, -0x00000000000000001>]: Assertion `lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()' failed.
[pointcloud_builder_node-6] process has died [pid 27970, exit code -6].
Improved code:
try
{
projector_.transformLaserScanToPointCloud("/base_link", previous_scan_, c_tmp, tfListener_);
if (!initialized_cloud_)
{
cloud = c_tmp;
initialized_cloud_ = true;
}
else
{
pcl::concatenatePointCloud(cloud, c_tmp, temp_cloud);
cloud = temp_cloud;
pcl_ros::transformPointCloud("/base_link", cloud, cloud2, tfListener_);
ROS_INFO("From c_tmp: Got cloud with %u of width and %u of height", c_tmp.width, c_tmp.height);
ROS_INFO("From temp_cloud: Got cloud with %u of width and %u of height", temp_cloud.width, temp_cloud.height);
ROS_INFO("From cloud: Got cloud with %u of width and %u of height", cloud.width, cloud.height);
ROS_INFO("########################################################################");
}
}
Asked by alfa_80 on 2012-02-05 23:53:24 UTC
Comments
@Lorenz and @DimitriProsser: Could we move our discussion to my older thread(http://answers.ros.org/question/3855/strange-output-of-point-cloud-building) , because over there is actually easier to debug the problem for I don't include pcl_ros::transformPointCloud.
Asked by alfa_80 on 2012-02-06 04:09:44 UTC
@DimitriProsser: Do you think, I need to transform the cloud as I did above using pcl_ros::transformPointCloud?
Asked by alfa_80 on 2012-02-06 04:05:47 UTC
If you don't get a three dimensional scan, your tf tree is probably wrong. Did you verify that your transform from base_link to laser (I assume your laser data is in the laser frame) is definitely changing over time?
Asked by Lorenz on 2012-02-06 04:05:01 UTC
Then, I use pcl_ros::transformPointCloud to transform the cloud, if I ever need it. But, then, I'm still not sure whether I really need it or not.
Asked by alfa_80 on 2012-02-06 04:04:09 UTC
@Lorenz: Yes, I want to gather the cloud(I've converted using transformLaserScanToPointCloud) over time. The cloud is gathered already over time, but then I only got 2D of them. Something wrong somewhere I think.
Asked by alfa_80 on 2012-02-06 04:01:49 UTC
Could you also provide a backtrace of the failure by running the node in GDB if you're still having this issue?
Asked by DimitriProsser on 2012-02-06 03:44:48 UTC
That makes more sense to me. It would really help if you explain what exactly you are trying to do. If it's just creating one big point cloud from a bunch of laser scans, you can use point_cloud_assembler (http://ros.org/wiki/laser_assembler).
Asked by Lorenz on 2012-02-06 02:51:53 UTC
Do you mean, I should have done as above(just edited)?
Asked by alfa_80 on 2012-02-06 02:47:34 UTC
To me, it is not clear what you are trying to achieve with your transforms. Why are you publishing /world->base_link but then building the laser cloud in the laser frame. Then you call transformPointCloud with target /laser which should't have any effect. Why don't you transform it to base_link
Asked by Lorenz on 2012-02-06 01:39:54 UTC
pcl_ros::transformPointCloud has just been introduced as I faced an issue as described here(http://answers.ros.org/question/3855/strange-output-of-point-cloud-building).
Asked by alfa_80 on 2012-02-06 01:07:38 UTC
Kindly refer as above. Thanks anyway.
Asked by alfa_80 on 2012-02-06 00:30:12 UTC
Can you please add the error message to your question?
Asked by Lorenz on 2012-02-06 00:20:43 UTC