0
$\begingroup$

Rosanswers logo

EDIT - TLDR: If I have a singular source of absolute odometry providing x,y,z,roll,pitch & yaw (visual odom), how do I account for the camera's offset from base_link? Simply setting up a base_link -> zed_frame (zed is my camera) transform doesn't work, because the odometry data is published under the odom frame. EG: My camera is mounted at the center of my robot at a 45 degree pitch incline. If I move the robot in the x direction by 10 units, my odometry reports an offset of ( 10Sin(45) 0 10Cos(45) ) which gets fed to the filter and misplaces my odom -> base_link transform.

I have tried having an intermediate frame, zed_initial_frame. In this scenario my visual odometry is published under the zed_initial_frame, and a static tf with a rotation of 45 deg pitch links odom to zed_initial_frame. Now moving the robot 10 units in x results in the odometry pose being(10 0 0); HOWEVER, the base_link frame now gets reported in line with zed_initial_frame, i.e at 45 deg pitch. This is because at t (time) = 0, the odometry reports a 0 (eigen) transform. But this is then transformed to the odom frame, resulting in the initial odometry reading showing a 45 deg pitch. This is where I possibly need to point the finger at robot_localization, because even setting the odom0_relative parameter to true, doesn't fix this. Here is a the potential offending source code, my theory is that with the variable set to true, the first measurement is set to zero in section 7g, and only then transformed in 7h, resulting in a non-zero initial measurement after all due to the rotation.

Original Question: Hello, I have been stuck on this for days and am sure I am fundamentally missing something. I have read:

http://www.ros.org/reps/rep-0103.html
http://www.ros.org/reps/rep-0105.html
http://wiki.ros.org/robot_localization
http://answers.ros.org/question/235228/how-is-the-orientation-of-frame-odom-initialized/
http://answers.ros.org/question/239198/robot_localization-does-not-produce-odom-base_link-transform/
http://answers.ros.org/question/237295/confused-about-coordinate-frames-can-someone-please-explain/
http://answers.ros.org/question/216750/robot_localization-how-to-set-up-tf/
http://library.isr.ist.utl.pt/docs/roswiki/hector_slam(2f)Tutorials(2f)SettingUpForYourRobot.html

Yet I am unable to produce the results I want. My scenario:

Physically my set up is the ZED Camera mounted on a tripod at a 45 degree inclination downwards (pitch = 45). This is because I would like to see the floor in front of me. it is roughly 1.1m off the ground.

What I am trying to achieve: Get everything to line up in the visualizer in terms of transforms and frames. The ZED ouputs all data in the frame zed_tracked_frame, except for the zed_tracked_frame itself, which I have tried in the zed_initial_frame or odom frame as parent. My launch files (and config files):

Some example output TF trees, the first is with my static tf linking base_link to zed_initial_frame, the other with is linking base_link to zed_tracked_frame:

TF_Tree_01

TF_Tree_02

Here are some pics from RVIZ:

Initial startup - Everything is fine and aligned
Here you can see the initial frames. odom and base_link are the ones flat with the ground plane, the zed_initial_frame and zed_tracked_frame are the ones pointing down at 45 degrees. Worth noting: the green arrow is that of the odom camera, pre filter. Whilst this should (I think) be pointing in line with the zed_tracked_frame, it isn't because I had to change the camera's odometry parent frame to odom. If I put it to zed_initial_frame i get: " Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees." Which it is. odom, zed_initial_frame and zed_tracked_frame are now not linked to base_link, despite my static TF still being there.

Rotated the camera by 90 degrees YAW ONLY - Note that the base link as been rotated about an offset axis (Using TF_Tree_02)
Here You can see base_link has diverged downwards from zed_tracked_frame. it has not been translated by the same offset.

Same as above, rotated camera YAW only (using TF_Tree_01)

TLDR: Please help me understand how to properly setup my frames and transforms. I've tried to find out how but am not able to hack it.

Thanks in advance. Let me know if any critical info is missing. I've tried to be as thorough as I can be.

EDIT 26/07/2016: Fixed signs on pitch values (should be positive, in ENU Frame).


Originally posted by NZNobody on ROS Answers with karma: 156 on 2016-07-12

Post score: 2


Original comments

Comment by NZNobody on 2016-07-12:
I think I have found the cause of the issue, but am unsure who (which package) should be responsible for correcting it. I will edit my question accordingly.

If my obsolete odometer is offset from my base_link, how and who accounts for this offset?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

It appears your stereo camera doesn't support velocities, as that would be a solution. You could use a static transform from base_link in that case.

The short answer is that the EKF and UKF in r_l don't do a great job of handling sensors that are measuring world-frame data from an offset position. I've run into this same issue with UUVs and depth sensors.

In any case, if you define a transform directly from odom, it can't be static, as it will be dependent on your pose in the odom frame. Relative mode won't help you either, as that's simply meant to remove the first pose measurement from a given sensor so that it effectively starts at 0. Differential mode converts subsequent poses to velocities, but then doesn't (and I'm not convinced it should) apply the transform to base_link afterwards.

So there are three options:

  1. Write a quick node that computes the odom->zed transform dynamically, and run everything else as-is
  2. Modify the camera driver to output velocities in its odometry message
  3. Add some kind of parameter to r_l for pose sensors that tell it that it needs to dynamically adjust the "static" transform from odom based on the vehicle's current pose.

EDIT in response to comments:

I think there are a couple of unknowns for me. First, you said your camera is mounted at -45 degrees pitch, which would point it at the ceiling (positive pitch is down in an ENU frame). Second, even if the camera is pointed down at 45 degrees, does that make a difference in the reported odometry? For example, if you aim the camera down 45 degrees toward the floor and drive straight forward for 10 meters, what do you get for reported odometry? Does it say you are at (10, 0, 0) or (7.07, 0, 7.07)? I think I will know how to address this once I have that information.

EDIT in response to updates:

OK, so the raw output from the camera (not passed through any transform), when you drive forward 10 meters, reports that you at ~(7.07, 0, 7.07). There are two things to consider:

  1. Imagine your camera wasn't inclined, so it was facing straight forward. Then, when driving straight forward or backward, the pose data from the camera would be correct (though instead of going from X = 0 to N, the robot's X position would be 0 - linear_offset to N - linear_offset). However, when turning, the camera would register linear motion as a result of its offset and the rotational motion. In the most extreme case, the robot is turning in place, and the camera is registering its pose change along the circle with radius r, which is equal to the X/Y distance from the center of rotation.

Assuming you have defined a static transform from base_link->zed_frame, since the camera is measuring pose data, the standard method of applying the transform from the zed_frame frame to the odom frame won't work. This is fundamentally because the sensor isn't measuring data that is relative to the sensor (like, e.g., a laser), but rather it is measuring global pose data. So if you take the pose as estimated by your camera and try to transform it through base_link to odom, the output will be incorrect. For example, let's say your robot is at (4.8, 0) with a yaw of 0, and that data was obtained through means other than the camera. Then the first camera measurement comes in, and it says we're at (5, 0). That gets passed through the zed_frame->base_link transform, which says that we have a linear offset of 0.2 meters, so we get (4.8, 0). Then it goes through the base_link->odom transform and becomes (9.8, 0), which is wrong.

Instead, what should happen is that we need to take the offset from the centroid, which is currently defined in the base_link->zed_frame transform, and apply only the rotation of the odom->base_link transform to it, then use the resulting transform on the measured pose data. That way, if the robot has a yaw of 0, we remove the linear offset of (0.2, 0) meters to get the pose of the robot. If it has a yaw of 45, we correctly remove (0.14, 0.14).

So: what you need is to do the above yourself: take the camera offset from the robot centroid, rotate it by the rotation in the odom->base_link transform, then apply the "corrected" linear offset/transform to the camera's pose, and that becomes your measurement. In order to facilitate this, you need a node that is generating the corrected transform (rotating the linear offset, then broadcasting the transform as a child of odom, not base_link). Then r_l will apply that transform and all (should) be well, assuming you also handle (2).

  1. I think the rotational element to your camera's offset should just be a matter of rotating the camera's pose by 45 degrees, as you discovered. Then (I think) you can apply (1). You can probably do all of that in whatever node is handling generating the transform in (1). I haven't given it a huge amount of thought, so I may be missing something.

Originally posted by Tom Moore with karma: 13689 on 2016-07-13

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by NZNobody on 2016-07-13:\

  • Option 1: Sorry, I am a little confused what this would achieve? If the odometry data is reported in the odom frame, base_link will still be offset will it not? Since r_l computes odom -> base_link. Unless you mean report odometry data in the dynamically computed zed frame?

Comment by NZNobody on 2016-07-13:\

  • Option 2: This is an option, but wouldn't this decrease accuracy and cause drift on a sensor that is semi drift robust? The camera performs loop closure detection and so if I return to my starting position, it is very accurate at outputting the exact same location again. Or would it not be affecte

Comment by NZNobody on 2016-07-13:\

  • Option 3: If I understand this correctly, this parameter could effectively look up the static transform from base_link -> zed_frame and apply the inverse of this to the data provided by in the odom frame to correctly position the base_link. I imagine it like this: (Photo in next comment)

Comment by NZNobody on 2016-07-13:
Sketch of Option 3 - The odometry reports pose C -> A, normally r_l would place base_link at A. Use A -> B to calc base_link offset as C -> A + A -> B resulting in C -> B. Does this make sense?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.