Marker Visual Loc

Marker Based Visual Localization

Goal of the Project

The goal of this project is to estimate the 2D pose (x, y, yaw) of a mobile robot using AprilTag-based visual localization.

The robot must:

• Detect AprilTags using its onboard camera.

• Compute the relative pose between the camera and each detected tag using PnP.

• Transform this pose into the global reference frame using the known tag positions.

• Filter unstable visual measurements caused by noise.

• Maintain a continuous pose estimate using odometry when no tags are visible.

The result is a localization system capable of correcting odometry drift using vision while remaining stable during temporary visual loss.

Project Overview

The localization implemented in this project follows a vision-first approach with odometry fallback.

Whenever a tag is detected, the robot estimates its pose using a full geometric transformation chain. If visual data is not available, the robot propagates the last valid visual pose using odometry increments.

The main challenges addressed are:

• Correct handling of coordinate frames.

• Robust pose estimation using PnP.

• Noise filtering to avoid large localization jumps. 

AprilTag Detection

AprilTags are markers that encode a unique ID and allow accurate pose estimation from a single camera.

Each control cycle:

• The robot captures an image from the camera.

• The image is converted to grayscale.

• The AprilTag detector extracts tag IDs and the pixel coordinates of the four tag corners.

The detected tag borders are drawn on the image and displayed in the WebGUI to verify detection quality and orientation.

Camera Model and PnP Pose Estimation

To compute the relative pose between the camera and a detected tag, I use Perspective-n-Point (PnP).

The tag is modeled as a planar object with known dimensions (0.24 × 0.24 m). Its four corner points are defined in the tag reference frame (Z = 0). These 3D points are matched with their corresponding 2D image projections.

Using the camera matrix and assuming zero distortion, OpenCV’s "solvePnp" function computes:

• A rotation vector rvec.

• A translation vector tvec.

These describe the pose of the tag with respect to the camera frame.

The SOLVEPNP_IPPE_SQUARE method is used because it is optimized for square planar markers and provides stable results for AprilTags.

Homogeneous Transformations

The rotation vector returned by PnP is converted into a rotation matrix using Rodrigues formula. The rotation and translation are then combined into a homogeneous transformation matrix.

From PnP, the transformation obtained is:

• Camera -> Tag

To compute the robot pose, this transformation is inverted to obtain:

• Tag -> Camera

Homogeneous matrices are used throughout the system to allow chaining of rotations and translations in a consistent way. 

Coordinate Frame Alignment and Fixed Rotations

The coordinate frame returned by OpenCV does not match the robot’s coordinate frame.

Specifically:

• OpenCV uses a camera frame where Z points forward, X to the right, and Y downward.

• The robot frame assumes X forward, Y left, and Z upward.

To align these frames correctly, two fixed rotations are applied:

• A rotation around the X axis to correct the vertical axis orientation. 

• A rotation around the Z axis to correct the horizontal axis alignment.

These rotations are applied in a specific order using matrix multiplication, ensuring the camera pose is properly expressed in the robot reference frame.

Without this alignment, the estimated yaw and position would be inconsistent with the robot’s real motion.

Transformation to World Coordinates

Each AprilTag has a known pose in the world frame, loaded from a YAML configuration file.

For a detected tag:

• A world-to-tag transformation is built using the tag’s global position and yaw.

• This transformation is multiplied with the tag-to-camera transformation to obtain:

World -> Camera

From this final matrix:

• The robot position is extracted from the translation component.

• The robot yaw is computed from the forward axis of the rotation matrix.

This yields a complete (x, y, yaw) estimate in world coordinates. 

Visual Pose Selection and Noise Filtering

When multiple tags are visible, the detection with the smallest camera distance is selected, as it generally provides better accuracy.

However, visual pose estimation is inherently noisy, especially at larger distances. To address this, I implemented a jump rejection mechanism:

• A maximum allowed position change is defined.

• If the new visual pose differs too much from the previous one and the tag is far away, the measurement is discarded.

This prevents sudden unrealistic jumps while still allowing the system to correct accumulated drift when the robot legitimately moves.

Odometry-Based Pose Propagation

When no valid visual measurements are available, the robot continues estimating its pose using odometry.

The approach is incremental:

• The odometry pose at the last visual update is stored.

• Current odometry is compared against it to compute relative motion.

• This relative motion is applied to the last visual pose.

This ensures smooth pose continuity and allows the robot to recover seamlessly once a tag becomes visible again.

Robot Motion and Safety

To test localization under motion, the robot moves forward at a constant speed.

A simple laser-based safety check is implemented:

• If an obstacle is detected in front of the robot, forward motion stops.

• The robot rotates until the path is clear.

This allows continuous localization testing while avoiding collisions.

Problems and Solutions

Unstable visual estimates at long distances

Far tags produced noisy pose estimates.

Solution: introduce distance-based filtering and reject large pose jumps. 

Pose discontinuities when switching from odometry to vision

Sudden jumps occurred when visual data became available again.

Solution: compare new visual estimates with the last valid pose and discard inconsistent measurements. 

 

 Video

---

 

Laser Mapping

Goal of the Project

The goal of this project is to build a navigation system that allows the robot to autonomously explore an unknown environment while generating an occupancy map using LIDAR data.

The robot must:

• Detect obstacles and free space using a probabilistic laser sensor model.

• Explore unknown areas by moving toward the closest frontier.

• Safely update its trajectory as the occupancy grid changes.

• Move waypoint by waypoint until no more unexplored space is reachable.

This results in a robot capable of mapping an entire warehouse-like area on its own.

 

Project Overview

In this project I implemented a frontier-based exploration system combined with a probabilistic occupancy grid. The idea is simple: the robot keeps expanding the map and always tries to move toward the nearest “unknown border” of the map until the entire environment has been explored.

To do this, the robot maintains a continuous estimate of its own position in pixel coordinates.
The simulator provides the robot position in meters, and I use poseToMap() to convert the robot’s real-world position into the exact pixel indices inside the occupancy grid.

I also use the same conversion to compute the robot’s heading direction in pixel coordinates, which is essential for the angular control and for knowing where the robot is “looking” inside the map.

 

Implementation 

BFS Frontier Search

At the beginning of each cycle, the robot launches a BFS (Breadth-First Search) that tries to locate the closest unknown pixel in the occupancy grid.

To make the search faster, I don’t explore pixel by pixel.
Instead, I move in steps of 15 pixels, which drastically reduces the amount of visited nodes and still gives a smooth exploration behavior.

The BFS expands outward from the robot’s current pixel position and checks cells that are:

• free according to the occupancy grid, and

• safe (meaning no obstacles are too close in the vicinity).

Safety rectangle (experimentally tuned)

To determine whether a cell is safe for the robot, I implemented a function that checks a rectangular region around each candidate pixel.
This rectangle extends 12 pixels in all directions, forming a 25×25 pixel safety box.

These exact dimensions were not arbitrary:
I determined them experimentally by observing how many pixels a wall or obstacle occupied inside the grid and how much clearance was needed to prevent the robot from clipping corners or touching walls.

This ensures that BFS never selects waypoints that are technically free but too close to walls in practice.

As soon as the BFS finds a free cell that is adjacent to an unknown area, that position is considered a frontier, and the BFS reconstructs the shortest path back to the robot.

This path becomes the list of waypoints the robot will follow.

If the BFS can’t find any safe frontier, the robot backs up very slowly.
This is something I added because sometimes the robot gets too close to a wall, and then the starting BFS node is not considered safe, causing BFS to refuse to expand.
By adding a small negative velocity, the robot moves away from the obstacle until a safe BFS start becomes possible again.

 

Probabilistic Laser Mapping (Log-Odds)

The map is built using a classical log-odds occupancy update. For each laser beam:

• I compute its world angle using the robot’s yaw.

• I check the measured distance.

While testing, I confirmed that the effective maximum range of the laser in this environment is about 3.5 meters, so:

• If the laser detects an obstacle, I mark the endpoint as occupied.

• If the laser returns no hit, I treat it as a max-range reading and set its reading to 3.5 m.

In both cases, I fill all intermediate cells between the robot and the endpoint as free.

For every cell traversed:

• I add log_free.

• If the endpoint corresponds to an actual hit, I also add log_occ at the final cell.

These log-odds updates accumulate over time and converge to probabilities where:

• positive means occupied,

• negative means free,

• values close to zero remain unknown.

This results in a clean, continuously updated occupancy map.

The laser mapping is made every 0.5m so it has independent measurements and prevent from saturation of the probabilistic grid

 

Robot Movement and Path Following

Once BFS generates the waypoint list, the robot follows each point sequentially.

I use a P-controller for angular velocity:

w = K * angular_error

To compute the angular error, I use the robot’s orientation in pixel coordinates.
I take the robot’s current world position, convert it to pixel coordinates with poseToMap(), and do the same with a point located one meter in front of the robot.

This gives me two pixel points:

• the robot’s current pixel location

• the “front direction” pixel location

Using these two points I compute the robot’s heading angle in pixel space and compare it with the angle to the waypoint.

The robot:

• Rotates toward the waypoint using the P-controller.

• Moves forward only when the heading error is small enough.

• Switches to the next waypoint when it gets sufficiently close.

This leads to smooth and reliable navigation.

 

Safety Checks While Moving

Even after BFS finds a path, the robot constantly rechecks if the next waypoint is safe.
Because the map updates continuously, an area thought to be free may later be detected as dangerous when new laser scans arrive.

If a waypoint becomes unsafe:

• the robot cancels the current path,

• switches back to BFS,

• and if the robot is too close to a wall, it backs up slowly until a safe position is reached again.

This combination of real-time mapping and active safety makes the robot robust in tight or cluttered environments.

 

Problems and Solutions

Robot stuck near walls.

Sometimes BFS rejected the robot’s starting cell because it was too close to a wall.
Solution: add a slow backward motion when BFS fails.

Laser maximum distance behavior.

The simulation’s effective maximum laser distance is ~3.5 m.
Solution: treat no-return readings as 3.5 m and mark the entire ray as free.

Efficient exploration.

A pixel-by-pixel BFS was too slow.
Solution: BFS moves in steps of 15 pixels, which preserves exploration quality while being much faster.

The robot hit the walls and shelves when moving.

At first the movement was done with the bfs just checking pixel by pixel. What happened was that the robot moved too close to the obstacles and hit them.

Solution: I had to create a function that checks a square of pixels around the pixel is being checked so neither the pixel or the surroundings are occupied.  

The laser problems.

The laser sometimes assigned values to pixels behind obstacles and that generate errors in my algorithm.

Solution: I had to stablish that when a laser detects an object it doesn't paint further away 

 

Video

    Perfect Localization 

         

    Odom localizations:

Using Odom the robot doesn't have an exact location so it starts in the location it should and then it starts to slowly deteriorate and maps wrong the room and in Odom3 it finally hits an obstacle.

 

    Odom1: 

             

    Odom2:

We can observe that using Odom2 the location of the robot is not an exact location so it maps the room deformed.

 

    Odom3:

        

 It hits the wall because of the odom3 location

 

Amazon Warehouse

Goal of the Project

The objective of this exercise is to implement the logic that allows a logistics robot to deliver shelves to the required place by making use of the location of the robot. The robot is equipped with a map and knows its current location in it. The main objective will be to find the shortest path to complete the task.

Project Overview

This project was made in different stages.

1. First stage:

    Holonomic robot.

    Checking its ability to fit into narrow spaces by representing the robot as a single pixel and computing the path using one-pixel steps.

2. Second stage:
    Holonomic robot

    Checking whether the path is valid by using the robot’s geometry. A bounding box is created based on the robot’s size and rotated according to its orientation so the planner can determine whether the robot can follow the path using basic trigonometry.

hl = ROBOT_LENGTH / 2

hw = ROBOT_WIDTH / 2

verts = np.array([

[hl, -hw],

[hl, hw],

[-hl, -hw],

[-hl, hw]

])

c = math.cos(theta)

s = math.sin(theta)

pts = []

for lx, ly in verts:

rx = lx * c - ly * s

ry = lx * s + ly * c

px, py = coord_to_pixel(x + rx, y + ry)

pts.append((px, py))

 The algorithm iterates over the bounding-box vertices, rotates them according to the robot’s orientation using sine and cosine, and stores the transformed vertices in the points list.

 3:

    Ackerman robot:

    This stage included tuning the P controller values, adding a reverse state, adding an ALIGN state that orients the robot to the heading expected by the planner before generating the next plan, and selecting an appropriate turning radius by testing several candidate paths and choosing the one the robot could follow with high angular velocity.

     The same valid state checking using bounding box.

 

IMPLEMENTATION: 

 

 states:

     

PLAN

The robot computes a path to its next target.
It uses OMPL to generate a valid trajectory and converts it into waypoints.
When the plan is ready, it switches to GO.

GO

The robot follows the planned waypoints:

• Moves toward the next point

• Adjusts speed and steering based on orientation error

• Automatically switches to reverse if the turn is too sharp and the angle is reversed. 

• When the final waypoint is reached, it either aligns (ALIGN) or stops (STOP) depending on orientation

ALIGN 

If the robot reaches the goal position but the orientation is incorrect, it corrects its heading.
It turns slowly until the angle is within tolerance, then transitions to STOP.

STOP

The robot stops for 3 seconds.
After the pause:

• If it is not carrying a shelf → transition to LIFT

• If it is carrying a shelf → transition to PUT DOWN

This state works as a synchronization step between actions

LIFT

The robot lifts the shelf.
It updates its size to include the carried shelf and temporarily removes the obstacle from the map.
Then it returns to PLAN to compute the route to the drop-off location.

PUT DOWN

The robot places the shelf on the ground.
The obstacle is restored in the map and the robot’s size returns to normal.
If more shelves remain, it goes back to PLAN; otherwise, it enters DONE.

DONE

Final state.
The robot stops completely and the task is finished.

 

Problems and Solutions

During the development of this project, several issues appeared that required specific adjustments to ensure correct navigation and smooth motion:

• Orientation mismatch at the end of the plan
  The planner sometimes produced a final orientation that did not exactly match the robot’s actual heading when reaching the goal.
  This caused planning failures because OMPL expected the robot to be aligned before generating the next trajectory.
  To fix this, I introduced the ALIGN state, allowing the robot to correct its final orientation before switching to STOP or generating a new plan.

• Planning time too short
  When the allowed planning time was small, OMPL produced overly complex paths with aggressive turns.
  Due to the robot’s Ackermann steering, these paths were difficult to follow and caused large orientation errors.
  To reduce this problem, I increased the planning time to 20 seconds, which produced smoother and more feasible trajectories.

• Need for interpolation
  The raw OMPL output contained large jumps between states, which made the robot movement unstable.
  I solved this by interpolating the plan, generating a smoother set of intermediate waypoints for more consistent tracking.

• System orientation reference problem
  An additional issue appeared related to how the orientation system behaved.

   It was necessary to verify the system’s orientation conventions, and adjustments of ±π radians had to be applied depending on the context.

  f I set the planner’s goal orientation to 0 radians, it planned a trajectory that oriented the car backwards. If i plan adding math.pi radians to the goal it plans and sets the robot looking to the front. Nevertheless, the robot orientation is with 0 radians by looking to the left. To make 0 radians correspond to “facing forward” and compute the orientation to each waypoint correctly, I had to subtract π/2 radians.

  Situations in which I had to add or reduce angles:

   Moving:

  The robot's yaw value looking to the front is 0 radians thanks to the first correction: 

  yaw = pose.yaw + math.pi/2

  and the goal stablished by the planner is π to make it look to the front. When the car is with yaw = 0 radians the error must be 0 at the goal. So:

  err_goal = normalize_angle(goal_theta - math.pi - yaw)

  Planning:

  the car initial orientation is 0 radians to the left with the car looking to the front. So when looking at the robots orientation with 

  pose = HAL.getPose3d()

  pose.yaw 

   This returns -pi/2 so I had to reduce pi/2 when calculating the plan with the initial orientation of 0º

   

  Video: 

    Warehouse_1_Ackerman.

  The shelves are manually rotated because the friction of the simulator causes them to change their intended orientation when the robot sets them down.

  
  

  Warehouse_1_holonomic.

  The shelves were being moved by hand because, as the robot transported them, their orientation shifted. When the robot set them down, the shelves ended up in an unexpected position, which could cause the robot to collide with them.

  
  
  
  
  
  

   

   

   

Autoparking

 

 Autoparking

Goal:

 The objective of this exercise is to implement the logic of a navigation algorithm for an automated vehicle. The vehicle must find a parking space and park properly.

Implementations:

First, the system reads the laser data from the sensors. Then, it executes the following finite state machine (FSM):

 

- APPROACH:

 

    P Controller:

 

    In this state the car follows a P controller that compares in the right_laser every angle with its opposite angle. If one of those angles are inf they are ignored but if they have values we calculate the error and we sum all the errors multiplied by their factor which is higher the nearer the laser is to the middle of the car so it doesn't oscilate. 

 

Then we calculate the mean of the errors and using the laser at the lateral, exactly at 90º we calculate the current distance, and by knowing that we want to be at 1.5 meters of distance we calculate the distance error and our angular velocity (w) is:

    w = -kp_angle * orientation_error - kp_distance * distance_error. This way it keeps the orientation and distance desired.

 

    Hole detection:

    We establish a simulated hole with the size we want it to be.  Taking account the laser is in the middle of the car: 

half_len = HOLE_LENGTH / 2.0

x_min, x_max = -half_len, half_len

y_min, y_max = obstacle_dist, obstacle_dist + HOLE_WIDTH 

 

Then we look at every angle of the right laser and using trigonometry with the distance and the angle we calculate the position where the laser ends. If that position is inside the hole simulated then there is not hole. If all laser points have been analyzed and the hole is free then we go to the next state.

 

- PARK_1:

    Now we have detected the hole we look to the front laser. If there are no cars in the front we park with just the car behind the hole and go to the state DIRECT_PARK.

 

Nevertheless, if a car has been detected in front the car is going to go parallel to that car by looking to the right side of the back laser (170 to 180º) and the front laser (0 to 20º). If the right section of the front laser has detected an obstacle, which is when the car passed the hole, and the right section of the back laser detects something in less than 3 meters then goes to the state PARK_2.

 

- PARK_2:

    It starts going backwards and to the right until

        - The right laser detects nothing in the range (0 to 40º) this is effective because it means our car has turn enough using the car it was parallel to as its reference.

             

         - The car behind is at less than 6 meters. 

 

- PARK_3:

     It starts going backwards and left until 2 options:

        -The front car is 2 meters far away just looking at the front middle section (80 to 100º)

        -Our car is at less than 5 meters from the car behind. Just in case not to crash.

 

- STOP:

     The car starts going to the front and right until the front car is at less than 2 meters.

 

 

- DIRECT_PARK: 

    This is state is used because when we don't have a car reference to start our parking it was more realistic if the car just turned right until it was at a safe distance to start maneuvering backwards and the car is at less than 2 meters. 

 

Problems and Solutions: 

 Just by using all the lasers values i had problems because they cover a very large area so the distances used weren't as i though. The solution to this was to use the lasers by sections. Choosing the angles i wanted to use and this way I could use angles to park in a more realistic way.

 

Other problem was that my algorithm was made by using the front car as a reference. Because of this i had to implement a new way of parking my car when there is no car in front. 

videos: 

 No cars behind

No cars in front

Normal parking

 

Rescue People

Rescue People — Practice Report

Objective

The objective of this project is to implement and test a drone navigation and face detection algorithm to develop a rescue people system capable of taking off a boat and searching for faces in the sea until the battery is over or the 6 persons have been detected.

 

Localized Vacuum Cleaner

 

Localized Vacuum Cleaner — Practice Report

Objective

The objective of this project is to implement and test a navigation algorithm for an autonomous vacuum cleaner capable of covering the largest possible area of a house using localization and mapping techniques.