{"id":24539701,"url":"https://github.com/stephlin/lio-segmot","last_synced_at":"2025-04-07T11:11:35.176Z","repository":{"id":87664245,"uuid":"419640786","full_name":"StephLin/LIO-SEGMOT","owner":"StephLin","description":"LiDAR-Inertial Odometry via Simultaneous Ego-motion Estimation and Multiple Object Tracking (ICRA 2023)","archived":false,"fork":false,"pushed_at":"2024-03-02T08:19:34.000Z","size":141108,"stargazers_count":237,"open_issues_count":0,"forks_count":26,"subscribers_count":5,"default_branch":"master","last_synced_at":"2025-03-31T10:04:54.826Z","etag":null,"topics":[],"latest_commit_sha":null,"homepage":"https://stephlin.github.io/projects/2023-LIO-SEGMOT.html","language":"C++","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"bsd-3-clause","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/StephLin.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE","code_of_conduct":null,"threat_model":null,"audit":null,"citation":"CITATION.bib","codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null}},"created_at":"2021-10-21T08:25:43.000Z","updated_at":"2025-03-11T06:54:33.000Z","dependencies_parsed_at":"2024-03-11T16:46:14.320Z","dependency_job_id":null,"html_url":"https://github.com/StephLin/LIO-SEGMOT","commit_stats":null,"previous_names":[],"tags_count":0,"template":false,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/StephLin%2FLIO-SEGMOT","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/StephLin%2FLIO-SEGMOT/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/StephLin%2FLIO-SEGMOT/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/StephLin%2FLIO-SEGMOT/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/StephLin","download_url":"https://codeload.github.com/StephLin/LIO-SEGMOT/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":247640465,"owners_count":20971557,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":[],"created_at":"2025-01-22T17:15:46.814Z","updated_at":"2025-04-07T11:11:35.138Z","avatar_url":"https://github.com/StephLin.png","language":"C++","funding_links":[],"categories":[],"sub_categories":[],"readme":"# LIO-SEGMOT\n\nThe official implementation of LIO-SEGMOT (**L**iDAR-**I**nertial **O**dometry\nvia **S**imultaneous **Eg**o-motion Estimation and **M**ultiple **O**bject\n**T**racking), an optimization-based odometry approach targeted for dynamic\nenvironments. LIO-SEGMOT can provide continuous object tracking results while\npreserving the keyframe selection mechanism in the odometry system. This work is\naccepted for publication in [ICRA 2023](https://www.icra2023.org/).\n\n![TEASER_GIF](./docs/_static/images/LIO-SEGMOT_KITTI_teaser.GIF)\n\nYou can check out [our video](https://youtu.be/5HtnDFPerVo) to understand the\nmain idea of LIO-SEGMOT. For more, please refer to our paper:\n\n- Yu-Kai Lin, Wen-Chieh Lin, Chieh-Chih Wang, **Asynchronous State Estimation of Simultaneous Ego-motion Estimation and Multiple Object Tracking for LiDAR-Inertial Odometry**. _2023 International Conference on Robotics and Automation (ICRA)_, pp. 10616--10622, May 2023. ([paper](https://doi.org/10.1109/ICRA48891.2023.10161269)) ([preprint](https://gpl.cs.nctu.edu.tw/Steve-Lin/LIO-SEGMOT/preprint.pdf)) ([code](https://github.com/StephLin/LIO-SEGMOT)) ([video](https://youtu.be/5HtnDFPerVo))\n\nIf you use this project in your research, please cite:\n\n```bibtex\n@article{lin2023lio-segmot,\n  title={Asynchronous State Estimation of Simultaneous Ego-motion Estimation and Multiple Object Tracking for LiDAR-Inertial Odometry},\n  author={Lin, Yu-Kai and Lin, Wen-Chieh and Wang, Chieh-Chih},\n  booktitle = {2023 International Conference on Robotics and Automation, {ICRA} 2023,\n               London, UK, May 2023},\n  pages     = {10616--10622},\n  year      = {2023},\n}\n```\n\n- [:newspaper: Poster](#newspaper-poster)\n- [:gear: Installation](#gear-installation)\n  - [Step 1. Preparing the Dependencies](#step-1-preparing-the-dependencies)\n  - [Step 2. Building the LIO-SEGMOT Project](#step-2-building-the-lio-segmot-project)\n  - [Step 3. Preparing Object Detection Services](#step-3-preparing-object-detection-services)\n- [:card_file_box: Sample Datasets](#card_file_box-sample-datasets)\n- [:running_man: Run](#running_man-run)\n- [:wheelchair: Services of LIO-SEGMOT](#wheelchair-services-of-lio-segmot)\n  - [`/lio_segmot/save_map`](#lio_segmotsave_map)\n  - [`/lio_segmot/save_estimation_result`](#lio_segmotsave_estimation_result)\n- [:memo: Remarks](#memo-remarks)\n  - [Hyperparameters](#hyperparameters)\n    - [Hierarchical Criterion](#hierarchical-criterion)\n    - [Factor Graph Optimization](#factor-graph-optimization)\n    - [High-speed Moving Object Supports in Early Steps](#high-speed-moving-object-supports-in-early-steps)\n    - [Lifecycle Management of Tracking Objects](#lifecycle-management-of-tracking-objects)\n  - [Limitations of LIO-SEGMOT](#limitations-of-lio-segmot)\n  - [Possible Future Research Directions](#possible-future-research-directions)\n- [:gift: Acknowledgement](#gift-acknowledgement)\n\n## :newspaper: Poster\n\n![Poster](./docs/_static/images/poster.png)\n\n## :gear: Installation\n\nThe project is originally developed in **Ubuntu 18.04**, and the following\ninstruction supposes that you are using Ubuntu 18.04 as well. I am not sure if\nit also works with other Ubuntu versions or other Linux distributions, but maybe\nyou can give it a try :+1:\n\nAlso, please feel free to open an issue if you encounter any problems of the\nfollowing instruction.\n\n### Step 1. Preparing the Dependencies\n\nPlease prepare the following packages or libraries used in LIO-SEGMOT:\n\n1. [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) (w/ Desktop-Full\n   Install) and the following dependencies:\n   ```bash\n   #!/bin/bash\n   sudo apt update\n   sudo apt install -y \"ros-${ROS_DISTRO}-navigation\" \\\n                       \"ros-${ROS_DISTRO}-robot-localization\" \\\n                       \"ros-${ROS_DISTRO}-robot-state-publisher\" \\\n                       \"ros-${ROS_DISTRO}-jsk-recognition-msgs\" \\\n                       \"ros-${ROS_DISTRO}-jsk-rviz-plugins\"\n   ```\n2. [gtsam 4.0.3](https://github.com/borglab/gtsam/tree/4.0.3)\n   ```bash\n   #!/bin/bash\n   cd ~\n   git clone -b 4.0.3 https://github.com/borglab/gtsam \u0026\u0026 cd gtsam\n   mkdir build \u0026\u0026 cd build\n   cmake ..\n   sudo make install\n   ```\n\n### Step 2. Building the LIO-SEGMOT Project\n\nYou can use the following command to build the project:\n\n```bash\n#!/bin/bash\nsource \"/opt/ros/${ROS_DISTRO}/setup.bash\"\nmkdir -p ~/catkin_ws/src\ncd ~/catkin_ws/src\ngit clone git@github.com:StephLin/LIO-SEGMOT.git\ncd ..\ncatkin_make\n```\n\n### Step 3. Preparing Object Detection Services\n\nWe provide two object detection services for LIO-SEGMOT:\n\n- [StephLin/SE-SSD-ROS (Apache-2.0 license)](https://github.com/StephLin/SE-SSD-ROS)\n  (based on [Vegeta2020/SE-SSD](https://github.com/Vegeta2020/SE-SSD))\n- [StephLin/livox_detection_lio_segmot (GPL-3.0 license)](https://github.com/StephLin/livox_detection_lio_segmot)\n  (based on [Livox-SDK/livox_detection](https://github.com/Livox-SDK/livox_detection))\n\nPlease refer to their installation instructions accordingly.\n\n## :card_file_box: Sample Datasets\n\nWe provide the following pre-built bag files for KITTI raw sequences and the Hsinchu\ndataset (GuangfuRoad sequence):\n\n| Dataset | Sequence    | Bag File                                             | Ground Truth Trajectory                              |\n| ------- | ----------- | ---------------------------------------------------- | ---------------------------------------------------- |\n| KITTI   | 0926-0009   | [bag](http://140.113.150.180:5000/sharing/BrHtqyElq) | [tum](http://140.113.150.180:5000/sharing/HGebyDSCR) |\n| KITTI   | 0926-0013   | [bag](http://140.113.150.180:5000/sharing/AQQa3kMSH) | [tum](http://140.113.150.180:5000/sharing/gREpr4xdI) |\n| KITTI   | 0926-0014   | [bag](http://140.113.150.180:5000/sharing/HOgV5T79H) | [tum](http://140.113.150.180:5000/sharing/POuFRJBQI) |\n| KITTI   | 0926-0015   | [bag](http://140.113.150.180:5000/sharing/XnoNLKSUQ) | [tum](http://140.113.150.180:5000/sharing/RBw1BeftU) |\n| KITTI   | 0926-0032   | [bag](http://140.113.150.180:5000/sharing/ikbtkpWve) | [tum](http://140.113.150.180:5000/sharing/aQdaEnVjc) |\n| KITTI   | 0926-0051   | [bag](http://140.113.150.180:5000/sharing/N1o9NcgU4) | [tum](http://140.113.150.180:5000/sharing/Wzu8QWoEC) |\n| KITTI   | 0926-0101   | [bag](http://140.113.150.180:5000/sharing/lhhohwfJT) | [tum](http://140.113.150.180:5000/sharing/JCOaJHw04) |\n| Hsinchu | GuangfuRoad | [bag](http://gofile.me/56KxB/mz6HOYOMG)              | [tum](http://gofile.me/56KxB/t5smaOAZk)              |\n\nIf you cannot download sample datasets from above links, please refer to\n[this alternative link (Google Drive)](https://drive.google.com/drive/folders/17QwfTMCv-2XdgOLxGdrnCwybcSYQhs7S?usp=drive_link).\n\nGround truth robot trajectories (based on GPS data provided by KITTI) are stored\nas [the TUM format](https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats#ground-truth_trajectories).\nEach row has 8 components containing timestamps (sec), xyz-position (meter), and\nxyzw-orientation (quaternion):\n\n```\ntimestamp x y z qx qy qz qw\n```\n\n## :running_man: Run\n\nPlease follow the steps to execute LIO-SEGMOT properly:\n\n1. (Optional) Launch the ROS core:\n\n   ```bash\n   roscore\n   ```\n\n2. Launch the core LIO-SEGMOT service:\n\n   ```bash\n   #!/bin/bash\n   # Please select one of the following configs to launch the service properly:\n   # 1. With KITTI configuration\n   roslaunch lio_segmot run_kitti.launch\n\n   # 2. With Hsinchu configuration\n   roslaunch lio_segmot run_hsinchu.launch\n\n   # 3. Undefined (same as KITTI configuration)\n   roslaunch lio_segmot run.launch\n   ```\n\n3. Launch the selected object detection service:\n\n   ```bash\n   #!/bin/bash\n   # SE-SSD-ROS \u0026 livox_detection_lio_segmot\n   # Please check their documentation to see how they are launched\n   ```\n\n4. Start the customized ROS bag player:\n\n   ```bash\n   #!/bin/bash\n   rosrun lio_segmot lio_segmot_offlineBagPlayer _bag_filename:=\"path/to/your/sequence.bag\"\n   ```\n\n   The default registered LiDAR and IMU topics are `/points_raw` and `/imu_raw`,\n   respectively. If you want to register other LiDAR/IMU topics, please add\n   additional options `_lidar_topic` and `_imu_topic`. For example, if you are\n   using the GuangfuRoad sequence (`/velodyne_points` and `/imu/data` for LiDAR\n   and IMU topics, respectively):\n\n   ```bash\n   rosrun lio_segmot lio_segmot_offlineBagPlayer _bag_filename:=\"GuangfuRoad-06-13.bag\" \\\n                                                 _lidar_topic:=\"/velodyne_points\" \\\n                                                 _imu_topic:=\"/imu/data\"\n   ```\n\n## :wheelchair: Services of LIO-SEGMOT\n\n### `/lio_segmot/save_map`\n\n```txt\nUsage: rosservice call /lio_segmot/save_map [RESOLUTION] [OUTPUT_DIR]\nExample: rosservice call /lio_segmot/save_map 0.2 /path/to/a/directory/\n```\n\nThis service saves LiDAR map to the local machine.\n\n### `/lio_segmot/save_estimation_result`\n\n```txt\nUsage: rosservice call /lio_segmot/save_estimation_result\n```\n\nThis service outputs current estimation results including\n\n- `nav_msgs::Path robotTrajectory`: The robot trajectory\n- $\\color{gray}\\textsf{(INTERNAL USE)}$ `nav_msgs::Path[] objectTrajectories`: Trajectories for each object (indexed by the factor graph)\n- $\\color{gray}\\textsf{(INTERNAL USE)}$ `nav_msgs::Path[] objectVelocities`: Linear and angular velocities for each object (indexed by the factor graph)\n- `nav_msgs::Path[] trackingObjectTrajectories`: Trajectories for each object (indexed by LIO-SEGMOT)\n- `nav_msgs::Path[] trackingObjectVelocities`: Linear and angular velocities for each object (indexed by LIO-SEGMOT)\n- `lio_segmot::ObjectStateArray[] trackingObjectStates`: States for each object during its lifetime (indexed by LIO-SEGMOT)\n- $\\color{gray}\\textsf{(INTERNAL USE)}$ `lio_segmot::flags[] objectFlags`: Flags for each object during its lifetime (indexed by the factor graph)\n- `lio_segmot::flags[] trackingObjectFlags`: Flags for each object during its lifetime (indexed by LIO-SEGMOT)\n\nin which custom types `lio_segmot::ObjectStateArray` (underlying `lio_segmot::ObjectState`) and `lio_segmot::flags` are given by\n\n- `lio_segmot::ObjectStateArray`\n\n  ```cpp\n  Header header\n  lio_segmot::ObjectState[] objects\n  ```\n\n- `lio_segmot::ObjectState`\n\n  ```cpp\n  Header header\n\n  // The corresponding detection (measurement)\n  jsk_recognition_msgs::BoundingBox detection\n\n  // States of object pose and velocity in the factor graph\n  geometry_msgs::Pose pose\n  geometry_msgs::Pose velocity\n\n  // Residual and innovation of the tightly-coupled detection factor\n  bool hasTightlyCoupledDetectionError\n  float64 tightlyCoupledDetectionError         // Residual\n  float64 initialTightlyCoupledDetectionError  // Innovation\n\n  // Residual and innovation of the loosely-coupled detection factor\n  bool hasLooselyCoupledDetectionError\n  float64 looselyCoupledDetectionError         // Residual\n  float64 initialLooselyCoupledDetectionError  // Innovation\n\n  // Residual and innovation of the smooth movement factor\n  bool hasMotionError\n  float64 motionError         // Residual\n  float64 initialMotionError  // Innovation\n\n  int32 index            // Object index\n  int32 lostCount        // Counter of losing detections\n  float64 confidence     // Detection's confidence score (given by detection methods)\n  bool isTightlyCoupled  // Is the object tightly-coupled at this moment?\n  bool isFirst           // Is the object just initialized at this moment?\n  ```\n\n- `lio_segmot/flags`\n\n  ```cpp\n  // Flags of the object in its lifetime\n  int32[] flags  // 1: the object is tightly-coupled\n                 // 0: the object is loosely-coupled\n  ```\n\n## :memo: Remarks\n\n### Hyperparameters\n\nThere are various covariance matrices designed for the hirarchical criterion\n(innovation filtering) and factor graph optimization (increments of LIO-SEGMOT\nw.r.t. LIO-SAM) in LIO-SEGMOT. This section is going to explain them.\n\n\u003e All covariance matrices in settings are expressed as diagonal vectors.\n\nTaking [the KITTI configuration](./config/params_kitti.yaml) for example, we have the following settings:\n\n#### Hierarchical Criterion\n\nThis section collects settings for the hierarchical criterion. It can be viewed\nas a kind of innovation filtering. In brief, the criterion is designed to\nprogressively make the following decisions when a new detection\n$\\boldsymbol{z}\\in SE(3)$ is coming into the system:\n\n| ID       | Description                                                                 |\n| -------- | --------------------------------------------------------------------------- |\n| **(Q1)** | Does the detection belong to any existing object $\\boldsymbol{x}_{t,i}$?    |\n| **(Q2)** | If Q1 holds, does $\\boldsymbol{z}$ follows the $i$-th object's motion?      |\n| **(Q3)** | If Q1 and Q2 holds, should the tightly-coupled detection factor be applied? |\n\nThe first two questions **(Q1)** and **(Q2)** are determined by using the\nMahalanobis distance of the error vector,\n\n$$\n\\Big\\Vert\\text{ detection error of }\\boldsymbol{z}\\text{ and the }i\\text{-th object } \\boldsymbol{x}_{t,i} \\text{ }\\Big\\Vert_{\\Sigma}\n\\leq \\varepsilon,\n$$\n\nwith given covariance with given covariance matrices\n$\\Sigma\\in\\{\\Sigma_ {\\text{Q}_ 1},\\Sigma_ {\\text{Q}_ 2}\\}\\subsetneq\\mathbb{R}^{6\\times 6}$\nand a threshold $\\varepsilon\u003e0$. We assume that\n$\\Sigma_ {\\text{Q}_ 2}-\\Sigma_ {\\text{Q}_ 1}$\nis positive semidefinite (PSD), i.e.,\n$\\Sigma_ {\\text{Q}_ 2}-\\Sigma_{\\text{Q}_ 1} \\succeq 0$,\nto prevent ambiguity of the hierarchical criterion that **(Q2)** holds but\n**(Q1)** does not hold.\n\nTwo spatial information-based tests are conducted to determine **(Q3)**, which\nare the detection constraint and the velocity constraint:\n\n- **(Detection Constraint)** The above equation holds with another given\n  covariance matrix\n  $\\Sigma_ {\\text{Q}_ {3,1}}$\n  that satisfies\n  $\\Sigma_ {\\text{Q}_ {3,1}}-\\Sigma_ {\\text{Q}_ {2}} \\succeq 0$.\n\n- **(Velocity constraint)** The variance of velocities in previous steps is\n  small enough. That is,\n\n  $$\\frac{1}{N}\\sum_ {s=1}^{N} \\Big\\Vert \\text{Log}(\\boldsymbol{v}_ {t-s,i}) - \\text{Log}(\\bar{\\boldsymbol{v}}_ {t,i}) \\Big\\Vert_ {\\Sigma_{Q_ {3,2}}}^2 \\leq \\varepsilon$$\n\n  with a given covariance matrix $\\Sigma_{Q_{3,2}}$, where $N$ is the fixed\n  number of previous velocities of object states and\n  $\\bar{\\boldsymbol{v}}_{t,i}\\in SE(3)$ is the mean of the $N$ previous\n  velocities.\n\nIf **(Q1)** holds for the detection $\\boldsymbol{z}$ and the corresponding\n$i$-th object, the new state of the $i$-th object along with a loosely-coupled\ndetection factor would be added to the factor graph.\n\nFurthermore, if **(Q2)**\nholds, a constant velocity factor and a smooth movement factor would be also\nadded to the factor graph.\n\nFinally, if **(Q3)** holds, the loosely-coupled\ndetection factor would be replaced with a tightly-coupled detection factor. It\nmeans that the $i$-th object are regarded as a reliable object that are suitable\nto refine the odometry.\n\n| Notation                           | Setting                                             | Description                                                                                                                                                                                                                                   | Default Value                                      |\n| ---------------------------------- | --------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------- |\n| $\\varepsilon$                      | `detectionMatchThreshold`                           | The threshold to classify all Mahalanobis distances in the hirarchical criterion (except for the tightly-coupled detection factor).                                                                                                           | 19.5                                               |\n| $\\Sigma_{\\text{Q}_1}$              | `dataAssociationVarianceVector`                     | The covariance matrix to determine if a detection belongs to a given object. This covariance is used to maintain tracking ID.                                                                                                                 | `[3.0e-4, 3.0e-4, 3.0e-4, 5.0e-2, 3.0e-2, 3.0e-2]` |\n| $\\Sigma_{\\text{Q}_2}$              | `looselyCoupledMatchingVarianceVector`              | The covariance matrix to determine if a detection follows the object's motion.                                                                                                                                                                | `[1.0e-4, 1.0e-4, 1.0e-4, 2.0e-3, 2.0e-3, 2.0e-3]` |\n| $\\varepsilon^\\prime$               | `tightCouplingDetectionErrorThreshold`              | The threshold to classify the Mahalanobis distance in the detection constraint of the tightly-coupled detection factor checks.                                                                                                                | 26.0                                               |\n| $\\Sigma_{\\text{Q}_{3,1}}^\\prime$   | `tightlyCoupledMatchingVarianceVector`              | The covariance to determine if a detection satisfies the detection constraint in the tightly-coupled detection factor checks.                                                                                                                 | `[8.0e-6, 8.0e-6, 8.0e-6, 1.0e-4, 1.0e-4, 1.0e-4]` |\n| $N$                                | `numberOfVelocityConsistencySteps`                  | The number of samples used in velocity constraint of the tightly-coupled detection checks.                                                                                                                                                    | 4                                                  |\n| $N^\\prime$                         | `numberOfPreLooseCouplingSteps`                     | The number of steps that objects should only use loosely-coupled detection factors (due to velocity constraint of the tightly-coupled detection checks, see [below](#high-speed-moving-object-supports-in-early-steps) for more information). | 6                                                  |\n| $\\sigma_{\\text{Q}_{3,2}}^\\text{A}$ | `objectAngularVelocityConsistencyVarianceThreshold` | The angular part of the covariance matrix to determine if the object satisfies the velocity constraint in the tightly-coupled detection factor checks.                                                                                        | 1.0e-5                                             |\n| $\\sigma_{\\text{Q}_{3,2}}^\\text{L}$ | `objectLinearVelocityConsistencyVarianceThreshold`  | The linear part of the covariance matrix to determine if the object satisfies the velocity constraint in the tightly-coupled detection factor checks.                                                                                         | 1.0e-2                                             |\n\nFor engineering purposes, we use two thresholds $\\varepsilon$ and\n$\\varepsilon^\\prime$ in the implementation. In addition, we decouple the angular\npart and the linear part of $\\Sigma_{\\text{Q}_{3,2}}$. The following equations\nare shown to coincide with the expression used in our paper:\n\n$$\n\\begin{aligned}\n\\displaystyle\\Sigma_{\\text{Q}_{3,1}} \u0026= \\left(\\frac{\\varepsilon^\\prime}{\\varepsilon}\\right)^2 \\cdot \\displaystyle\\Sigma_{\\text{Q}_{3,1}}^\\prime, \\\\\n\\displaystyle\\Sigma_{\\text{Q}_{3,2}}^\\prime \u0026= \\begin{bmatrix}\\sigma_{\\text{Q}_{3,2}}^\\text{A} \\\\\n\u0026 \\sigma_{\\text{Q}_{3,2}}^\\text{A} \\\\\n\u0026\u0026 \\sigma_{\\text{Q}_{3,2}}^\\text{A} \\\\\n\u0026\u0026\u0026 \\sigma_{\\text{Q}_{3,2}}^\\text{L} \\\\\n\u0026\u0026\u0026\u0026 \\sigma_{\\text{Q}_{3,2}}^\\text{L} \\\\\n\u0026\u0026\u0026\u0026\u0026 \\sigma_{\\text{Q}_{3,2}}^\\text{L}\n\\end{bmatrix}, \\\\\n\\displaystyle\\Sigma_{\\text{Q}_{3,2}} \u0026= \\frac{1}{\\varepsilon^2} \\cdot \\displaystyle\\Sigma_{\\text{Q}_{3,2}}^\\prime.\n\\end{aligned}\n$$\n\n#### Factor Graph Optimization\n\nCovariance matrices used in factor graph optimization. Different from the above\nsection, they are essential to \"balance\" different types of measurements in a\nunified factor graph. Those matrices are relatively rare to be modified.\n\n| Notation             | Setting                                  | Description                                                                                   | Default Value                                      |\n| -------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------- | -------------------------------------------------- |\n| $\\Sigma_{\\text{C}}$  | `constantVelocityDiagonalVarianceVector` | The covariance matrix of constant velocity factors used in factor graph optimization.         | `[2.0e-4, 2.0e-4, 1.0e-3, 2.0e-1, 1.0e-1, 1.0e-1]` |\n| $\\Sigma_{\\text{M}}$  | `motionDiagonalVarianceVector`           | The covariance matrix of smooth movement factors used in factor graph optimization.           | `[2.0e-4, 2.0e-4, 1.0e-3, 1.0e-1, 1.0e-2, 1.0e-2]` |\n| $\\Sigma_{\\text{LC}}$ | `looselyCoupledDetectionVarianceVector`  | The covariance matrix of loosely-coupled detection factors used in factor graph optimization. | `[2.0e-4, 2.0e-4, 2.0e-4, 1.5e-3, 1.5e-3, 1.5e-3]` |\n| $\\Sigma_{\\text{TC}}$ | `tightlyCoupledDetectionVarianceVector`  | The covariance matrix of tightly-coupled detection factors used in factor graph optimization. | `[2.0e-4, 2.0e-4, 2.0e-4, 1.5e-3, 1.5e-3, 1.5e-3]` |\n\n#### High-speed Moving Object Supports in Early Steps\n\nAs all tracking objects' velocities are initialized with zero-speed, it may be\nhard to associate detections in different moments for high-speed moving objects\nin the early stage. To mitigate this issue, a larger covariance matrix for\n**(Q2)** in the hierarchical criterion is used in the first few steps to\naccommodate objects that are moving fast.\n\nSince those steps are used to figure out the inital speed of an object, we do\nnot account them for the velocity constraint in the tightly-coupled detection\nfactor checks. Therefore, we have $N^\\prime = N + N^\\text{E}$.\n\n| Notation                       | Setting                                       | Description                                                                                   | Default Value                                      |\n| ------------------------------ | --------------------------------------------- | --------------------------------------------------------------------------------------------- | -------------------------------------------------- |\n| $N^\\text{E}$                   | `numberOfEarlySteps`                          | Number of the first steps to accommodate high-speed moving objects.                           | 2                                                  |\n| $\\Sigma_{\\text{Q}_2}^\\text{E}$ | `earlyLooselyCoupledMatchingVarianceVector`   | The covariance matrix to determine if a detection follows the object's motion.                | `[3.0e-4, 3.0e-4, 3.0e-4, 5.0e-2, 5.0e-3, 5.0e-3]` |\n| $\\Sigma_{\\text{C}}^\\text{E}$   | `earlyConstantVelocityDiagonalVarianceVector` | The covariance matrix of loosely-coupled detection factors used in factor graph optimization. | `[2.0e-4, 2.0e-4, 1.0e-3, 2.0e-1, 1.0e-1, 1.0e-1]` |\n\n#### Lifecycle Management of Tracking Objects\n\nIn real world applications, it's likely to lose detections of tracking objects\ndue to occlusion or other complicate environment circumstances. To mitigate\nfrequent ID-switching in multiple object tracking due to the above issue, we\nstill track each object for a little while, even though they do not have any\ncorresponding detections.\n\n| Notation     | Setting                      | Description                                                                                | Default Value |\n| ------------ | ---------------------------- | ------------------------------------------------------------------------------------------ | ------------- |\n| $N^\\text{L}$ | `trackingStepsForLostObject` | Number of steps that LIO-SEGMOT still keeps an object of missing detections in the system. | 3             |\n\n### Limitations of LIO-SEGMOT\n\nCurrently, most state-of-the-art object detection approaches (e.g.,\n[SE-SSD](https://arxiv.org/abs/2104.09804),\n[PointPillars](https://arxiv.org/abs/1812.05784),\n[PointRCNN](https://arxiv.org/abs/1812.04244),\n[PV-RCNN](https://arxiv.org/abs/1912.13192),\n[SPG](https://arxiv.org/abs/2108.06709), and\n[ST3D](https://arxiv.org/abs/2103.05346)) are constructed under machine\nlearning-based neural network architectures, while the issue of domain\nadaptation for data-driven object detection approaches still remains a\nchallenging open problem targeted by recent researches (e.g,\n[SPG](https://arxiv.org/abs/2108.06709) and\n[ST3D](https://arxiv.org/abs/2103.05346)). That is, the performance of object\ndetection models change varying under different geographic appearances or\nweather conditions.\n\n[The model weight of SE-SSD](https://github.com/Vegeta2020/SE-SSD) used in\nLIO-SEGMOT is trained under a subset of the KITTI dataset, and thus it might\nwork well in other subsets of the KITTI dataset. However, we can observe that\nthere are numerous false positive detections in the Hsinchu dataset. Therefore,\nit forces us to choose different detection models (i.e.,\n[PointPillars w/ Livox's model weights](https://github.com/Livox-SDK/livox_detection))\nwhen experimenting LIO-SEGMOT in different real world datasets. In addition,\nsince the model still cannot perform as good detection results in the Hsinchu\ndataset as SE-SSD does in the KITTI dataset, we need to use a more strict\ncriterion for the detection constraint in the tightly-coupled detection factor\nchecks (by decreasing $\\varepsilon^\\prime$ from 26.0 to 19.0). This points out\nthe first limitation of LIO-SEGMOT. That is, covariance matrices and thresholds\nrelated to object detections (mainly hyperparameters in the hierarchical\ncriterion) are required to be adjusted according to the stability of object\ndetections. Despite it affects generalization capability of the proposed method,\nwe believe that the problem can be mitigated with the breakthrough of the domain\nadaptation for 3-D object detection.\n\nThe second limitation of LIO-SEGMOT is related to the motion model of tracking\nobjects. If an object does not move at constant velocity, LIO-SEGMOT may\nmiscalculate the object velocity, leading to inaccurate predicted object pose in\nthe subsequent state. The main reason is that objects' velocities are supposed\nto be steady and constant in LIO-SEGMOT. It is possible to be resolved by\nintroducing multiple motion models to LIO-SEGMOT and adaptively selecting proper\nmodels during the factor graph optimization, in which the concept is similar to\nan interacting multiple model (IMM) in filtering-based object tracking\napproaches.\n\n### Possible Future Research Directions\n\nThere are two possible future research directions of LIO-SEGMOT:\n\n- The optimization problem of LIO-SEGMOT produces a multi-robot architecturethat\n  may break the efficiency of maintaining single root Bayes trees in iSAM2. It\n  causes an unignorable computational cost, especially when there are lots of\n  dynamic objects coupled in the system. In forthcoming researches, we would\n  like to overcome the bottleneck by introducing the\n  [multi-robot iSAM2 (MR-iSAM2)](http://doi.org/10.1109/iros51168.2021.9636687)\n  algorithm.\n- In addition, rule-based coupling conditions make the proposed method lack the\n  ability to explore global optimality when considering combinatorial ambiguity\n  as unknown integer variables. It raises a complicated mix-integer programming\n  (MIP) problem, whereas a recent work called\n  [multi-hypothesis iSAM (MH-iSAM2)](https://www.cs.cmu.edu/~kaess/pub/Hsiao19icra.pdf)\n  still promotes us to investigate the challenging problem in the future.\n\n## :gift: Acknowledgement\n\nThe project is mainly developed based on [Tixiao\nShan](https://github.com/TixiaoShan)'s excellent work\n[LIO-SAM](https://github.com/TixiaoShan/LIO-SAM), which helps me a lot in\nconstructing LIO-SEGMOT. I would like to express my sincere thanks first. In\naddition, I would like to thank [Wu Zheng](https://github.com/Vegeta2020) and\n[Livox](https://github.com/Livox-SDK) for developing and releasing their awesome\nobject detection modules [SE-SSD](https://github.com/Vegeta2020/SE-SSD) and\n[Livox Detection](https://github.com/Livox-SDK/livox_detection) respectively.\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fstephlin%2Flio-segmot","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fstephlin%2Flio-segmot","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fstephlin%2Flio-segmot/lists"}