Transform issue in buiding point cloud [closed]

asked 2012-02-05 22:53:24 -0600

alfa_80 gravatar image

updated 2014-01-28 17:11:17 -0600

ngrennan gravatar image

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("########################################################################");


          }
    }
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2012-06-09 11:23:05

Comments

Can you please add the error message to your question?
Lorenz gravatar imageLorenz ( 2012-02-05 23:20:43 -0600 )edit
Kindly refer as above. Thanks anyway.
alfa_80 gravatar imagealfa_80 ( 2012-02-05 23:30:12 -0600 )edit
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).
alfa_80 gravatar imagealfa_80 ( 2012-02-06 00:07:38 -0600 )edit
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
Lorenz gravatar imageLorenz ( 2012-02-06 00:39:54 -0600 )edit
Do you mean, I should have done as above(just edited)?
alfa_80 gravatar imagealfa_80 ( 2012-02-06 01:47:34 -0600 )edit
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).
Lorenz gravatar imageLorenz ( 2012-02-06 01:51:53 -0600 )edit
Could you also provide a backtrace of the failure by running the node in GDB if you're still having this issue?
DimitriProsser gravatar imageDimitriProsser ( 2012-02-06 02:44:48 -0600 )edit
@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.
alfa_80 gravatar imagealfa_80 ( 2012-02-06 03:01:49 -0600 )edit