Ask Your Question

Revision history [back]

Create a tf point from your Eigen point, query the tf server for the right transform, and then apply the transform.

Here's some code to get you started. (I haven't actually compiled this)

#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

// the point, in the camera frame
tf::Vector3 point(x, y, z);

// request the transform between the two frames
tf::TransformListener listener;
tf::StampedTransform transform;
try{
  listener.lookupTransform("/base_link", "/camera_depth_optical_frame",  
                            ros::Time::now(), transform);
}
catch (tf::TransformException ex) {
  ROS_WARN("Base to camera transform unavailable %s", ex.what());
}

// the point, in the base link frame
tf::Vector3 point_bl = transform * point;

If the transform doesn't change, you probably want to cache it once, and then apply it to your data without querying it over and over.

Create a tf point from your Eigen point, query the tf server for the right transform, and then apply the transform.

Here's some code to get you started. (I haven't actually compiled this)

#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

// the point, in the camera frame
tf::Vector3 point(x, y, z);

// request the transform between the two frames
tf::TransformListener listener;
tf::StampedTransform transform;
ros::Time time = ros::Time::now();

try{
  listener.waitForTransform("/base_link", "/camera_depth_optical_frame", 
                            time, ros::Duration(3.0));
  listener.lookupTransform("/base_link", "/camera_depth_optical_frame",  
                            ros::Time::now(), time, transform);
}
catch (tf::TransformException ex) {
  ROS_WARN("Base to camera transform unavailable %s", ex.what());
}

// the point, in the base link frame
tf::Vector3 point_bl = transform * point;

If the transform doesn't change, you probably want to cache it once, and then apply it to your data without querying it over and over.

Create a tf point from your Eigen point, query the tf server for the right transform, and then apply the transform.

Here's some code to get you started. (I haven't actually compiled this)

#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

// the point, in the camera frame
tf::Vector3 point(x, y, z);

// request the transform between the two frames
tf::TransformListener listener;
tf::StampedTransform transform;

// the time to query the server is the stamp of the incoming message
ros::Time time = ros::Time::now();
cloud_cup->header.stamp;

// frame of the incoming message
std::string frame = cloud_cup->header.frame_id;

try{
  listener.waitForTransform("/base_link", "/camera_depth_optical_frame", 
                            frame, time, ros::Duration(3.0));
  listener.lookupTransform("/base_link", "/camera_depth_optical_frame",  
                            frame, time, transform);
}
catch (tf::TransformException ex) {
  ROS_WARN("Base to camera transform unavailable %s", ex.what());
}

// the point, in the base link frame
tf::Vector3 point_bl = transform * point;

If the transform doesn't change, you probably want to cache it once, and then apply it to your data without querying it over and over.