Certifiable Tracking (MATLAB Implementation)

This is the repository for the paper "A Certifiable Algorithm for Simultaneous Shape Estimation and Object Tracking" by Lorenzo Shaikewitz, Samuel Ubellacker, and Luca Carlone.

Please reference the RAL paper (arXiv version) and Video.

Setup

To run the solver, this repository has the following dependencies:

• CertifiablyRobustPerception (you need to run SPOTLESS install script)
• MOSEK
• SDPNAL+

For outlier rejection, we use the COPT solver:

• COPT (we also provide a cvx version that works with MOSEK, it is just slower)

Reproducing results also requires the following datasets:

• PASCAL3D+
• YCBInEOAT

Reproducing ablations requires the following dependency:

• ROBIN (for OURS+ROBIN; be sure turn OFF the unit tests in cmakelists.txt)

You may set the paths to these dependencies in setup.m. I recommend cloning this repository and these dependencies in the same parent folder.

Comments on Python

MATLAB's python integration isn't the easiest to debug. If you run into any python errors, first make sure you have all the dependencies installed.

Running MATLAB

On my machine (Ubuntu 22.04) I needed to set the LD_PRELOAD environment variable. To do this, launch MATLAB with the following line:

LD_PRELOAD=/lib/x86_64-linux-gnu/libstdc++.so.6 matlab

This appears to be necessary to run ROBIN.

Quick Start

Each time you start MATLAB you must run setup.m to add the necessary paths. The setup file also allows you to select which experiment/dataset you wish to work with. Options are the subfolders within experiments.

Suppose you wish to run the experiments that use synthetic data. First run setup with experiments="synthetic". Then, from the home directory run tracking_outlier_free.

Reproduce Experiments

All experiment data is linked in this Google Drive folder.

Synthetic Experiments

Please download the PASCAL3D+ dataset (3D models in mat form only). Update the cadpath path in setup.m and run setup.m with experiments="pascal". The experiments may be run via the following scripts:

• EKF_Landm runs the measurement noise experiment.
• EKF_Landp runs the process noise experiment.
• EKF_outlier_ablation runs the outlier ratio experiment.
• EKF_EKF runs the ground truth EKF comparison.

Note that your results may not exactly match the results from our paper due to the use of random numbers. MAT archives used to generate the data in our paper are available on Google Drive.

YCBInEOAT Experiments

We do not provide the keypoint detector open source. It is trained on BOP YCBV synthetic data. Download the processed YCBInEOAT data and update the path jsondir in ycbineoat.m to the folder. Similarly, update the paths under % get CAD keypoints if there to the shapes directory within the folder you downloaded. Lastly, update the path models_dir in visualize2.m to the models3 directory within the folder you downloaded. Run setup.m with experiments="ycbineoat2".

YCBInEOAT experiments may now be replicated using run_ycbineoat.m. Simply set the videoNumber (index of videos array) to select the video. To run with ground truth pixel keypoints, set params.gt = true. To view results, use visualize2.m. Set the video number as you did in run_ycbineoat.

To avoid rerunning CAST# and ablations, move the data from YCBInEOAT/mat_archives into this repositority's directory and run visualize2.m. This is the data used to compute ADD and ADD-S scores in the text.

For for advanced visualization, you will need the YCBInEOAT data. We do not release the code used to generate videos here.

Drone Experiment

Please download the racecar data from Google Drive. Update the cadpath path in setup.m and run setup.m with experiments="racecar_offline". For CAST# data use tracking_frame_lorenzo, and for PACE use tracking_pace. Update the paths problem.json to the file from Google Drive and problem.savefile as desired.

We also provide mat archives of each run in Google Drive. To visualize these, load the data and skip the solve for each batch section of the script.

Notes on Organization

General purpose scripts are in the visualization, utils, solvers, and outlier_rejection folders. Within the experiments folder are scripts specialized to individual experiments. In general, scripts that begin with "tracking" are single-batch test scripts, while scripts that begin with "EXP" are full experiments.

Bibtex

@misc{Shaikewitz24-trackingCAST,
      author={Shaikewitz, Lorenzo and Ubellacker, Samuel and Carlone, Luca},
      journal={IEEE Robotics and Automation Letters},
      title={A Certifiable Algorithm for Simultaneous Shape Estimation and Object Tracking},
      year={2024},
      volume={9},
      number={12},
      pages={11873-11880},
      keywords={Shape;Estimation;Three-dimensional displays;Solid modeling;Active shape model;Shape measurement;Noise;Libraries;Computational modeling;Target tracking;active shape model;certifiable optimization;pose estimation;robot sensing systems;RGB-D perception;visual tracking;shape estimation},
      doi={10.1109/LRA.2024.3501684}}
}