How to create a static tf from artags to map frame?

asked 2018-01-18 11:05:18 -0500

b-sriram
105 ●24 ●27 ●33

updated 2018-02-27 02:38:04 -0500

Hi,

I am using the ar_track_alvar package to detect artags placed in my environment.

When the tags are detected I get a new frame like "ar_marker_1" and this is linked to the robots camera frame.

What I want to do is basically place many tags in the environment and move the robot around and calculate the odometry using an EKF of the robot_localisation package. The input to the EKF would be the output from the ar_track_alvar package which gives how far the robot is from the tags (so relative to the tags).

I am not able to figure out how to create a static tf that links the artag frames to a world frame (say map frame) which would then help the EKF node to calculate the odometry.

This is my EKF launch parameters

frequency: 30

sensor_timeout: 1

two_d_mode: true

debug: false

publish_tf: true

publish_acceleration: false

map_frame: map              
odom_frame: odom            
base_link_frame: base_link  
world_frame: map           

dynamic_process_noise_covariance: true

pose0: marker1
pose0_config: [true,  true,  true,
               true, true, true,
               false, false, false,
              false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 40
pose0_nodelay: false

pose0: marker2
pose0_config: [true,  true,  true,
               true, true, true,
               false, false, false,
              false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 40
pose0_nodelay: false

pose0: marker3
pose0_config: [true,  true,  true,
               true, true, true,
               false, false, false,
              false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 40
pose0_nodelay: false

pose0: marker4
pose0_config: [true,  true,  true,
               true, true, true,
               false, false, false,
              false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 40
pose0_nodelay: false

process_noise_covariance: 
    [0.0001, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
     0,    0.0001, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
     0,    0,    0.0001, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
     0,    0,    0,    0.0001, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
     0,    0,    0,    0,    0.0001, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
     0,    0,    0,    0,    0,    0.0001, 0,     0,     0,    0,    0,    0,    0,    0,    0,
     0,    0,    0,    0,    0,    0,    0.0001, 0,     0,    0,    0,    0,    0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,     0.0001, 0,    0,    0,    0,    0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,     0,     0.0001, 0,    0,    0,    0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,     0,     0,    0.0001, 0,    0,    0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.0001, 0,    0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.0001, 0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.0001, 0,    0,
     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.0001, 0,
     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0 ...

(more)

answered 2018-01-19 06:19:35 -0500

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

9529 ●30 ●143 ●135 https://www.linkedin.c...

updated 2018-01-19 11:08:59 -0500

The ar_track_alvar package publishes transforms an it's own custom message type. The custom message type includes a confidence metric along with the pose for each detected tag, this confidence value is extremely valuable when used with a Kalman filter so I would encourage you to use this topic instead of the TFs.

Before you add each pose estimate to your Kalman filter if you apply a unique transformation to each tag estimate which transforms the tag location to the origin of your world coordinate frame. After this step each tag estimate is then an estimate of the location of the world frame in terms of the ar_track_alvar detection frame, this makes the C matrix of the Kalman filter unity since each measurement is estimating the same thing. The confidence value can be used to make a diagonal matrix for the covariance, then you should be setup.

If some form of odometry is also added to the Kalman filter then this should be a very robust localisation setup. Let us know how you get on.

Update: Getting the robot pose estimate in the world frame

The tag poses from ar_track_alvar describe the position of the tag in the frame setup in it's launch file. If you store this pose in a transform object then it's quite easy to do the manipulation you'll need to do.

For example you can use the inverse method to reverse the transform, in this case this will give you the position of the ar_track_alvar frame in terms of the position of the marker. Getting closer to what you want.

Now if you define another transform object which describes the pose of the marker in question, in the world frame, then multiply your pose estimate from the previous step by the pose of the marker, you now have a pose estimate in the world frame. This pose along with its confidence value can now be used by the EKF to estimate your robots position. This is really hard thing to describe in words, so I hope this makes sense.

edit flag offensive delete link

Comments

Ok I'll try it and get back to you.

Just one doubt, how do you get the origin of the world co ordinate system (say I want to use the map frame)? I'm using Gazebo to simulate the environment and my Map has different origin (as I started mapping at a different location than the Gazebo origin).

b-sriram ( 2018-01-19 06:25:19 -0500 )edit

If you're ar tags are defined in world gazebo coordinates then you'll have to use that as your mapping frame too. You would have to know the transform between your mapping frame and the world frame, but it will be much easier to use the world frame for mapping too.

PeteBlackerThe3rd ( 2018-01-19 06:51:14 -0500 )edit

Ok I do know the transformation between the gazebo frame and map origin. The only problem is I cannot create a static TF as the map frame would change in time as the error accumulates with the robot motion.

b-sriram ( 2018-01-19 07:06:56 -0500 )edit

If you're using the AR markers to localise the robot then there will be no error accumulation. You don't need a map frame. You can use the world frame for this.

PeteBlackerThe3rd ( 2018-01-19 08:24:01 -0500 )edit

I already have an EKF running to localise using my encoder and Imu . This would then publish a transfrom from the Odom frame to the base_link. So now I'm using the ar tags to get global localisation (like using a GPS).

b-sriram ( 2018-01-19 10:01:25 -0500 )edit

So my tags when sensed would give me the relative position of the camera(robot) with respect to it's position. Now what I want to do is connect the artag frame to a fixed frame so my robot now's the position of the tag in the world and not the relative position like now.

b-sriram ( 2018-01-19 10:01:25 -0500 )edit

I get I have to connect the tags to a world frame but I'm really new to Ros and don't know the code to do it. If you could point me to some reference code, would be really great for me to solve this problem

b-sriram ( 2018-01-19 10:03:23 -0500 )edit

The problem here is the TF system is not designed to do this. If you have circular links in your TF tree with conflicting estimated poses it will go bonkers. You'll need to add the pose estimates into your existing EKF.

PeteBlackerThe3rd ( 2018-01-19 11:01:05 -0500 )edit

see more comments

How to create a static tf from artags to map frame?

1 Answer

Comments

Question Tools

Stats

Related questions

How to create a static tf from artags to map frame? edit

1 Answer

Comments

Question Tools

Stats

Related questions

How to create a static tf from artags to map frame?