0
$\begingroup$

Rosanswers logo

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.0001]
    
    
initial_estimate_covariance: 
    [1e-2, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
     0,    1e-2, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
     0,    0,    1e-2, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
     0,    0,    0,    1e-2, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
     0,    0,    0,    0,    1e-2, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
     0,    0,    0,    0,    0,    1e-2, 0,    0,    0,    0,     0,     0,     0,    0,    0,
     0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

image description


Originally posted by b-sriram on ROS Answers with karma: 105 on 2018-01-18

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-01-19

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by b-sriram on 2018-01-19:
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).

Comment by PeteBlackerThe3rd on 2018-01-19:
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.

Comment by b-sriram on 2018-01-19:
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.

Comment by PeteBlackerThe3rd on 2018-01-19:
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.

Comment by b-sriram on 2018-01-19:
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).

Comment by b-sriram on 2018-01-19:
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.

Comment by b-sriram on 2018-01-19:
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

Comment by PeteBlackerThe3rd on 2018-01-19:
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.

Comment by b-sriram on 2018-02-27:
I created two transform objects: 1. Position of Markers in marker frame (inverse of the tf published by ar_track_alvar) , 2. Marker in the map frame. Then tried to multiply them to get the pose of the robot on the map. The problem is when I inverse the tf published by the ar_track_alvar package ...

Comment by b-sriram on 2018-02-27:
... I dont get the proper inverse. This is because the coordinate system of the ar tags detected are rotated as compared to my camera_link frame and the inverse function deos not give me the required value. Should I rotatae the tf by 90 in yaw & pitch direction manually or is there a easier way.

$\endgroup$

Your Answer

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