Ask Your Question
0

Why is tf2::doTransform not transforming my pointcloud?

asked 2018-05-10 09:39:55 -0600

Hakaishin gravatar image

updated 2018-05-10 10:13:14 -0600

The function tf2::doTransform does not seem to transform my pointcloud. I made a minimal example to showcase my problem or maybe I am misunderstanding something.

In rviz i subscribe to both topics: "/realsense_zr300/points2" and "/world/source", but the two pointclouds lie on each other, why? Shouldn't they be different because the source is in the realsense frame and the transformed cloud is in the world frame? I also checked the data field of the input and output and they are the same, how can this be if I am applying a transform? The transform sets the frame_id of the output correctly to world, but does not change any value in the data. Attached are two pictures to clarify how things look like:

EDIT: the frame id of the input cloud is “realsense_zr300_ir_optical_frame“ and of the output cloud it is “world“. The first few points are exactly the same in both clouds, I am not at the pc currently, but the valued were something similiar to [0,0,~179, ~240, 0,0,~ 179, ~240...] when I echoed the messages and looked at the data field. Thank you for the good edit.

https://i.imgur.com/aQdxjsG.jpg https://i.imgur.com/UhXKNYg.png

rviz and gazebo

I excpected this transformation to transform the pointcloud onto the floor, or atleast to not be the same as the input cloud. What am I doing wrong?

main.cpp

#include "my_class.h"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test");
  MyClass mc;
  ros::spin();
  return 0;
}

my_class.h

#ifndef CPP_MY_CLASS_H
#define CPP_MY_CLASS_H

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/PointCloud2.h>

class MyClass
{
public:
  ros::NodeHandle nh_;
  tf2_ros::Buffer tfBuffer_;
  tf2_ros::TransformListener tfListener_;
  ros::Publisher W_source_pub_;
  ros::Subscriber points_sub_;

  MyClass();
  void point_cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& msg);
};

#endif //CPP_MY_CLASS_H

my_class.cpp

#include "my_class.h"

MyClass::MyClass() : tfListener_(tfBuffer_)
{
  points_sub_ = nh_.subscribe("/realsense_zr300/points2", 1, &MyClass::point_cloud_cb, this);
  W_source_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/world/source", 10);
}

void MyClass::point_cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& msg) {
  geometry_msgs::TransformStamped C_to_W_transform;
  try {
    C_to_W_transform = tfBuffer_.lookupTransform("world", "realsense_zr300_ir_optical_frame", ros::Time(0));
  }
  catch (tf2::TransformException &ex) {
    ROS_WARN("%s", ex.what());
    ros::Duration(1.0).sleep();
    return;
  }
  sensor_msgs::PointCloud2 w_cloud;
  std::cout << C_to_W_transform.transform.translation.x << std::endl;
  std::cout << C_to_W_transform.transform.translation.y << std::endl;
  std::cout << C_to_W_transform.transform.translation.z << std::endl;
  std::cout << C_to_W_transform.transform.rotation.x << std::endl;
  std::cout << C_to_W_transform.transform.rotation.y << std::endl;
  std::cout << C_to_W_transform.transform.rotation.z << std::endl;
  std::cout << C_to_W_transform.transform.rotation.w << std::endl;
  tf2::doTransform(*msg, w_cloud, C_to_W_transform);
  W_source_pub_.publish(w_cloud);
}

I wrote a gazebo model plugin that publishes the tf tree. To check it is correct I print the values of the transform I get and they are, as they should be:

x: 0 y: -0.5 z: 0.5

x: -0.189251 y: 0.184585 z: 0.690409 w: 0.673385

The update loop of the plugin looks like this:

void ModelTf::OnUpdate() {
    pose_ = this->model->GetWorldPose();
    transformStamped_.header.stamp = ros::Time::now();
    transformStamped_.header.frame_id = "world ...
(more)
edit retag flag offensive close merge delete

Comments

I reformatted so users would see the question and images before the wall of code/xml.

lucasw gravatar imagelucasw ( 2018-05-10 09:48:25 -0600 )edit

Print out the first 3 or 4 xyz values of the transformed and untransformed point clouds, and the frame ids of each cloud, and update your question with those values.

lucasw gravatar imagelucasw ( 2018-05-10 10:00:04 -0600 )edit
2

It makes sense that the point clouds in rviz are on top of each other, but the underlying point xyz values should be different. rostopic echo is printing raw bytes not the floating point values, so although the bytes should be different printing the xyz from code is more informative.

lucasw gravatar imagelucasw ( 2018-05-10 10:39:55 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2018-05-11 05:17:46 -0600

Hakaishin gravatar image

My problem was that the points were published in the realsense_zr300_ir_optical_frame which is a rotated frame. Maybe I am doing something wrong and the tf tree should be automatically published by the libgazebo_ros_openni_kinect.so plugin, but it is not publishing any tf information for me. I then made another frame kinect and rotated it RPY: -pi/2, 0, -pi/2 as said in this answer: https://answers.ros.org/question/5278...

So my final tf tree looks like this: world-> kinect_link-> realsense_zr300_ir_optical_frame where the transform between realsense_zr300_ir_optical_frame and kinect_link is RPY: -pi/2, 0, -pi/2 and between world and kinect_link like the one I get from this->model->GetWorldPose().

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-05-10 09:39:55 -0600

Seen: 392 times

Last updated: May 11 '18