Robotics StackExchange | Archived questions

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

Answers