Jekyll2026-06-09T14:48:31+00:00https://olayasturias.github.io/feed.xmlblankPostdoctoral researcher in Robotics, Computer Vision, and Deep Learning — focusing on Visual SLAM for underwater applications. My ROSCON 2024 digest - Days 2 and 32024-10-25T06:15:00+00:002024-10-25T06:15:00+00:00https://olayasturias.github.io/blog/2024/roscon-odense-2This year, our neighbor city Odense held the ROSCON conference, which I had the honour to attend :slightly_smiling_face: I collected some notes for my future self (and maybe the present you?), that I have collected in this post for days 2 and 3, and on a previous post for day 1 (which consisted of workshops and birds of feather sessions).

If any of these notes raises your interest, you can watch the full talks in this link. Enjoy!

Simulation and AI Tools

Robotec.ai: Simulation and AI tools

alt text

Robotec.ai introduced us to their simulator. It integrates seamlessly with ROS (though it can function independently as well), utilizing NVIDIA’s Physx engine, and the renderings from Open 3D Engine. Essentially, Robotec.ai has migrated the ROS stack from Gazebo to O3DE, enhancing both the visual and functional capabilities of simulations.

The simulator is modular, allowing components like the physics engine to be swapped out based on specific project needs. While optimized for ROS, the simulator’s modular nature allows it to adapt to various robotic frameworks, offering flexibility for different use cases.

Robotec.ai provides tailored services to set up simulators and simulation environments suited to the customer’s specific requirements.

Later in the conference, a talk from Michal Pelka showed us how easy it is to set up O3DE for simulation: alt text

USBL Simulator

USBL sim

One of my quick notes includes this repo to an USBL simulator. The simulator is written in C++, with a ROS 2 node acting as wrapper for it.

The simulator promisis a high-fidelity simulation, with features including (but not limited to) Round-trip-time (RTT) and Time-Difference-of-Arrival (TDOA) noise and quantization simulation.

The parameters are configured in a YAML file, and the repository includes an example config file for an OEM USBL by Evologics.

Automated testing framework with AWSIM and OpenScenario

alt text AWSIM is an Open source simulator for self-driving vehicles based in Unity.

Data Management

Segments.ai

alt text

Segments.ai is a company located in Belgium. They provide tools for data labelling in 2D and 3D, and an API to export it to whichever framework you use (e.g. Pytorch). They also provide data labelling service. Their labels seem to be focused on object detection and segmentation in 2D and 3D (as you might have guessed by the name). Currently, it doesn’t appear that they support localization directly, though I plan to explore this further through their demo offerings. Segments.ai offers free academic licenses and demo datasets.

Roboto.ai

Roboto.ai provides a platform for curating and analyze robotics data. https://docs.roboto.ai/_images/overview.png

With Roboto.ai, you can extract specific data from large datasets, making it especially useful for working with rosbags. Instead of handling one massive file, you can query only the data you need, optimizing memory usage. The platform also allows you to curate datasets based on events or anomalies detected within the data. Roboto’s API includes built-in functions for automatic anomaly detection, streamlining data analysis and cleanup. More info on the concepts and functionality can be found here, or in the Github repo.

Foxglove

Foxglove is a well-known tool for visuallizing and debugging data. The data can be streamed from the cloud, local files, or in real time from the robot. Foxglove also allows replaying the data, (read here) (or here). One of Foxglove’s standout features is its ability to display multiple data sources simultaneously, offering flexible, customizable visualizations. For instance, it can look something like this:

…but it doesn’t have to—Foxglove is fully customizable! By the way, the sample above showcases our SubPipe dataset! :smiley: Foxglove has a free plan, and provides free access to students and academics.

Communication Protocols and Middleware

Zenoh

alt text

Zenoh made a bold entrance at this year’s ROSCon with a strong marketing push to position itself as the next standard in robotics middleware, potentially overtaking DDS. While choosing between these options requires more than a quick discussion or blog post, if you’re undecisive, you might like to know that Zenoh offers a bridge to ROS 2 DDS. This bridge allows you to integrate Zenoh’s capabilities alongside DDS without needing to replace the existing setup, giving you the flexibility to leverage Zenoh’s features while maintaining DDS compatibility.

eProsima

alt text

eProsima is well-known for providing the most adopted DDS solution as of today.Although their ROS 2 implementation of Fast DDS has faced criticism for some networking issues, it’s worth noting (as their CEO emphasized to us) that these issues have been addressed in recent updates.

In addition to their popular open-source libraries, eProsima offers commercial solutions, including specialized plugins for low-bandwidth connections, catering to diverse and demanding network requirements.

Pixi package manager

Pixi is a package manager that works seamlessly with Ubuntu, MacOS, and Windows. There are examples on setting Python (as alternative to conda) and C++ packages. Here’s an example package on how to use pixi for a ROS2 workspace.

Diagnosis and Performance Monitoring

How is my robot? - On the state of ROS diagnostics

alt text

Christian Henkel introduced to as a series of ROS packages for diagnosis from the ROS diagnostics stack. Aside from that, he presented us a series of good practices that his team at Bosch applies for diagnosis:

  • Diagnosis phylosophy
    • Main Purpose: Observe the current state of the robot.
    • Think of it as a control panel where operators has all the information they need.
    • Try to limit the metrics to <10, ideally 2-3 per component.
    • Warnings are states that are unusual but allow continued operation.
    • Errors indicate states that do not allow the robot to operate further and shall be immediately addressed.
    • Think about a logging and diagnostics concept in your team and document it.
  • Comparison to other concepts
    • Diagnostics vs:
      • Logging:
        • Logging is (a lot) more verbose.
        • Captures the inner state of a SW component.
        • Are (usually) for later consumption and analysis.
      • Bagfiles:
        • Are useful to record diagnostics in bagfiles.
        • Will also contain non-critical state info.
      • Testing:
        • Diagnostics help to find more quickly causes for failing tests, but don’t replace testing.
        • Crucial diagnostics may be tested themselves.
  • Antipatterns
    • In general, diagnosis are not meant to be used functionally:
      • The error handling that a robotic system does by itself should not depend on diagnosis.
      • Diagnostics should help a human observer or technician to understand a problem that was not recovered from.
    • The “right” amount of red:
      • Diagnostics must be tuned in a way such that they really mean a problem.
      • Otherwise, human observers get used to seeing error messages and don’t recognize critical ones.
      • In a similar theme, warnings should not be too frequent to not become meaningless.
    • Diagnosis must be received:
      • Diagnostics are meant as a communication method from robot to human.
      • So, in fully autonomous systems, they must be logged correctly and evaluated retroactively.
      • It is also worth to difference between roles:
        • For example, if an end user will see and/or understand diagnostics content or
        • if it must only be consumed by the trained technician.

Scenic: a probabilistic language for world modelling

alt text

Scenic is a stochastic scenario generator and a powerful language for defining spatio-temporal relationships in scenes. Whether you’re creating environments for CARLA, Gazebo, Unity, or Webots, Scenic can model scenarios as a combination of scene distributions and agent behaviors.

scenic Why is it useful? Scenic helps test cyber-physical systems against rare edge cases, design environments for exploration, and generate synthetic data for robust machine learning models. While it’s not available for Unreal Engine yet, its versatility makes it a game-changer for simulation and testing.

Scenario Execution for Robotics: A generic, backend-agnostic library for running reproducible robotics experiments and tests

alt text Scenario execution for robotics is a backend - and a middleware - agnostic library, that enables the robotics community to perform reproducible experiments at scale. Written in Python and built upon the generic scenario descrition language OpenScenario2 and pytrees, Scenario Execution reads a scenario definition from a file, translates it to a pytrees behavior tree, and then executes it. Although scenario execution can be used as a pure Python library, it is mainly targeted towards ROS 2. The backend-agnostic implementation allows Scenario Execution to be used with both simulated and physical robots, with minimal adaptations necessary in the scenario description file.

This tool allows testing the robot in execution time. It allows testing of various actions in parallel, as well as error injection.

With Scenario Execution, you create a logical scenario to reduce the amount of possible scenarios within your abstract scenario. This concrete scenario will have specific values to test: alt text

Localization, Navigation, Motion Planning and Collision Avoidance

Learn Probabilistic Robotics with ROS 2

If you work in robotics, it is extremely likely that you’ve heard about the Probabilistic Robotics book. There are plenty of repositories in the literature implementing such algorithms, however, as a developer you might want to code your own implementation that is adapted to your needs.

However, as addressed by Carlos Argueta, learning these algorithms is challenging, specially when it comes to translating theory into actual code (and making it work in the real world).

Under this repo, you can find a compilation of open-source implementations of the algorithms under the probabilistic robotics book, accompanied by an article explaining them.

Beluga AMCL: a modern Monte Carlo Localization implementation for ROS

alt text

In this talk, Franco Cipollone first introduced Beluga, a toolkit for Monte Carlo Localization (MCL) which operates in 2D over laser scan measurements. He mentioned, however, how the ACML algorithms are aging:

alt text

Under that premise, he introduced a new library EKUMEN is working on: the Localization And Mapping BenchmarKINg Toolkit (LAMBKIN)

Lambkin introduces a benchmarking application for localization and mapping systems, in which each component runs in a separate container. It relies on a series of open-source libraries and benchmarks, including evo-tools and timem.

A ROS2 Package for Dynamic Collision Avoidance Based on On-Board Proximity Sensors for Human-Robot Close Interaction

alt text

GSplines: Generalized Splines for Motion Optimization and Smoot Collision Avoidance

logo

Rafael A. Rojas presented his open-source library to represent and formulate motion and trajectory planning problems with generalized splines and piece-wise polynomials. Considering that motion planners have waypoints as output, these points must be joined to form a trajectory. This work presents a library to obtain these trajectories with GSplines.

“A generalized spline is a piece-wise defined curve such that in each interval it is the linear combination of certain linearly independent functions”.

In contrast to ROS classes, the introducied GSplines class contains all the necessary elements to derive polynomial trajectories for a given set of waypoints. ROS classes define points and trajectories as nested structures. An example can be seen with the JointTrajectory and JointTrajectoryPoint classess (or messages):

JointTrajectory:

# The header is used to specify the coordinate frame and the reference time for
# the trajectory durations
std_msgs/Header header

# The names of the active joints in each trajectory point. 
string[] joint_names

# Array of trajectory points.
JointTrajectoryPoint[] points

JointTrajectoryPoint:


# Single DOF joint positions for each joint relative to their "0" position.
float64[] positions

# The rate of change in position of each joint. 
float64[] velocities

# Rate of change in velocity of each joint.
float64[] accelerations

# The torque or the force to be applied at each joint.
float64[] effort

# Desired time from the trajectory start to arrive at this trajectory point.
builtin_interfaces/Duration time_from_start

This array leads to inefficient memory layout and complicates the translation into the flat vector format that optimizers require. There is no clear interface for evaluation at arbitrary time instances, computation of derivatives, or time scaling.

To efficiently derive the trajectories for the given waypoints, the GSplines class represents:

  • Solutions of the minimum-X trajectory passing through the given waypoints.
  • Representing Transformations:
    • Derivatives
    • Linear operations
    • Linear scaling

That is, the class itself includes the derivative linear operations and linear scaling. The class’goal is to achieve the monoid property on the derivative, scaling and addition/scalar multiplication operations. These operations will become a monoid under the composition. The monoid gives nice properties to program. By implementing the polynomial basis solution of the trajectory, the GSpline class allows implementing optimal control.

For more info visit the repositorie’s Github, which includes the code, theoretical explanations, and link to the associated publications.

alt text

Nav2 Docking Server is part of the Nav2 library, and it has its own dedicated repo. The Nav2 Docking Server can be used with arbitrary robots and docks for auto-docking. The plugins ChargingDock and NonChargingDock allows to implement parameters specific to your docking station on how to detect if the docking was succesful. The docking procedure seemed to be wrapped around a behavior tree. It can be set up to work within your own behavior tree. The docking steps are:

  1. Retrieve dock pose.
  2. Navigate to dock (if not within range).
  3. Use the docking pluging to detect the dock.
  4. Enter a vision-control loop to reach the docking pose.
  5. Exit the vision-control loop once the contact has been detected.
  6. Wait until chargin starts (if applicable) and return success.

The dock database is an external yaml in server config or in action request. The controller is nav2_graceful_controller. Further details on the docking API and dock configurations can be found on the server’s documentation.

Radar Tracks for Path Planning in the presence of Dynamic Obstacles

radar

Obstacles in ROS 2 are typically represented by a stationary point in an occupancy grid, usually inflated by a user-defined radius and cost scaling factor. These points are updated frame-to-frame via ray tracing. A limitation on this approach is the difficulty on using obstacle dynamics.

Mesh navigation

meshnav

The Mesh Navigation software bundle allows efficient robot navigation on 2D manifolds, which are represented in 3D as triangle meshes. It enables safe navigation in various complex outdoor environments by using a modularly extensible layered mesh map. Layers can be loaded as plugins representing specific geometric or semantic metrics of the terrain. This allows the incorporation of obstacles in these complex outdoor environments into path and motion motion planning.

Software and Tool Integration

Main Street Autonomy

alt text

The main product advertised by Main Street Autonomy was their sensor calibration “Calibration Anywhere”. This tool calibrates all lidar, radar, camera, IMU, and GPS/GNSS sensors in a single pass, providing both intrinsic and extrinsic parameters, along with time offsets between sensors—all without the need for checkerboards or targets. It is, definitely, a very interesting product. What impressed me most, however, was their vSLAM solution, which demonstrated impressively stable features even in challenging scenarios with highly uniform textures, as shown in the photo here.

CROSS: FreeCAD and ROS

CROSS is a FreeCAD workbench to generate robot description packages (xacro or URDF) for ROS.

Robotics Education and Community Resources

The defenitive guide to ROS Mobile Robotics

Steve Macenski introduced to us one of his last papers, a survey that answers to the following questions:

  • What are his algorithm choices
  • How do they work
  • When should I use what and why
  • What features exist?
  • How do they compare computationally?

Foss Books

VM (Vicky) Brasseur presented to us her books:

IEEE Robotics and Automation Practice (RA-P)

ieee

A new IEEE journal with less focus on theoretical contributions and more on applied research. Link to the journal here

Awesome conferences and schools list

A very much needed repository with links to interesting robotics conferences and schools, with a list going as far as 2028! Follow the link here.

Other Interesting tools

Happypose

happypose

Krzysztof Wojciechowski presented happypose_ros, a ROS 2 wrapper of Happypose for 6D object pose estimation. Given an RGB image and a 2D bounding box of an object with known 3D model, the 6D pose estimator predicts the full 6D pose of the object with respect to the camera. As a sum up, Happypose introduces a library for:

  • Single camera pose estimation.
  • Multi camera pose estimation.
  • 6D pose estimation of objects in the scene (for the pretrained objects). Easy Fine-tuning with different objects is currently a future work.
  • ROS API. The slides on how the method works can be found here

Pssst, ey, look, we are women in robotics, wooo! Pssst, ey, look, we are women in robotics, wooo!

]]>My ROSCON 2024 digest - Day 12024-10-21T06:15:00+00:002024-10-21T06:15:00+00:00https://olayasturias.github.io/blog/2024/roscon-odense-1This year, our neighbor city Odense held the ROSCON conference, which I had the honour to attend :slightly_smiling_face: I collected some notes for my future self (and maybe the present you?), that I have collected in this post for day 1, and on my next post for days 2 and 3.

If any of these notes raises your interest, you can watch the full talks in this link. Enjoy!

Workshop 3: Open source, open hardware hand-held mobile mapping system for large scale surveys

This workshop consisted of deploying an open-hardware hand-held device to record large-scale datasets and perform mapping tasks on them. The device comprises a low-cost Lidar sensor with a GPS unit and an IMU sensor. A Raspberry Pi 4 board collects the data. Data processing is performed offline after the collected data is transferred to our PCs.

The open hardware

The bill of materials for the device can be found under the Mandeye repository. The bill of materials comprises:

  • A Raspberry 4 (or 5).
  • The Lidar sensor Livox Mid-360. It is a low-cost lidar intended for low-speed robotics. It provides a field of view of 360*59 degrees, with a minimum range of 0.1m and a 40-line point cloud density.
  • 3D printed cases for the Lidar and the Raspberry Pi.
  • A Ulanzi tripod with a built-in battery is used to power up the unit (and hand-hold the device).
  • … among other materials you can check out here.

The main selection criteria for the provided materials is to keep the device (relatively) low cost, in this case less than 1000 euros.

The open software

The open softwareware is all collected under the HDMapping project. Some (quick) interesting notes about this project:

  • In Windows, the deployment is quite off-the-shell. You just need to download and execute the binaries without any installation process.
  • It has been developed with the pure objective of collecting ground-truth datasets. To that aim, it allows manual drift correction and algorithmic-based loop closing for data optimization.

BoF: Accelerating robot learning at scale in simulation :hatched_chick:

alt text

Aside from the workshops, day 1 also featured a series of “Bird of Feather” sessions. Birds of a Feather (BoF) sessions are informal gatherings at a pre-arranged time and place by individuals with a common interest in a topic. Markus Wuensch, head of the Robotics Ecosystem at NVIDIA, hosted this session and proposed the following bullet points:

  • Intro
  • State of your development
  • Products/use cases
  • ROS in robot learning
  • Tools
  • Challenges
  • Wishlist
  • Future

All things said, this was oriented to get ideas and feedback from the attendants about using NVIDIA hardware for simulators in general, and NVIDIA’s simulator Isaac Sim in particular.

alt text

The BoF session was joined by people with very different backgrounds (manipulation, planning, navigation…) with a vast majority of us applying computer vision to these problems. Therefore, most use simulators with realistic renderings: Unreal Engine, Unity, or Open3D (more on this one later), were the most mentioned. Gazebo is the primary ROS simulator, which, despite its outstanding capabilities for robot simulation, falls short when it comes to visual renderings. Surprisingly, very few people in the room were using Isaac Sim. People commented on the high computational requirements and the difficulty of deploying it. People in underwater robotics commented on the need for underwater simulation :wink:

Most people utilize simulators for training models that will be deployed in real robots later. Markus concurred on this and mentioned that NVIDIA aims for what they call the “three body computation”, divided into train, simulation, and run. Their aimed solution consists of training in simulation and deploying the trained models at runtime, which is the aim for most of the attendees.

We discussed limitations in simulators. Beyond the limitations that I mentions earlier, when it comes to computer vision, the repetition of texture patterns can be troublesome. If, for example, you’re using those texture renderings in a visual localization algorithms, the detection of those repeated patterns can mislead the localization estimate. One of the attendees mentioned the use of procedural generation of textures to address this problem.

My main take-home message from this discussion is a confirmation of the lack of consensus regarding simulation. Most of us take whichever simulation in the literature looks more convenient and adapt it to our specific needs. For this purpose, the preferred simulators are open-source and have a relatively simple API (which is not the case for Isaac Sim).

This raises a few questions: does having a simulator that fulfills everyone’s needs make sense? If so, how could that be possible? Is there any billionaire in the room willing to fund such an open-source initiative?

]]>Setting a Jetson AGX Orin for robotics development (III) - event-based SLAM2024-01-04T06:15:00+00:002024-01-04T06:15:00+00:00https://olayasturias.github.io/blog/2024/the-cpp-diaries-3Welcome to the third post on setting up a Jetson AGX Orin for robotics development! Today, we will focus on setting up a set of state-of-the-art methods for event-based localization. The structure of the post goes as follows:

Reminder: For this project I will be using the JETSON AGX ORIN DEVELOPER KIT. Some things to take into account about this board:

  • It is an ARM64 architecture.
  • It has Ubuntu 20.04 installed
  • It comes with the Nvidia Jetpack 5.1.2
  • It has ROS Noetic

1. ESVO-Extension

This installation has been successfully tested in Ubuntu 20.04 with ROS Noetic, OpenCV 4.5.4 and Eigen 3.4.

1.1 Prerequisites

sudo apt-get install libqhull-dev python3-pycryptodome
pip3 install python-gnupg
  • FLANN library == 1.7.0. Clone and compile it in your favourite folder: 
      git clone https://github.com/flann-lib/flann.git
  cd flann
  mkdir build && cd build
  cmake ..
  make -j$(nproc)
  sudo make install
  • Bullet library . Clone and compile it in your favourite folder: 
      git clone https://github.com/bulletphysics/bullet3.git
  cd bullet3
  mkdir build && cd build
  cmake ..
  make -j$(nproc)
  sudo make install
  • PCL library v. 1.10. Follow the installation instructions here. When building, ensure that it is compiled with QHull as follows: 
      cmake -DWITH_QHULL=TRUE ..
  • ROS Noetic dependencies. I will install this in my ros_noetic_ws, therefore, all the comands for each package below are preceded by a cd ros_noetic_ws/src.
    • vision_opencv 
        git clone https://github.com/ros-perception/vision_opencv.git
  cd vision_opencv && git checkout noetic
  cd ../..
    • pcl_msgs 
        git clone https://github.com/ros-perception/pcl_msgs.git
  cd pcl_msgs && git checkout noetic-devel
    • geometry2 
        git clone https://github.com/ros/geometry2.git
  cd geometry2 && git checkout noetic-devel
    • perception_pcl 
        git clone https://github.com/ros-perception/perception_pcl.git 
  cd perception_pcl && git checkout melodic-devel
  • Other ROS dependencies. These are the dependencies that I will install in my catkin_ws. Similarly, all the comands for each package below are preceded by a cd catkin_ws/src.
    • cnpy_catkin 
        git clone https://github.com/uzh-rpg/cnpy_catkin.git

1.2 Installation

ESVO_extension is a ROS package that I will intall in my catkin_ws workspace.

cd catkin_ws/src
git clone https://github.com/olayasturias/ESVO_extension.git
cd ..
catkin build esvo_time_surface esvo_core

For installing it and other dependencies, follow the installation instructions in the repo.

2. EventEMin

2.1 Dependencies

sudo apt install libomp-dev
sudo apt-get install libgsl-dev

2.2 Installation

This worked straight away. The repo claims that it needs an Eigen3 version lower (but not including) 3.4. However, I haven’t had any trouble with Eigen 3.4 so far!

cd ~/catkin_ws/src
git clone --recurse-submodules https://github.com/ImperialCollegeLondon/event_emin_ros.git
cd ..
catkin build event_emin_ros

3. EVO

3.1 Dependencies

The command ./rpg_dvs_evo_open/install.sh install the dependencies. However, some need to be installed manually (only applicable for ARM architectures though):

sudo apt-get install libfftw3-dev

cd ~/catkin_ws/src
git clone https://github.com/uzh-rpg/fast_neon.git
git checkout test/aarch64-compilation 
cd ../..
catkin build fast

3.2 Installation

cd src/ && https://github.com/uzh-rpg/rpg_dvs_evo_open.git

]]>Setting a Jetson AGX Orin for robotics development (II) - Inivation’s event camera2024-01-02T06:15:00+00:002024-01-02T06:15:00+00:00https://olayasturias.github.io/blog/2024/the-cpp-diaries-2Welcome to the second post on setting up a Jetson AGX Orin for robotics development! Today, we will focus on setting up all the required drivers and dependencies for Inivation’s event camera, including the ROS installation and ROS packages. The structure for this post goes as follows:

Once you’ve gone through all these steps, you will be ready to run the iniVation’s event camera on your board.

Reminder: For this project I will be using the JETSON AGX ORIN DEVELOPER KIT. Some things to take into account about this board:

  • It is an ARM64 architecture.
  • It has Ubuntu 20.04 installed
  • It comes with the Nvidia Jetpack 5.1.2

1. Prerequisites

sudo apt install liblz4-dev libzstd-dev libgoogle-perftools-dev ninja-build python3-pip
  • Compilers
    • GCC and G++ compiler should be version 10.0 as required by the inivation driver. Check which version you have set up by default as: 
        gcc -v
  g++ -v

      For me it is:

        gcc (Ubuntu 9.4.0-1ubuntu1~20.04.2) 9.4.0
  g++ (Ubuntu 9.4.0-1ubuntu1~20.04.2) 9.4.0

      Therefore, we first need to install gcc and g++ 10.0:

        sudo apt install gcc-10 g++-10

      And then set it as the default compiler:

        sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-10 10
  sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-10 10
  • Boost library >= 1.76. Check which version you have installed as: 
      apt list --installed | grep libboost

    If your version is less than 1.76, you can install a newer version as follows:

      sudo add-apt-repository ppa:mhier/libboost-latest
  sudo apt update

    And then (e.g. for version 1.83):

      sudo apt install libboost1.83-all-dev

    However, before deciding which version to install, I recommend you to read section 3. Setting up ROS Noetic in this post.

  • OpenSSL 
      sudo apt install libssl-dev

  • Eigen3. In Ubuntu 20.4, the eigen3 version installed by the apt is 3.3.4. To install the required version 3.4, follow the instructions depicted here.

  • fmt. Similarly, the apt manager installs an older version of the fmt library. To install the latest release: 
      wget https://github.com/fmtlib/fmt/releases/download/10.1.0/fmt-10.1.0.zip
  unzip fmt-10.1.0.zip
  cd fmt-10.1.0
  mkdir build && cd build
  cmake -DCMAKE_POSITION_INDEPENDENT_CODE=ON ..
  make
  sudo make install

  • OpenCV additional modules. The default installation is missing some OpenCV modules. You can follow the instructions from my previous post to install all required modules.

  • (Optional) SDL, with installation instructions here.
  • (Optional) Aravis, with installation instructions here.
    • In Ubuntu 20.04, the meson version installed by the apt is too old for this build. Therefore, you need to install it manually: 
        git clone https://github.com/mesonbuild/meson
  cd meson
  python3 -m pip install -I meson
  python3 -m pip install ninja
    • When running ninja install for installing Aravis, you might get the error ModuleNotFoundError: no module named 'mesonbuild'. This is due to the installation path for the meson library. You can fix it by adding it to the PYTHONPATH: 
        export PYTHONPATH=$(python3 -m site --user-site)

      and then, running the ninja install command as:

        sudo -E ninja install

2. Setting up the camera driver

Inivation provides instructions for the Ubuntu packages (for Ubuntu 20.04 in my case) on their official website. However, the apt installation did not work: The Orin comes with CuDNN 8.0, which requires OpenCV >= 4.4, and the apt dv-runtime-dev installation is looking for OpenCV 4.2. Therefore, we’ve installed the libraries from source. For that, we must download the DV repos in our favorite folder (for me, $HOME/dev). Let’s go one by one:

  • libcaer: 
      git clone https://gitlab.com/inivation/dv/libcaer
  cd libcaer
  cmake -DCMAKE_INSTALL_PREFIX=/usr .
  make
  sudo make install
  • dv-processing: 
      git clone https://gitlab.com/inivation/dv/dv-processing
  cd dv-processing
  cmake -DCMAKE_INSTALL_PREFIX=/usr .
  make
  sudo make install
  • dv-runtime: 
      git clone https://gitlab.com/inivation/dv/dv-runtime
  cd dv-runtime
  cmake -DCMAKE_INSTALL_PREFIX=/usr DVR_ENABLE_PROFILER=ON .
  make
  sudo make install

3. Setting up ROS Noetic

Fighting demons or being the demon? Yes, we’re setting up ROS1 here. It is still the primary OS for event cameras. This section needs a bit of storytelling: This is not my first iteration of installing all these libraries in the Orin. In a previous iteration, I installed all the libraries (OpenCV, ROS, etc.), and the installation of the iniVation drivers would fail. I blamed OpenCV for these issues (don’t look at me like that, OpenCV; we’ve been through this before). But I think now that the problems actually came from ROS and its dependencies!

It turns out that ROS Noetic uses a version of Boost older than the one that the inivation driver needs. Because I installed the iniVation driver first, the compilation went down smoothly. However, now I can’t install ROS Noetic with apt. We will need to install it from source. Damm! Seems like we’re being the demons and fighting demons after all.

Note for future self: maybe if instead of Boost 1.83 I would have installed the minimum required version by inivation, that is, Boost 1.76, then maybe sudo apt install ros-noetic-desktop-full would have worked? It does not seem likely because I tried running that command with aptitude, and the only option it gave for Boost was 1.71. But this could be something worth a try.

Now, I will put here the instructions for installing ROS Noetic from source. They’re slightly modified with respect to the original instructions. For convenience, I will follow the same structure.

3.1 Installing bootstrap dependencies

sudo apt-get install python3-rosdep python3-rosinstall-generator python3-vcstools python3-vcstool build-essential

3.2 Initializing rosdep

sudo rosdep init
rosdep update

3.3 Installation

3.3.1 Create a catkin workspace

I will call my workspace ros_noetic_ws. You can give it whichever name you want.

mkdir ~/ros_noetic_ws
cd ~/ros_noetic_ws

Download all the ROS packages into the source:

rosinstall_generator desktop --rosdistro noetic --deps --tar > noetic-desktop.rosinstall
mkdir ./src
vcs import --input noetic-desktop.rosinstall ./src

3.3.2 Resolve dependencies

I’m putting this command here, but it gave me an error. It gave me the same error as for the sudo apt install ros-noetic-desktop-full: unresolved dependencies. Basically, I had to manually install all the dependencies myself. A bit annoying but not too bad.

 rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro noetic -y

The dependencies I had to install are:

sudo apt install -y libassimp-dev liburdfdom-dev liblog4cxx-dev libgpgme-dev libbz2-dev libgtest-dev sip-dev pyqt5-dev python-sip-dev pyqt5-dev-tools python3-pyqt5 libtinyxml2-dev libtinyxml-dev libpoco-dev  libconsole-bridge-dev python3-empy

Another dependency available in apt but that needs to be installed manually due to the unresolved dependencies with Boost is libogre, with building instructions indicated here.

  • pugixml (required by Ogre): 
      git clone https://github.com/zeux/pugixml.git
  cd pugixml
  mkdir build && cd build
  cmake -DCMAKE_INSTALL_PREFIX=/usr DVR_ENABLE_PROFILER=ON ..
  make && sudo make install
  • Ogre v1.12.2, as it is the latest version supported by ROS Noetic. First, install the dependencies: 
      sudo apt install libcppunit-dev

    Then proceed to the installation itself:

      git clone https://github.com/OGRECave/ogre.git
  cd ogre
  git checkout v1.12.2  # Replace with the version you need

    NOTE: If you get compilation issues with Ogre, you can proceed without installing the ROS packages that depend on it. Those are rviz and all the packages depending on rviz. This means that you will have to run the catkin_make_isolated command with the flag: --ignore-pkg rviz rviz_plugin_tutorials rviz_python_tutorial librviz_tutorial rqt_rviz

  • image_common ROS stack: for some reason, the image_common stack downloaded by the previous steps does not include all the packages that we will need. You can manually download it into the workspace as: 
      cd ~/ros_noetic_ws/src
  git rm -rf image_common && git clone https://github.com/ros-perception/image_common.git
  cd image_common && git checkout noetic-devel

3.3.3 Building the catkin workspace

This command is slightly different from the original instructions. The difference lies in the flag -DCMAKE_CXX_FLAGS=-DBOOST_TIMER_ENABLE_DEPRECATED. Without it, the compilation gives an error due to a deprecated header:

/usr/include/boost/progress.hpp:23:3: error: #error This header is deprecated and will be removed. (You can define BOOST_TIMER_ENABLE_DEPRECATED to suppress this error.)

What the flag does is ignore the error and resume the compilation:

./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=/usr/bin/python3 -DCMAKE_CXX_FLAGS=-DBOOST_TIMER_ENABLE_DEPRECATED

Now, you should have successfully compiled all the ROS Noetic packages! You can source the workspace by adding the following line to the end of the ~/.bashrc:

source ~/ros_catkin_ws/install_isolated/setup.bash

4. Setting up the ROS camera driver

We finally have all the external libraries ready to run the ROS driver for the iniVation camera. Let’s install it!

4.1 Some extra dependencies

After following the previous instructions, you might still miss the following dependencies:

pip3 install defusedxml netifaces

4.2 Setting up the workspace

Let’s set up a separate workspace for ROS development. Doing so is as simple as creating a folder containing a source workspace and building it with catkin:

mkdir -p catkin_ws/src 
cd catkin_ws
catkin build

4.3 Cloning and building dv-ros

The ROS package that contains the code to run the iniVation camera is dv-ros. The original code is in this repo. However, we will use this fork of the repo. This is because, as of today (Jan 24), the original code is not compatible with fmt10 (although that’s not the case for the camera drivers).

cd catkin_ws/src 
git clone https://github.com/olayasturias/dv-ros
cd ..
catkin build --cmake-args -DCMAKE_C_COMPILER=gcc-10 -DCMAKE_CXX_COMPILER=g++-10

If the compilation is successful, test that the driver is working:

source devel/setup.bash
roslaunch dv_ros_visualization event_visualization.launch
]]>My ICRA 2023 digest2023-06-05T06:15:00+00:002023-06-05T06:15:00+00:00https://olayasturias.github.io/blog/2023/my-icra-digestMucho texto: Table of contents

Day 1. Workshops and Tutorials

W.1. Distributed graphs workshop

The chair for this workshop was Andrew Davison, who did a very short but interesting presentation as introduction to the workshop. He definitely left me with the wish of listening more from him, but two things he mentioned particularly catched my interest:

  1. The importance of graphs (well, yes, of course, is a workshop about graphs), and how now that the software aiming for graph structures is raising, we need hardware that is particularly designed for graphs: the Graphcore Intelligence Processing Unit (IPU). When he talked about it, it sounded a lot like something I used during my bachelors: the FPGAs. Basically, a computer specifically designed for parallel computing. I did a quick read online, and it seems like both are optimized for parallel computing, with the Graphcore being specially designed for (surprise surprise) graph structures, and the FPGA being more flexible for any kind of parallelized computing that you need. This post in Medium does quite a throughout analysis.
  2. FutureMapping: factorized computation for spatial AI. A framework that parallelizes computing between the nodes in a graph. Then, those nodes randomly communicate with each other in order to optimize computational resources. Here is a link to the paper and a link to an online tutorial.

W.1.A. A new factor-graph lens on (Clustered) Belief Propagation

by Frank Dellaert

Before getting into factor graphs, he quickly mentioned his recent work in NeRFs. See a post about it here. He also advertised a free book with jupyter notebooks with different aspects about robotics available here. Definitely worth looking into!

Brief review of factor graphs

Frank Dellaert’s work on factor graphs is very well known and available on a free book. At the begginning of the talk, he did a quick review on the theory within that book.

My quick notes:

The use of factor graphs allows Sparse matrix factorization. Then we can apply QR factorization on Factor Graphs. The Bayes tree is a powerful graphical model that enables incremental Smoothing and Mapping (iSAM). An example on iSAM is available here at section 7 and here.

Gibbs sampling via elimination

This procedure was explained with code examples available here.

Take a nonlinear factor graph, for example, one in which the measurements come from a bearing sensor. When obtaining joint Marginals of graph variables results are “wrong” because the problem is nonlinear, and the result is a Laplace approximation. We can get better results by Gibbs sampling. First pick Markov blanket. Keep sucesion of samples given the current state of the other variables (elimination). Instead of Laplace approximation, use it as Metropolis proposal.

A different take on loopy BP

Similarly, code examples available here.

Belief propagation: variation approximation. Approximate true posterior with simpler density. Pretend true posterior is multiplication of factors. Converges to true mean but covariances are overconfident. Loopy SAM (2007): Gauss-Seidel propagation. Opposite to Gibbs Sampling: eliminate separator last.

Clustered belief propagation

More code examples.

Identify clusters and for a variational approximation at the cluster level – rather than individual belief factors, have Bayes nets/trees as variational components. Basically cluster the graph. Uses all sparsity available in the system. Converges much faster because explores inherent structure of the domain.

W.2. ICRA 2023 Workshop on Unconventional spatial representations: Opportunities for robotics

I only attended one talk in this workshop. It seemed to be about using unconventional sensors for perception tasks in general, for what I could see on the posters. Unfortunately, the posters are not available online and I didn’t take photos of them.

W.2.A. Understanding and Representing the Sea environment for Autonomous Ship Navigation

by Hyun-Taek Choi

As a professor in the Korea Research Institute of Ships and Ocean Engineering, he presented quite some interesting field words in that context. I noted down this one, available as a paper here:

“Marine Object Segmentation and Tracking by Learning Marine Radar Images for Autonomous Surface Vehicles”. The title is self-explanatory. Some capabilities they claim for they network are

  • Noise reduction (because radar is extremely noisy)
  • Detecting Small object and fast moving objects.

W.3. Workshop on effective Representations, Abstractions, and Priors for Robot Learning (RAP4Robots)

When I joined this workshop there was a Pannel session taking part, with professors discussing around the question: “Do better representation lead to better generalization?”. E.g., about the possibility of using an alternative to Euclidean maps and object poses, and about applying representation learning. There was no clear conclusion on the question, which I think is a good sign for this being an open research question.

W.3.A. A trilogy of priors: the vision the design and the data

by Edward Johns

Edward introduced to us a series of very works within his field of research, which is robot manipulation. This one called my attention, since they use generative models (more concretely, DALL-E) to generate different arrangements for the objects in the image.

W.4. Pretraining for Robotics (PT4R)

W.4.A. Where are we in the search for an Artificial Visual Cortex for Embodied Intelligence?

by Franziska Meier

From the paper under the same title which can be found here

Day 3. Orals and Posters

O.1. Award Finalists

O.1.1. Graph Neural Networks for Multi-Robot Active Information Acquisition

by Mariliza Tzes
  • Active information acquisition
  • Team of drones – no dinamics – they have sensors. The dynamics are the hidden state.
  • Planning horizon – informative path
  • They try to reach scalability, computational efficiency and optimality with I-GBNet.
  • Robots exchange info with neighbors, forming a communications graph. Network graph contains info about robot state – fed into I-GBNet to be updated. For each node, predict control input for robot.
  • Message passing layer:
    • Robot grid map.
    • Message passing aggregates robots position.
    • Each robot has its own estimate about hidden state that is propagated through graph.
    • Bayes filter to fuse measurements.
    • Obtain occupancy grid of environment.
    • ResNet blocks to encode the paths within the grid representation.
    • Then predict control input.

O.2. Localization I

O.2.1. Robust Visual Localization of a UAV Over a Pipe-Rack Based on the Lie Group SE(3)

by Jonathan Cacace

They fed the CAD model of the pipe rack to estimate the scale. Feature points are projected onto the CAD.

The code (MATLAB, simulated) for this work is available here

O.2.2. Homography-Based Loss Function for Camera Pose Regression

by Clémentin Boittiaux

Convergence of the proposed Homography loss Image: Convergence of the proposed Homography loss. From the original repo available here.

  • SOA: Inertial measuruement based catenary […] underwater vehicles.
  • Same metric as structure from motion
  • Projected to infinity function no differentiable
  • Reprojection loss doesn’t have these problems.

O.3. Deep Learning for Visual Perception

O.3.1. Object-Aware Monocular Depth Prediction with Instance Convolutions

by Enis Simsar and Evin Pinar

  • InstanceConv takes segmentation mask into account.
  • Aggregates features coming from same segment. They use superpixels segmentation mask as input.
  • Superpixels can oversegment depending on light intensity, but they can used in “any” scence as opposed to trained methods. MaskCNN etc don’t have guarantee over occlusion boundaries. Superpixels are off-the-shelf, don’t require training.

The code is available here

O.3.2. Uncertainty guided policy for active robotic 3D reconstruction using Neural Radiance Fields

by Soomin Lee

  • Instead of voxels n stuff, using NERFs.
  • They suggest new paradigm: Ray-based volumentric uncertainty. Uncertainty in ray space.
    • Weighted combination of colors, weight distribution of colors to measure how certain – a color has to have a clear peak?
    • Entropy metric – low entropy clear distribution peak

This work’s website (not code though) is here.

O.3.3 Detaching and Boosting: Dual Engine for Scale-Invariant Self-Supervised Monocular Depth Estimation

by Peizhe Jiang

  • Zooming and moving similar effects – they use zooming for data augmentation (extrinsics need to be adapted). Predict same depth for zoomed and unzoomed images (if they are the same).
  • Cross attention to get relationship between objects.

This post is a work in progress…

]]>Setting a Jetson AGX Orin for robotics development (I) - building the basis2023-06-05T06:15:00+00:002023-06-05T06:15:00+00:00https://olayasturias.github.io/blog/2023/the-cpp-diariesFor this project I will be using the JETSON AGX ORIN DEVELOPER KIT. Some things to take into account about this board:

  • It is an ARM64 architecture.

1. Setting up Ubuntu

Setting up Ubuntu in the Jetson is quite straightforward - or at least, similar to what you do in a normal PC. Here are some of the steps that showed up in the process:

APP Partition size

Just leave the default, which is the maximum accepted size: 59342 MB

Select Nvpmodel Mode

What amount of power you want to use MAXN - (Default) which corresponds to maximum

Install Chromium Browser

I selected yes but it gave the error “Cannot install chromium snap”, so let’s skip it for now.

Another thing we will want to do is to ssh into our board. Good news: it’s already set up and enabled in the board!

2. Libraries

Let’s start by checking which library version the board has by default:

  • Compilers
    • GCC compiler: GCC is the GNU compiler. It includes a compilation of compilers, including…
    • G++ compiler, which is specifically designed for compiling C++ source code. It is not installed by default, can be installed as: 
        sudo apt update
  sudo apt install g++

      The version installed should be the same as for gcc:

        gcc -v
  g++ -v

      Which for me is:

        gcc (Ubuntu 9.4.0-1ubuntu1~20.04.2) 9.4.0
  g++ (Ubuntu 9.4.0-1ubuntu1~20.04.2) 9.4.0
  • C++ version. The compilers are compatible with several C++ versions, which are usually pointed during compilation using the flag -std=c++14, -std=c++17, or -std=c++20. The c++ version used will be conditioned by the ROS version is compatible with NVidia’s setup.
  • Debugger: to install the gdb debugger run: 
      sudo apt install build-essential gdb
  • Visual Studio Code: I will install whichever last version they have (not so important), but taking into account that the binary should be for a arm64 architecture. I followed the setup instructions here.
  • Follow NVIDIA’s quickstart under this link.
  • ROS version: it is not installed, but it is important to note that only ROS Foxy is supported, which is compatible with C++14. You can follow the official installation instructions here.
  • CUDA. The latest CUDA version compatible with the Jetson Orin must be 11.4, as pointed out in this forum. If you installed the nvidia-jetpack it should be as simple as: 
      sudo apt update
  sudo apt install cuda-toolkit-11-4

    And then open your .bashrc file and add the CUDA installation directory to your path as:

      export PATH=/usr/local/cuda-11.4/bin${PATH:+:${PATH}}
  export LD_LIBRARY_PATH=/usr/local/cuda-11.4/lib64{LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}
  • OpenCV (GPU accelerated). Actually, the nvidia-jetpack already installs openCV, although not with CUDA. You can check so with the following command: 
      dpkg -l | grep libopencv

    I followed the instructions introduced in this video pointing to this installation file. Some things to note:

    • -D CUDA_ARCH_BIN=8.7 for Orin.
    • -D CUDNN_VERSION='8.0' for the CUDA version used here. Funnily enough, OpenCV would not find my CuDNN, which should have been automatically installed with the nvidia-jetpack. I ran again sudo apt install nvidia-jetpack, and now it installed some remaining packages (including CuDNN).
    • -D BUILD_opencv_python2=OFF because I’m not interested in using python 2.

    Once you’ve set up everything you can install OpenCV automatically as ./build_opencv.sh 4.5.4.

Next: setting up the driver for the event-camera DVXplorer Mini!

]]>Surveying SLAM algorithms with ROS (I) - Installation2022-03-01T06:15:00+00:002022-03-01T06:15:00+00:00https://olayasturias.github.io/blog/2022/slam-surveying-installIn this post we will install different state-of-art SLAM algorithms and test that they work in our computer.

1. ORB-SLAM3

ORB-SLAM3 is the first real-time SLAM library able to perform Visual, Visual-Inertial and Multi-Map SLAM with monocular, stereo and RGB-D cameras, using pin-hole and fisheye lens models. In all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature, and significantly more accurate.

1.1 Installation

Install from this fork on ORB-SLAM3’s repository.

  • Some notes in the dependencies:
    • If OpenCV gives you a version error, modify the CMakeLists to look for the opencv version that you have installed. For example, if you are using ROS Melodic you will have OpenCV 3.2: find_package(OpenCV 3.2)
    • ROS packages for ORB-SLAM3 need rospkg: 
        sudo apt install python-rospkg
    • Install Eigen3 (I am using 3.4.0)
      • Some dependencies for eigen (optional): 
          sudo apt install qt5-default libsparsehash-dev libadolc2 libmpfr-dev fftw-dev
      • Download the source code in your preferred folder: 
          wget https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz
      • unzip the file: 
          tar -zxvf eigen-3.4.0.tar.gz
      • prepare the directory for building 
          cd eigen-3.4.0
  mkdir build
  cd build
      • Build it 
          cmake ..
  make
  sudo make install
      • check which eigen version you have installed with: 
          pkg-config --modversion eigen3
    • Install Pangolin
      • Install pangolin dependencies 
          sudo apt-get install -y  libglew-dev libboost-dev libboost-thread-dev  
  libboost-filesystem-dev ffmpeg libavutil-dev libpng-dev && \

        Install Pangolin last version in your preferred directory

          git clone https://github.com/stevenlovegrove/Pangolin.git Pangolin && cd Pangolin
  mkdir build
  cd build/
  cmake -D CMAKE_BUILD_TYPE=RELEASE \
  -DCPP11_NO_BOOST=1 \
  ../ && \
  make -j4 && \
  sudo make install

        if you experience errors installing Pangolin (may happen in lasts Ubuntu’s distros) try installing it with vcpkg.In that case:

          git clone https://github.com/stevenlovegrove/Pangolin.git Pangolin && cd Pangolin
  mkdir build
  cd build/
  cmake -S . -DCMAKE_TOOLCHAIN_FILE=[path to vcpkg]/scripts/buildsystems/vcpkg.cmake \
  ../ && \
  make -j4 && \
  sudo make install

      If ORB-SLAM is giving you trouble with Pangolin, try installing Pangolin v.06.

    • Install OpenCV

      • Install OpenCV dependencies

          sudo apt-get install -y libgtk2.0-dev libavcodec-dev libavformat-dev libswscale-dev software-properties-common

      • Clone and install opencv

          git clone https://github.com/Itseez/opencv.git opencv && \
  git clone https://github.com/Itseez/opencv_contrib.git opencv_contrib

  cd opencv
  mkdir build && \
  cd build/ && \
  cmake -D CMAKE_BUILD_TYPE=RELEASE \
  -D BUILD_TIFF=ON \
  -D WITH_CUDA=OFF \
  -D ENABLE_AVX=OFF \
  -D WITH_OPENGL=OFF \
  -D WITH_OPENCL=OFF \
  -D WITH_IPP=OFF \
  -D WITH_TBB=ON \
  -D BUILD_TBB=ON \
  -D WITH_EIGEN=ON \
  -D WITH_V4L=OFF \
  -D WITH_VTK=OFF \
  -D BUILD_TESTS=OFF \
  -D BUILD_PERF_TESTS=OFF \
  -D OPENCV_GENERATE_PKGCONFIG=ON \
  -D OPENCV_EXTRA_MODULES_PATH=<your-path>/opencv_contrib/modules \
  /<your-path>/opencv/ && \
  make -j4 && \
  sudo make install

    • Install ORB-SLAM third-party modules

        git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3

  cd /<your-dir>/ORB_SLAM3/ && \
  chmod +x build.sh && \
  ./build.sh

    Some recent distros for Ubuntu (20 and 22) give compilation errors. Try using C++14 instead of 11 for the compilation by replacing it on the CMakeLists as follows: sed -i 's/++11/++14/g' CMakeLists.txt

    • Build for ROS usage

        cd /<your-dir>/ORB_SLAM3 && \
  chmod +x build_ros.sh && \
  ./build_ros.sh

1.2 Testing

We will test that the ORB-SLAM scripts work for us with our webcam.

  • Install the required package to publish the webcam image as ROS messages: 
      sudo apt install ros-melodic-usb-cam
  • Start a roscore session: 
      roscore
  • Run the webcam publisher. Note that we need to remap the default topic from usb_cam because ORB-SLAM3 reads from /camera/image_raw, but usb_cam is publishing in /usb_cam/image_raw. 
      rosrun usb_cam usb_cam_node /usb_cam/image_raw:=/camera/image_raw _pixel_format:="yuyv"
  • Run the monocular version of ORB-SLAM3 as: 
      rosrun ORB_SLAM3 Mono ORB_SLAM3_PATH/Vocabulary/ORBvoc.txt ORB_SLAM3_PATH/Examples/Monocular/EuRoC.yaml

If everything is working correctly, you should be seeing something like this:

2. DSO

2.1 Installation

DSO has a ROS wrapper, but it first requires that you install DSO:

  • Install DSO. Follow the instructions from the readme in DSO’s repository.
    • If you get an OpenCV error, modify the CMakeLists to point to your version (mine is 3.2) find_package(OpenCV 3 QUIET)
    • This will compile a library libdso.a, which can be linked from external projects. It will also build a binary dso_dataset, to run DSO on datasets. It is stored in /build/lib.
  • Install ROS wrapper. Download the source code.
    • It will look for your lib file in ${DSO_PATH}/build/lib. We need to specify our DSO_PATH, which is the path to your dso repository folder: 
            export DSO_PATH=YOUR_PATH/dso

      you chan check if it was correctly set with

            echo $DSO_PATH
    • build it with: 
            catkin build dso_ros

2.2 Testing

We will test that the dso_live scripts work for us with our webcam.

  • If you didn’t do it before, install the required package to publish the webcam image as ROS messages: 
      sudo apt install ros-melodic-usb-cam
  • Start a roscore session: 
      roscore
  • Run the webcam publisher. Note that usb_cam is publishing in /usb_cam/image_raw. 
      rosrun usb_cam usb_cam_node /usb_cam/image_raw:=/camera/image_raw _pixel_format:="yuyv"
  • Run dso: 
      rosrun dso_ros dso_live image:=/usb_cam/image_raw \
      calib=XXXXX/camera.txt \
      gamma=XXXXX/pcalib.txt \
      vignette=XXXXX/vignette.png \

    • Note that you need to provide at least a camera.txt file. It has the format:

        fx fy cx cy k1 k2 r1 r2
  in_width in_height
  "crop"/"full"/"fx fy cx cy 0"
  out_width out_height

    • An example of a camera.txt could be like this:

        Pinhole 458.654 457.296 367.215 248.375 0
  640 480
  crop
  640 480
  0.110074

Now if everything works as it should, you should be seeing something like this:

3. RDS-SLAM

3.1 Installation

  • You will need to install some requirements:

    • Caffe. Follow the instructions here

      • If you get errors with hdf5 
          cd /usr/lib/x86_64-linux-gnu
  sudo ln -s libhdf5_serial.so.100.0.1 libhdf5.so # or whichever version you have
  sudo ln -s libhdf5_serial_hl.so.100.0.0 libhdf5_hl.so # same applies here
      • If you get errors with Opencv
  • Clone the RDS-SLAM repository in your /catkin_ws/src 
      git clone https://github.com/yubaoliu/RDS-SLAM
  • cd into the SLAM folder and build with the following script: 
      cd SLAM/
  ./build_thridparty.sh
  • go back to your /catkin_ws/src and build rds_slam: 
      cd ~/catkin_ws
  catkin build rds_slam

3.2 Testing

If everything is working correctly, you should be seeing something like this:

[under construction]

]]>creating ROS metapackages2022-02-28T06:15:00+00:002022-02-28T06:15:00+00:00https://olayasturias.github.io/blog/2022/ros-metapackagesThis post will make sense to you if you have already familiarized yourself with ROS packages.

As you may know, one of the main strengths of ROS is the possibility of making modular software. By creating specialized packages, you can easily reuse them for other projects. However, as your project gets bigger, so will get your source folder, making it messy. What’s more, if you have a set of packages you are currently working on, compiling them separately might get painful.

ROS has a solution for both problems: metapackages (referred to as stacks in older versions of ROS). A metapackage is a special kind of package meant to gather a subset of packages in it. It allows you to organize your packages within a single folder and compile all packages inside at the same time.

Creating metapackages is straightforward, but I haven’t found much documentation, so here is a simple tutorial on creating your own metapackages!

First of all, you need to know what the structure of a stack of metapackages looks like:

metapackage
│
└───package1
│   │   CMakeLists.txt
│   │   package.xml
│   └─── ...
│       │   ...
│   
└───package2
│   │   CMakeLists.txt
│   │   package.xml
│   └─── ...
│       │   ...
│ 
└───metapackage
    │   CMakeLists.txt
    │   package.xml

That is, you will have a folder with the same name as your metapackage, containing the metapackage itself and the other packages that you want to gather in it. In this example, you will create a folder called metapackage in your source folder, and then move inside your packages.

First, create the folder that will contain your metapackage and cd into it:

mkdir metapackage && cd metapackage

Next, create a package like this:

catkin_create_pkg metapackage --meta

The --meta flag indicates to catkin that you are creating a metapackage, and thus it creates a CMakeLists.txt and a package.xml accordingly. Let’s take a look to what’s inside those files:

cmake_minimum_required(VERSION 2.8.3)
project(metapackage)
find_package(catkin REQUIRED)
catkin_metapackage()

The CMakeLists of a metapackage is quite empty; we only have the catkin_metapackage() directive. We won’t need to modify it. On the other hand, we will need to modify the package.xml to indicate which packages are included within the metapackage. We can do so by writing:

    <exec_depend>package1</exec_depend>
    <exec_depend>package2</exec_depend>

Just with that, your metapackage is ready to run. You can build it, and it will build all the packages within it at the same time:

catkin build metapackage

And that’s it! Now you can have your workspace perfectly organized :smile:

]]>LateX table tricks for no more table flips (╯°□°)╯︵ ┻━┻2022-01-30T06:15:00+00:002022-01-30T06:15:00+00:00https://olayasturias.github.io/blog/2022/latex-tablesDo you know when you have an idea for something that can be done quickly, but then you start to have more ideas, one on top of the other and suddenly the small task has become a big task? Well, that’s what happened in this post!

Here, I’ve gathered together a small set of helpful latex commands that I always use, but still, I have to constantly google them because they don’t want to stick into my mind. This set of commands produces the featured image you can see on top of this text. Those are:

  • An amazing placeholder for images containing ducks, from the package duckuments (loving the package and its name).
  • An image that consists of a grid of images by using the tabularx package
  • Merging multiple columns with centred alignment.
  • Merging multiple rows. Interestingly enough, the vertical alignment has to be manually set (the [3em] you see in the latex text below).

  • Merging multiple columns and rows simultaneously.

    \documentclass[12pt,a4paper]{article}
\usepackage{graphicx}

\usepackage{duckuments}
\usepackage{tabularx}
\usepackage{adjustbox}
\usepackage{multicol}
\usepackage{multirow}
\begin{document}

\begin{figure}[b!]
\centering
\resizebox{\textwidth}{!}{\begin{tabular}{ccccc}

%%%%%%%%%%%%%%%%%%%%%%5
\includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}\\\relax
%%%%%%%%%%%%%%%%%%%%%%%%
\includegraphics[width=.2\linewidth]{example-image-duck}
& \multicolumn{2}{c}{\multirow{2}{*}[3em]{\includegraphics[width=.4\linewidth]{example-image-duck}}}
& \multicolumn{2}{c}{\includegraphics[width=.2\linewidth]{example-image-duck}}\\
%%%%%%%%%%%%%%%%%%%%%%5
\includegraphics[width=.2\linewidth]{example-image-duck}
& \multicolumn{2}{c}{}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}\\\relax
%%%%%%%%%%%%%%%%%%%%%%%%
\includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}\\
%%%%%%%%%%%%%%%%%%%%%%%%
\includegraphics[width=.2\linewidth]{example-image-duck}
& \multicolumn{2}{c}{\multirow{2}{*}[3em]{\includegraphics[width=.4\linewidth]{example-image-duck}}}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \multirow{2}{*}[1em]{\includegraphics[width=.2\linewidth]{example-image-duck}}\\
%%%%%%%%%%%%%%%%%%%%%%5
\includegraphics[width=.2\linewidth]{example-image-duck}
& \multicolumn{2}{c}{}
& \includegraphics[width=.2\linewidth]{example-image-duck}
&  \\\relax
%%%%%%%%%%%%%%%%%%%%%%%%
\includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}
& \includegraphics[width=.2\linewidth]{example-image-duck}


\end{tabular}}
%The more favourable lighting conditions in SeaFloor yield better results than SandPipe's dark environment
\label{fig:segmentation}
\end{figure}


\end{document}
]]>A notebook on the Extended Kalman Filter2021-05-13T06:15:00+00:002021-05-13T06:15:00+00:00https://olayasturias.github.io/blog/2021/ekfWhat to consider before diving into this post

If you have reached this post after googling how to start studying the SLAM problem and the Kalman Filter, up to this moment you might have found two kinds of sources of information:

  1. Very basic explanations but purely theoretical. You might be asking yourself: Ok, I got the basis, but what do I do know? I don’t know where to start!
  2. Not purely theoretical approaches, but too advanced for your current level.

When I found myself in this situation, I found a missing 1.2 point that helped me going from 1. to 2. So if you are in this situation, here is my recommendation:

First, the theory

First, go watch the Cyrill Stachniss course on youtube. He has a very popular course from 2013 in his channel, and all the videos are really good, but recently he has uploaded new versions of those videos that I find even better. He gives simple examples, closely related to the code, that make it much more clear to understand.

However, if you still find this course too advanced, the course from Claus Brenner, also on Youtube, could be a better start for you. But if you already know some concepts on Bayesian probability, the Cyrill course shoul be easy for you to get.

Now, start coding

Probability is the permanently underestimated and forgotten field in mathematics, or at least that’s my experience from studying engineering. Kalman filters are based in Bayesian filters, and I recommend you to do a quick review on all them, from the mathematics point of view, before getting into robotics. I found a gem for that in the internet, which is this notebook by Roger Labbe. It is really amazing. It meets a perfect balance between theory and examples, and you can execute the code in your notebook and play with it!

After that, the next step is applying the actual Kalman filter to a more “robotic” example. Here, we get back to Cyrill course. It has some code resources for the theoretical course. However, they are programmed in Octave, some sort of open-source matlab. Personally, I’m not very much into Octave… and that’s where this post arised! I have adapted the Octave functions from Cyrill’s course to Python, and I have executed them in a Jupyter notebook for the sake of clarity.

Here, you will find a combination of theory and code. But, as the title states, it is just a notebook. It is meant for someone that is somewhat familiar with Kalman filters, and is more a review notebook to refresh concepts than a studying notebook. It doesn’t contain all the code, neither all the theory. The Jupyter notebook is submitted on my github for you to execute. And if you need to clarify better the theory concepts, I recommend you to go to any of the previous sources I mentioned. In this post, only the most important functions and concepts are included.

First, I will briefly mention the Kalman filter, which is the most basic algorithm and that nowadays is not deployed into any real robot, but still very convenient to study as a basis to the subsequent algorithms. And then, we will get hands-on into the Extended Kalman Filter, one of the most (if not the most) popular versions of the Kalman filter.

The Kalman Filter

This filter is the optimal estimator for linear functions and Gaussian distributions. The Kalman filter can be used as a solution to the online SLAM problem, defined as follows:

Given:

  • The robot’s controls u_{1:T} = {u_1,u_2,u_3...,u_T}
  • Observations $z_{1:T} = {z_1,z_2,z_3...,z_T}$

Wanted:

  • Map of the environment m
  • Path of the robot $x_{0:T} = {x_0,x_1,x_2...,x_T}$

We will make a probabilistic estimation of the robot’s path and the map according to the motion and observation models.

Motion Model

The motion model describes the relative motion of the robot. Some examples of motion model are the Odometry-based model (wheeled robots) or the velocity-based model (flying robots).

The motion model is represented by matrices and thus is a linear function.

  • $A_t (n\times n)$ maps the state at $t$, given the previous state at $t-1$.
  • $B_t (n\times l)$ describes state change from $t-1$ to $t$, given control command.

Observation Model

The observation or sensor model relates measurements with the robot’s pose: what I am going to observe given that I know the pose or the map. The Beam-Endpoint model is characterized by a gaussian blur around the obstables. The Ray-cast model experiments an exponential decay and covers dynamic obstacles.

The linear mapping between the state and the observation space:

  • $C_t (k\times n)$ maps state $x_t$ to an observation $z_t$.

The Kalman filter is a recursive algorithm and it is computed as follows:

Where $Q_t$ and $R_t$ describe measurement and motion noise, \mathbf{\mu}_{t} is the mean, $\textstyle\sum_t$ is the covariance (uncertainty), $K_t$ is the Kalman gain. The Kalman gain computes how certain I am about the prediction with respect to the motion.

We introduce to the algorithm our current estimate of where we have been in tearms of mean estimate and covariance matrix, as well as the new control command and the observations. We want to update the mean and the covariance matrix so we transit from t-1 to t. We are computing a weighted sum with the Kalman gain weighting the prediction and the correction.

The Extended Kalman Filter

However, in most real scenarios we do not have linear functions to describe the movements or the sensor model. Non-linear functions lead to non-Gaussian distributions and we cannot use the Kalman filter anymore. This is resolved by the Extended Kalman Filter by local linearization. However, we’re still assuming Gaussian noise and Gaussian uncertainties.

The algorithm for the Extended Kalman Filter is:

The terms highlighted in blue are those in which the EKF perform the local linearization. Hereafter, I will introduce how these terms are computed.

Using the EKF to solve the SLAM problem

First we need to define our state vector, and the assumptions for the example that we are using in this notebook:

  • Assumption: known correspondences. That is, when I get an observation, I know which landmark it is in my map.
  • State space (for 2D plane) contains the robot pose (3 dimensions) and the landmark locations (2 dimensions each), and it’s defined as:

  • State representation (very compactly, with $x_R\rightarrow x)$ ):

With $\sum_{xx}$ representing the uncertainty around the pose, $\sum_{mm}$ the uncertainty about the landmark location, and $\sum_{xm}$ the link between the landmark locations and the position of the robot within the platform.

Prediction step: defining g

In this step we only update the pose of the robot $x,y,\theta$ and its covariance $\sum_{xx}$ according to the motion model $g$. Note that since we are just considering the robot’s movement, without taking into account the sensor’s measurements yet, the landmarks locations are not updated in this step.

The motion model

Here the motion model considered is the Odometry Model:

  • The robot moves from xytheta to xythetaprima
  • We have odometry information u

Thus, our odometry motion model (without the noise model) that we will use to update the robot pose in the state vector $\mu$ is:

Next, we update the elements in the covariance matrix associated to the robot pose by performing a local linearization of the function. This is achieved with the partial derivatives of the previous functions, that is, with the Jacobian matrix $G$.

Remember that we’re only updating the robot values in the state space. That is, our Jacobian will have the structure:

def prediction_step( mu, sigma, u):
    ''' Updates the belief concerning the robot pose according to the motion model,
    args:
    - mu :: 2N+3 x 1 vector representing the state mean.
    - sigma :: 2N+3 x 2N+3 covariance matrix.
    - u: odometry reading (r1, t, r2).    

    '''
    m,n = sigma.shape

    # Compute new mu based on the noise-free (odometry-based) motion model
    mu[0,0] = mu[0,0] + data['t'][0] * np.cos(mu[2,0] + data['r1'][0]) # x
    mu[1,0] = mu[1,0] + data['t'][0] * np.sin(mu[2,0] + data['r1'][0]) # y
    mu[2,0] = mu[2,0] + data['r1'][0] + data['r2'][0] # y
    mu[2,0] = normalize_angle(mu[2,0])

    # Compute the 3x3 Jacobian Gx of the motion model
    Gx = np.identity(3)
    Gx[0,2] = - data['t'][0] * np.sin(mu[2,0] + data['r1'][0])
    Gx[1,2] =   data['t'][0] * np.cos(mu[2,0] + data['r1'][0])

    # Construct the full Jacobian G
    G = np.concatenate((Gx,np.zeros((3,n-3))),axis = 1)
    aux = np.concatenate((np.zeros((n-3,3)), np.identity(n-3)),axis = 1)
    G = np.concatenate((G,aux), axis = 0)

    # Motion noise R
    motionNoise = 0.1
    R3 = np.identity(3)*motionNoise
    R3[2,2] = motionNoise/10

    R = np.zeros((sigma.shape))

    R[0:3, 0:3] = R3

    # Compute predicted sigma
    sigma = np.matmul(G,np.matmul(sigma,G.T)) +R # i.e.  sigma = G*sigma*G.T + R


    return mu,sigma

Correction step

Once the pose has been update, it’s time for the correction.

  • Predicted measurement $h(x)$ : we need to predict what the robot sees. For that, we take the current position of the robot, and the position of the landmarks in the map, and with that we compute our predicted measurement.
  • Obtained measurement $z$: we take the real observations of the landmarks according to the sensor.
  • Data association: we compute the discrepancy between $h(x)$ and $z$.
  • Update step: finally, all the elements in $\mu$ and $\sum$ are updated
def correction_step(mu,sigma,z,observedLandmarks):
    ''' Updates the belief, i. e., mu and sigma after observing landmarks, according to the sensor model.
        The employed sensor model measures the range and bearing of a landmark.
        mu: 2N+3 x 1 vector representing the state mean.
        The first 3 components of mu correspond to the current estimate of the robot pose [x; y; theta]
        The current pose estimate of the landmark with id = j is: [mu(2*j+2); mu(2*j+3)]
        sigma: 2N+3 x 2N+3 is the covariance matrix
        z: struct array containing the landmark observations.
        Each observation z(i) has an id z(i).id, a range z(i).range, and a bearing z(i).bearing
        The vector observedLandmarks indicates which landmarks have been observed at some point by the robot.
        observedLandmarks(j) is false if the landmark with id = j has never been observed before.
    '''
    # Number of measurements in this time step
    m = z.shape[0]
    # Number of dimensions to mu
    dim = mu.shape[0]
    # Z: vectorized form of all measurements made in this time step: [range_1; bearing_1; range_2; bearing_2; ...; range_m; bearing_m]
    # ExpectedZ: vectorized form of all expected measurements in the same form.
    # They are initialized here and should be filled out in the for loop below
    Z = np.zeros([m*2, 1],float)
    expectedZ = np.zeros([m*2, 1],float)

    # Iterate over the measurements and compute the H matrix
    # (stacked Jacobian blocks of the measurement function)
    # H will be 2m x 2N+3
    H = []

    j = 0
    for i,row in z.iterrows():
        # Get the id of the landmark corresponding to the i-th observation
        landmarkId = int(row['r1']) # r1 == ID here
        #landmarkId = landmarkId -1 # adapt the 1-9 range to 0-8 range of the array

        # If the landmark is obeserved for the first time:
        if (observedLandmarks[0][landmarkId-1] == 0):
            # Initialize its pose in mu based on the measurement and the current robot pose:
            a = float(row['t']*np.cos(row['r2']+mu[2]))
            b = float(row['t']*np.sin(row['r2']+mu[2]))
            mu[2*landmarkId+1 : 2*landmarkId+3] = mu[0:2] + np.array([[a], [b]])

            # Indicate in the observedLandmarks vector that this landmark has been observed
            observedLandmarks[0][landmarkId-1] = 1  

        # Add the landmark measurement to the Z vector   
        Z[2*j] = row['t']
        Z[2*j+1] = row['r2']

        # Use the current estimate of the landmark pose
        # to compute the corresponding expected measurement in expectedZ:
        delta = mu[2 * landmarkId + 1 : 2 * landmarkId + 2+1] - mu[0:2]
        q = np.matmul(delta.T,delta)

        expectedZ[2*j] = math.sqrt(q)
        expectedZ[2*j+1] = normalize_angle(np.arctan2(delta[1],delta[0]) - mu[2])      

        delta0 = float(delta[0])
        delta1 = float(delta[1])

        # Compute the Jacobian Hi of the measurement function h for this observation
        Hi = 1/q * np.array([[float(-math.sqrt(q)*delta0),-math.sqrt(q)*delta1, 0, math.sqrt(q)*delta0, math.sqrt(q)*delta1], [ delta1, -delta0, float(-q), -delta1, delta0] ])

        # Map Jacobian Hi to high dimensional space by a mapping matrix Fxj
        Fxj = np.zeros([5,dim])
        Fxj[0:3,0:3] = np.identity(3)
        Fxj[3,2*landmarkId+1] = 1
        Fxj[4,2*landmarkId+2] = 1

        Hi = np.matmul(Hi, Fxj)


        # Augment H with the new Hi     

        H.append(Hi)

        j+=1



    # Construct the sensor noise matrix Q
    Q = 0.01*np.identity(2*m)

    # Compute the Kalman gain
    # K = sigma * H.T * inv(H * sigma * H.T + Q)
    Hnp = np.asarray(H).reshape((2*m,dim))
    sigmaxHt = np.matmul(sigma,Hnp.T)
    inverse = np.linalg.inv(np.matmul(Hnp,sigmaxHt)+Q)    
    K = np.matmul(sigmaxHt,inverse)

    # Compute the difference between the expected and recorded measurements.
    # Remember to normalize the bearings after subtracting!

    diffZ = normalize_all_bearings(Z-expectedZ)

    # Finish the correction step by computing the new mu and sigma.
    # Normalize theta in the robot pose.
    mu = mu + np.matmul(K, diffZ)
    # sigma = (eye(dim) - K * H) * sigma
    sigma = np.matmul((np.identity(dim) - np.matmul(K, Hnp)), sigma)

    return mu, sigma, observedLandmarks

Initializing the belief

Given that we know the number of landmarks in our map, we can define the shape of the mean and the covariance matrix:

  • mu: 2N+3x1 vector representing the mean of the normal distribution. The first 3 components of mu correspond to the pose of the robot, and the landmark poses (xi, yi) are stacked in ascending id order.

  • sigma: (2N+3)x(2N+3) covariance matrix of the normal distribution.

Everything is completely unknown in the beginnig, so we define the starting point as our coordinate system. That is, $\mu = \textbf{0}$. Since we are completely certain about that (because we’ve defined it), the corresponding values in sigma are also zero $\sum_{xx} = \textbf{0}$. However, we don’t know anything about the landmarks, because we haven’t seen anything yet, so they have an infinite uncertainty.

# Initialize mu
mu = np.zeros((2*N+3,1),dtype = 'float')
# Initialize sigma
robSigma = np.zeros((3,3))
robMapSigma = np.zeros((3,2*N))
mapSigma = np.identity((2*N))*1000 # 1000 as a "infinite" or just "high" value

aux1 = np.concatenate((robSigma,robMapSigma),axis = 1)
aux2 = np.concatenate((robMapSigma.T,mapSigma), axis = 1)

sigma = np.concatenate((aux1,aux2),axis = 0)

The sigma values corresponding to the robot pose, as mentioned, have 0. value (no uncertainty)

sigma[0:3, 0:3]

array([[0., 0., 0.],
       [0., 0., 0.],
       [0., 0., 0.]])

However, the covariance associated to the landmark pose has infinite value(here 1000, just a high value), for instance, for the first landmark:

sigma[3,3]

1000.0

The EKF loop

Up to this point we have everything ready to execute the Extended Kalman Filter loop in our 2D environment and see what happens. Each element in the map is displayed altogether with the uncertainty of the measurement, plotted as an ellipse. You can see in the video, that the more a landmark is seen by the robot, the smaller the ellipse gets (and thus, the uncertainty).

def ekf_loop(mu,sigma,observedLandmarks,fig,ax,camera):

    for t in range (0,timestepindex[-1]):
        # Perform the prediction step of the EKF
        mu,sigma = prediction_step(mu, sigma, data.loc[(data['timestep'] == t) & (data['sensor'] == 'ODOMETRY')])


        #Perform the correction step of the EKF
        mu, sigma, observedLandmarks = correction_step(mu,sigma,data.loc[(data['timestep'] == t) & (data['sensor'] == 'SENSOR')],observedLandmarks)
        # Generate visualization plots
        fig = plot_state(mu,sigma,world_ldmrks,t,observedLandmarks,data.loc[(data['timestep'] == t) & (data['sensor'] == 'SENSOR')],fig,ax)
        camera.snap()
    return camera, mu[0:3],sigma[0:3,0:3]


observedLandmarks = np.zeros((1,N))
robSigma = np.zeros((3,3))
robMapSigma = np.zeros((3,2*N))
mapSigma = np.identity((2*N))*1000 # 1000 as a "infinite" or just "high" value
aux1 = np.concatenate((robSigma,robMapSigma),axis = 1)
aux2 = np.concatenate((robMapSigma.T,mapSigma), axis = 1)
sigma = np.concatenate((aux1,aux2),axis = 0)
mu = np.zeros((2*9+3,1),dtype = 'float')

fig,ax = plt.subplots()
camera = Camera(fig)

camera = ekf_loop(mu,sigma,observedLandmarks,fig,ax,camera)[0]

animation = camera.animate()
HTML(animation.to_html5_video())

png

]]>