Transformation of Coordinates Question

asked 2013-02-25 12:16:13 -0600

MartinW
464 ●56 ●66 ●77

updated 2013-02-26 06:14:59 -0600

Hello all,

I am trying to transform between my Kinect's /camera_depth_optical_frame and my robot's /base_link frame. I have an Eigen::Vector4f which contains a position (x,y,z) of my point cloud in the Kinect's frame but to use the inverse kinematics services I need to transform this into the /base_link coordinate system.

Using tf_echo I know the position/rotation transformation between these two links, I just need to physically change the x,y,z values into other x,y,z values. Does anyone know a way to do this?

EDIT: Using Ivan's answer I have updated my code but get different errors now.

Here is what I have outputting to the console:

[ INFO] [1361845870.463119633, 1361845870.461467027]: Centroid of cloud cluster is: (-0.049989, 0.026934, 0.519591)
[ WARN] [1361845870.466575058, 1361845870.461467027]: Base to camera transform unavailable Frame id /base_link does not exist! Frames (1): 
[ INFO] [1361845870.466641668, 1361845870.461467027]: 40837646868892605535297603077151234559928972087249378994085663878457812789968315215666377820143542193373920416857156869752951414053701443003877027669632883493287576239311400849870068362363273131362348953463857542966647618830627681059177758720.000000 -0.000000 0.000000

The code that corresponds to this output is:

Eigen::Vector4f centroid; 

pcl17::compute3DCentroid(*cloud_cup,centroid);
pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr cloud_cent (new pcl17::PointCloud<pcl17::PointXYZRGB>);
ROS_INFO("Centroid of cloud cluster is: (%f, %f, %f)",centroid[0],centroid[1],centroid[2]);

cloud_cent->points.resize(1);
cloud_cent->points[0].x = centroid[0];
cloud_cent->points[0].y = centroid[1];
cloud_cent->points[0].z = centroid[2];


tf::Vector3 point(centroid[0],centroid[1],centroid[2]);

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());
    }

tf::Vector3 point_bl = transform * point;

ROS_INFO("%f %f %f", point_bl[0],point_bl[1],point_bl[2]);

So it says my transformation doesn't exist but in my .urdf I have defined the joint from the base_link to the camera_link. When I run tf tf_echo /base_link /camera_depth_optical_frame I get an output of:

rosrun tf tf_echo /base_link /camera_depth_optical_frame
Failure at 1361845642.173521041
Exception thrown:Frame id /camera_depth_optical_frame does not exist! Frames (17): Frame /camera_link exists with parent /base_link.
Frame /base_link exists with parent NO_PARENT.
Frame /head_link exists with parent /base_link.
Frame /left_rotator_link exists with parent /left_shoulder_link.
Frame /left_shoulder_link exists with parent NO_PARENT.
Frame /left_upper_bicep_link exists with parent /left_tricep_link.
Frame /left_tricep_link exists with parent NO_PARENT.
Frame /left_lower_forearm_link exists with parent /left_upper_forearm_link.
Frame /left_upper_forearm_link exists with parent NO_PARENT.
Frame /right_rotator_link exists with parent /right_shoulder_link.
Frame /right_shoulder_link exists with parent NO_PARENT.
Frame /right_upper_bicep_link exists with parent /right_tricep_link.
Frame /right_tricep_link exists with parent NO_PARENT.
Frame /right_lower_forearm_link exists with parent /right_upper_forearm_link.
Frame /right_upper_forearm_link exists with parent NO_PARENT.
Frame /stand_link exists with parent /base_link.

The current list of frames is:
Frame /camera_link exists with parent /base_link.
Frame /base_link exists with parent NO_PARENT.
Frame /head_link exists with parent /base_link.
Frame /left_rotator_link exists with parent /left_shoulder_link.
Frame /left_shoulder_link exists with parent NO_PARENT.
Frame /left_upper_bicep_link exists with parent /left_tricep_link.
Frame /left_tricep_link exists with parent NO_PARENT.
Frame /left_lower_forearm_link exists with parent /left_upper_forearm_link.
Frame /left_upper_forearm_link exists with parent NO_PARENT.
Frame /right_rotator_link exists with parent /right_shoulder_link.
Frame /right_shoulder_link exists with parent NO_PARENT.
Frame ...

(more)

edit retag flag offensive close merge delete

Comments

The time must be a ros::Time, not a time_t.

dornhege ( 2013-02-26 06:09:21 -0600 )edit

Thanks, this worked too! I added an update but using just the four arguments worked well too. I guess it takes those as default values? Cheers!

MartinW ( 2013-02-26 06:17:09 -0600 )edit

Actually the version with two time stamps is a different version. It should give you the same, when using the same time stamp for both times (although technically you don't do this here!)

dornhege ( 2013-02-26 06:49:51 -0600 )edit

Note that I added another line in the edit: "ros::Time time = ros::Time::now();". The I pass that time to both waitForTransform and lookUptransform

Ivan Dryanovski ( 2013-02-26 06:52:49 -0600 )edit

Actually, given the application he should use the time stamp of the incoming point cloud here.

dornhege ( 2013-02-26 07:03:08 -0600 )edit

@dornhege Yes, good point. I've revised my answer. Also this reminds me that it's not good to hardcode the frame of the cloud - it should be taken form the header. Also revised in answer

Ivan Dryanovski ( 2013-02-26 07:17:47 -0600 )edit

add a comment

answered 2013-02-25 13:39:23 -0600

Ivan Dryanovski

4954 ●52 ●70 ●86 http://robotics.ccny.cuny.edu/

updated 2013-02-26 07:16:52 -0600

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 = cloud_cup->header.stamp;

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

try{
  listener.waitForTransform("/base_link", frame, time, ros::Duration(3.0));
  listener.lookupTransform("/base_link", 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.

edit flag offensive delete link

Comments

Hey Ivan, thanks for the answer! The code compiled correctly but now I have strange errors at run time. I have edited the previous question. If you could provide me with some more insight that would be fantastic!

MartinW ( 2013-02-25 14:30:18 -0600 )edit

Check the edit, it might help. I added a waitForTransform.

Ivan Dryanovski ( 2013-02-25 15:22:04 -0600 )edit

I tried to add this line but I am getting a compile error now, it expects 8 arguments but only 4 are provided. Do I need to include a different header file? I put the error in the EDIT2. Thanks for all your help Ivan!

MartinW ( 2013-02-26 06:01:17 -0600 )edit

Revisions to answer to account for correct time and frame when querying tf server

Ivan Dryanovski ( 2013-02-26 07:18:47 -0600 )edit

add a comment

Transformation of Coordinates Question

Comments

1 Answer

Comments

Question Tools

Stats

Transformation of Coordinates Question edit

Comments

1 Answer

Comments

Question Tools

Stats

Transformation of Coordinates Question