Visual Localization

 


 The goal of this exercise is to program a robot similar to an autonomous vacuum cleaner to enable the autolocalization based on the positions of pasive visual markers.


The pasive visual markers in this case are AprilTags. They are similar to QR codes and are used in applications such as robotics, augmented reality, and camera calibration.


To obtain the current position of the robot from the image we must obtain all of the translation and rotation matices of the following formula:


1.World2Marker

To get this matrix we must know which tag we have in the image and its position. We obtain the global positions from the file "apriltags_poses.yaw", and we can get the id tag from the image using the "pyapriltags.Detector()".

2.Marker2Camera

To get this matrix we must solve the Perspective-n-Point problem. We will use the "solvePnP()" function from Opencv. In order to use it we need:
  • The four corners of the tag in the camera frame (obtained from "pyapriltags.Detector()").

  • The four corners of the tag in the local frame (size of the april tag / 2).



We can use the translation components from the output of the function, but to get the rotation matrix we need use the "Rodrigues()" function from Opencv. After adding padding to both matrices and multipliying them we get the matrix "RT_Camera2Marker". We can obtain the one we need by computing the inverse matrix.

We also have to take into account the reference axis from the april tags are not the same as in gazebo:



To align them with the ones used in the simulation we must perform a rotation of -90 degrees in the z axis and the x axis to the matrix.

3.Camera2Robot

The last matrix is the easiest one, since we can get the information from the position of the camera in the robot sdf file.


Once we have all the matrices we multiply them. To obtain the "x" and "y" coordinates we look at the translation part of our result matrix (positions 0,3 and 1,3). To get the "yaw" orientation we must first get the pitch and then use it as seen in the next formulas (in python code):

 


In the cases where there are multiple AprilTags detections we simply perform the average position of the two we obtained.
If there are no AprilTags detections we update the position by calculating the difference between the last position we got and the odometry obtained from the sensor.

We can see two test executions in the next videos:








Comments

Post a Comment

Popular posts from this blog

Local Navigation with VFF

Image
       The main objective of the local navigation in robotic systems is to get to a near target in the safest and most efficient way possible. One of the many algorithms to solve this problem is the Virtual Force Field , developed by Borenstein and Koren.       The fundaments of it are that every obstacle in the environment generates a repulsive force towards the robot and the target generates an attractive force in the robot. So, with this two facts in mind we can find the combination of both forces that will guide the robot to its destination similar to the next image:        For a simplification each object generates only                      one mayor force, instead of multiple repulsive forces.      Now, we want to make a reactive navigation system for a car with an  implementation of  this algorithm. Since the sensor we are usin...
Read more

Autoparking

Image
  We have to make a program that parks a car in a parallel disposition. To do so the car must follow a trajectory similar to the one in this image: On the hardware part, we can access three 2D lasers positioned on the front, right and rear of the car. We can also send linear and angular speed commands.   The software implementation is based on a finite-state machine architecture.The algorithm can be divided into four main states: -First we have to find a free space and advance some distance to have enough space to move in reverse.  -Secondly we move backwards and move the steering wheel in the same direction of the space where we want to park.  -Then we steer in the other direction while moving backwards.  -Finally, to relocate the car into a position in line with the other car we can advance forward or backwards while steering to one side or the other depending on the situation. To find a free space to park we check the right-side laser, until we find a positio...
Read more

Global Navigation with GPP

Image
  This time the objective is to create a plan or path to guide a robot to a distant location, specifically a taxi. To solve this the Gradient Path Planing (GPP) will be used. This algorithm gives us the shortest way to reach the target by calculating the descending gradient of a given grid, similar to this figure:     In our case the planing will be done with an ascending gradient instead of a descending one, but this will be explained later.        At first instance we need to be aware that the GPP method needs a metric grid map of the world, this means that the map will consist of diferent cells that will mantain some correlation with the real world distances between objects, fo example it could look like this:               Now that we have the map, we need to fill the cells to calculate the path. In the algorithm we are using the target will generate an expansive wave that will assign lower values...
Read more