{"id":18792495,"url":"https://github.com/prbonn/simp","last_synced_at":"2025-04-13T14:31:18.419Z","repository":{"id":100745568,"uuid":"601165741","full_name":"PRBonn/SIMP","owner":"PRBonn","description":null,"archived":false,"fork":false,"pushed_at":"2023-11-30T10:56:03.000Z","size":2657,"stargazers_count":54,"open_issues_count":1,"forks_count":4,"subscribers_count":9,"default_branch":"master","last_synced_at":"2023-11-30T11:36:53.101Z","etag":null,"topics":[],"latest_commit_sha":null,"homepage":null,"language":"Python","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":null,"status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/PRBonn.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":null,"code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null}},"created_at":"2023-02-13T14:02:50.000Z","updated_at":"2023-11-24T20:31:52.000Z","dependencies_parsed_at":"2023-11-30T11:44:48.390Z","dependency_job_id":null,"html_url":"https://github.com/PRBonn/SIMP","commit_stats":null,"previous_names":[],"tags_count":0,"template":null,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/PRBonn%2FSIMP","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/PRBonn%2FSIMP/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/PRBonn%2FSIMP/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/PRBonn%2FSIMP/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/PRBonn","download_url":"https://codeload.github.com/PRBonn/SIMP/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":223589780,"owners_count":17170036,"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":"2024-11-07T21:20:09.258Z","updated_at":"2024-11-07T21:20:10.980Z","avatar_url":"https://github.com/PRBonn.png","language":"Python","funding_links":[],"categories":[],"sub_categories":[],"readme":"# SIMP: Constructing Metric-Semantic Maps Using Floor Plan Priors for Long-Term Indoor Localization \nThis repository contains the implementation of the following [publication](https://arxiv.org/abs/2303.10959):\n```bibtex\n@inproceedings{zimmerman2023iros,\n  author = {Zimmerman, Nicky and Sodano, Matteo and Marks, Elias and Behley, Jens and Stachniss, Cyrill},\n  title = {{Constructing Metric-Semantic Maps Using Floor Plan Priors for Long-Term Indoor Localization}},\n  booktitle = {IEEE/RSJ Intl.~Conf.~on Intelligent Robots and Systems (IROS)},\n  year = {2023}\n}\n```\n\n## Abstract\nObject-based maps are relevant for scene understanding since they integrate geometric and semantic information of the environment, allowing autonomous robots to robustly localize and interact with on objects. In this paper, we address the task of constructing a metric-semantic map for the purpose of long-term object-based localization. \nWe exploit 3D object detections from monocular RGB frames for both, the object-based map construction, and for globally localizing in the constructed map. To tailor the approach to a target environment, we propose an efficient way of generating 3D annotations to finetune the 3D object detection model.\nWe evaluate our map construction in an office building, and test our long-term localization approach on challenging sequences recorded in the same environment over nine months. The experiments suggest that our approach is suitable for constructing metric-semantic maps, and that our localization approach is robust to long-term changes. \nBoth, the mapping algorithm and the localization pipeline can run online on an onboard computer. \nWe release an open-source C++/ROS implementation of our approach.\n\n## Results\nOur live demos for both localization and mapping on the Dingo-O platform can seen in the following video:\n[![](http://img.youtube.com/vi/z5VKtl3Vyyw/0.jpg)](https://www.youtube.com/watch?v=z5VKtl3Vyyw \"SIMP\")\n\nThe Dingo-O is mounted with Intel NUC10i7FNK and NVidia Jetson Xavier AGX, both running Ubuntu 20.04. Both mapping and localization can be executed onboard of the Dingo.\n\nFor the quantitative evaluation, we collected data for 9 months, traversing our entire floor. Our data collection platform is a Kuka YouBot, and we recorded 4 RGB camera streams, ground truth poses and wheel odometry. We used a 30-minutes long sequence to construct an object-based map of our lab, enriching our floor plan with geometric and semantic information. We then localized on challenging sequences, spanning over 9 months, which include dynamics such a humans and changes to the furniture. \nA visualization of our 3D metric-semantic map, and qualitative results for the localization approach evaluated on our map\n\u003cp align=\"center\"\u003e\n\u003cimg src=\"resources/3dmap.png\" width=\"384\"/\u003e\n\u003cimg src=\"resources/traj.png\" width=\"384\"/\u003e\n\u003c/p\u003e\n\n\n\n### 3D Object Detection \nWe use [Omni3D \u0026 Cube R-CNN](https://github.com/FullMetalNicky/omni3d) for object detection, but in theory it can be replaced by any other model that provides our algorithm with class prediction, confidence score and 3D bounding boxes expressed as translation, rotation and scale. Our trained model can be downloaded [here](https://www.ipb.uni-bonn.de/html/projects/simp/model_recent.pth).\n![omniexample](resources/OmniExample.png)\n\n\n## Installation\nFor our evaluation we installed the code as suggested in the native installation instructions. We provide Docker installations for both ROS 2 Foxy and ROS Noetic, but we did not evaluate our code with them so performance may vary. \n\n### Docker Installation for ROS 2 Foxy \nMake sure you installed Docker Engine and NVidia container so you can run Dokcer with GPU support. \nIn the `SIMP` folder run \n```bash\nsudo make foxy=1\n```\nTo build the Docker. Then download the external resources in the host machine\n```bash\ncd /SIMP/ros2_ws/src/omni3d_ros \u0026\u0026 git clone https://github.com/FullMetalNicky/omni3d.git\ncd /SIMP/ros2_ws/src/omni3d_ros \u0026\u0026 mkdir models/ \u0026\u0026 cd models/ \u0026\u0026 curl -LO https://www.ipb.uni-bonn.de/html/projects/simp/model_recent.pth\n```\nTo enable RVIZ visualization from the Docker run in the host machine\n```bash\n xhost +local:docker\n```\nTo run the Docker\n```bash\nsudo make run foxy=1\n```\nThen in the Docker, build the code\n```bash\ncd ncore \u0026\u0026 mkdir build \u0026\u0026 cd build \u0026\u0026 cmake .. -DBUILD_TESTING=1 \u0026\u0026 make -j12 \ncd ros2_ws \u0026\u0026 . /opt/ros/foxy/setup.bash \u0026\u0026  colcon build \u0026\u0026 . install/setup.bash\n```\n\nYou can run the localization node using\n```bash\nros2 launch nmcl_ros confignmcl.launch dataFolder:=\"/SIMP/ncore/data/floor/GTMap/\"\n```\nAnd the mapping node using \n```bash\nros2 launch omni3d_ros omni3d.launch\n```\n### Docker Installation for ROS Noetic \nMake sure you installed Docker Engine and NVidia container so you can run Dokcer with GPU support. \nIn the `SIMP` folder run \n```bash\nsudo make noetic=1\n```\nTo build the Docker. Then download the external resources in the host machine\n```bash\ncd /SIMP/ros1_ws/src/ \u0026\u0026 git clone https://github.com/FullMetalNicky/omni3d.git\ncd /SIMP/ros1_ws/src/omni3d_ros \u0026\u0026 mkdir models/ \u0026\u0026 cd models/ \u0026\u0026 curl -LO https://www.ipb.uni-bonn.de/html/projects/simp/model_recent.pth\n```\nTo enable RVIZ visualization from the Docker run in the host machine\n```bash\n xhost +local:docker\n```\nTo run the Docker\n```bash\nsudo make run noetic=1\n```\nThen in the Docker, build the code\n```bash\ncd ncore \u0026\u0026 mkdir build \u0026\u0026 cd build \u0026\u0026 cmake .. -DBUILD_TESTING=1 \u0026\u0026 make -j12 \ncd ros1_ws \u0026\u0026 . /opt/ros/noetic/setup.bash \u0026\u0026  catkin_make \u0026\u0026 source devel/setup.bash\n```\n\nYou can run the localization node using\n```bash\nroslaunch nmcl_ros confignmcl.launch dataFolder:=\"/SIMP/ncore/data/floor/GTMap/\"\n```\nAnd the mapping node using \n```bash\nroslaunch omni3d_ros omni3d.launch\n```\n\n### Native Installation \nRequirements:\n* ROS 1 - Noetic\n* OpenCV 4 \n* Eigen\n* GTest\n* [JSON parser](https://github.com/nlohmann/json)\n* Packages - map_server\n* [Omni3D \u0026 Cube R-CNN](https://github.com/FullMetalNicky/omni3d) (our fork)\n\n\nRequired installations\n```bash\nsudo apt-get install nlohmann-json3-dev\nsudo apt-get install libgtest-dev\nsudo apt install libeigen3-dev\nsudo apt-get install ros-noetic-map-server\n```\n\nFirst you need to build the ncore library since I don't have a global CMake yet. Tough life. \u003cbr/\u003e\n```bash\ngit clone https://github.com/PRBonn/SIMP.git\ncd SIMP/ncore\nmkdir build \u0026\u0026 cd build\ncmake .. -DBUILD_TESTING=1 \nmake -j12\n```\nTo see everything is built correctly, run the tests from the build directory\n```bash\n./bin/NMCLUnitTests\n```\nTo build the ROS wrapper for ROS Noetic\n```bash\ncd SIMP/ros1_ws\ncatkin_make\nsource devel/setup.bash \n```\nYou will also need to install [Omni3D \u0026 Cube R-CNN](https://github.com/FullMetalNicky/omni3d). Please follow the installation guide, and make sure to **activate the cubercnn conda** environment every time you run our code. Place the omni3d folder in `SIMP/ros1_ws/src/` because the ROS nodes are expecting it to be there.\nIf you wish to run Omni3D \u0026 Cube R-CNN on NVidia Jetson Xavier AGX, you can use the Docker we provide. \nWe also provide an experiemntal Noetic docker, but it hasn't been tested much. \n\n## Running the Algo\n\nWe use ROS wrappers to run the algorithm. \n\n### Omni3DNode\nListens to Image topics, and broadcasts a Float32MultiArray topic, which includes the 3D bounding box, semantic class and confidence score for every detected object.\nIn our implementation, we sync 4 cameras with 360\u0026deg; FoV coverage, get the prediction for all 4 images, and publishes it as a combined message. \nOmni3DNode.py has a hard-coded path pointing to where ever the Cube R-CNN repository has been cloned to. So make sure to adapt it accordingly.\nParameters: \n* cameraTopics - topic names for 4 cameras (order expected to be front, right, back, left)\n* omniTopic - where to publish the 3D object detections\n* configPath - location of the model config file (.yaml)\n* modelPath - location of the model (.pt)\n* jsonPath - path to args.json file which includes information about the cameras extrinsics and intrinsics\n\nThe Omni3DNode can be replaced by a user-chosen 3D object detection pipeline, as long as the outputs conform to the format of the our detection message. Each detected objects is represented by 8 3D points corresponding to the corners of the bounding box, a confidence score and the semantic class. Detections are concatenated together as a list of floats (26 floats per object). \n\n### Omni3DMappingNode\nListens to an Image topic, and broadcasts a Omni3DArray topic, which includes the 3D bounding box, semantic class and confidence score for every detected object.\nOmni3DMappingNode.py has a hard-coded path pointing to where ever the Cube R-CNN repository has been cloned to. So make sure to adapt it accordingly.\nParameters: \n* cameraTopic - topic name for camera\n* omniTopic - where to publish the 3D object detections\n* configPath - location of the model config file (.yaml)\n* modelPath - location of the model (.pt)\n* jsonPath - path to .json file which includes cameras extrinsics and intrinsics, and algorithm parametrs\n\nThe Omni3DMappingNode can be replaced by a user-chosen 3D object detection pipeline, as long as the outputs conform to the format of the our Omni3DArray detection message. This message is an array of Omni3D message, which include the fields center, dim, rot, category and confidence.\n\n### GTNode\nBroadcasts PoseStamped messages with the ground truth pose of the robot.\n* jsonPath - path to .json file which includes cameras extrinsics and intrinsics, and algorithm parameters\n* gtTopic - topic name for ground truth pose\n* cameraTopic - topic name for camera\n* csvPath - path to csv file with ground truth poses\n\nThe GTNode can be replaced by any other pose source, such as VIO.\n\n### SIMPNode\nListens to Omni3DArray and PoseStamped messages, and broadcasts MarkerArray messages that visualize the constructed map in RVIZ.\nThis node synchronizes the 3D object detections with the robot's current pose to update a metric-semantic map, and is a ROS wrapper around the mapping algorithm in SIMP.py. \n* cameraTopic - topic name for camera\n* gtTopic - topic name for ground truth pose\n* dumpPath\n* jsonPath - path to .json file which includes cameras extrinsics and intrinsics, and algorithm parametrs\nThe service /dump_map can be used to get a .pickle file of the mapped objects. To run the mapping algorithm \n```bash\nroslaunch omni3d_ros omni3d.launch\n```\n\n### ConfigNMCLNode\nRequires as input 2 topics, Float32MultiArray with 3D object detections and Odometry from VO or wheel encoders. The topic names are configurable through the launch file. This runs the correct and predict steps asynchronously, so correct is executed whenever we have odometry and predict is executed when an observation arrives. ConfigNMCLNodE publishes PoseWithCovarianceStamped messages with the pose prediction. To see the predictions in RVIZ, run\n```bash\nroslaunch nmcl_ros youbot_localization.launch dataFolder:=\"path/to/data/folder/\"\n```\n\nThe data folder should have the following items:\n* nmcl.config - defines algorithm parameters, like sampling stratgy, motion/observation model, particle number, etc.  \n* building.config - defines how many floors there are in the map (only 1 for this code)\n* floor.config - defines the map resources:  \n  * roomseg.png - pixel-wise annotation associating a pixel with a room number (1, 2, 3...) \n  * Map.yaml (map_server format)\n  * Map.png  (map_server format)\n  * global_variance.json - stores the semantic, geometric and statistical infromation about the objects in that floor\n\u003cbr/\u003e\nFor an example of the content of these files, you can check out [this](ncore/data/floor/GTMap). The data folder contains mostly things that are environment specific, like maps and algorithms definitions. The global_veriance.json is computed using the statistical analysis script. We have only tested this code thoroughly on a signle floor, but it can be potentialy extended to multiple floors. We currently don't have support for multi-floor visualization. Notice that for each floor you'll need its own map resources. \n\u003cbr/\u003e\n\n\n### SemanticMapNode\nA visualizer for our abstract semantic maps. \nParameters: \n* dataset - the path for the dataset.yaml required for YOLO\n* dataFolder - as explained above\n* mapName \n* markerTopic - where to publish the semantic information\n\n## Pre-Processing Steps\n\n### Labeling Script\nNew training labels can be generated using NewAutomaticLabeling.py. The generated labels are in Hypersim format and are compatible with what is required to train Cube R-CNN. It requires 3D annotaions of the ground truth objects in the global coordinate system, which are labeled only once, as a pre-processing step. We acquired our ground truth annotations from manually annotating bounding boxes on a pointcloud in [segments.ai](https://segments.ai/). But you can also create annotations but hand-measuring the objects. The format for the ground truth objects is demonstrated in `ros1_ws/src/omni3d_ros/configs/Lab3D-v2.0.json`.\nFor every posed RGB frame that requires labeling, we also need 2D object detections. We used a YOLOv5 model, that can be downloaded [here](https://www.ipb.uni-bonn.de/html/projects/simp/yolo4omni3d_v0.1.pt).  \n\n### Inference Script\nTo infer bounding boxes from images, we have the `demo/Predict.py` script in our Cube R-CNN [fork](https://github.com/FullMetalNicky/omni3d) . It outputs the predictions in a format that is compatible with the statistical analysis scripts. \n```bash\npython3 demo/Predict.py --config configs/trained_Omni3D.yaml --input-folder \"datasets/Omni3D/Hypersim_test.json\" --threshold 0.25 MODEL.WEIGHTS your-model.pth\n```\n\n### Statistical Analysis\nThe statistical analysis was computed using ComputeStatsOnPred.py. It requires 3D annotaions of the ground truth objects in the global coordinate system, which are labeled only once, as a pre-processing step. The ground truth objects are matched against a list of detected objects, to provide the desired statistics. The output a file with the per class error variance, which we provide in a .csv format. Given a map (e.g. constructed by Omni3DMappingNode), the script also produces a file with per object error variance , which we provide in a .json and .csn formats.\nfor generated a correct prediction list, the script from our Omni3d \u0026 Cube R-CNN fork can be used.\n\n## Demo for ROS Noetic\nIf you would like to play a demo rosbag (from ROS1), you can download it [here](https://www.ipb.uni-bonn.de/html/projects/simp/2023-02-24-10-56-43_0.bag) and the corresponding [ground truth file](https://www.ipb.uni-bonn.de/html/projects/simp/raw_gt.csv). Notice that this is a random recording that was not used to evaluate the localization or the mapping, and it's provided just so people can play it and see that our code actually runs. Place the files in `SIMP/data/Demo/` and you can run the localization node with \n```bash\nroslaunch nmcl_ros dingo_localization.launch dataFolder:=\"/SIMP/ncore/data/floor/GTMap/\"\n```\nAnd the mapping node with \n```bash\nroslaunch omni3d_ros dingo_omni3d.launch\n```\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fprbonn%2Fsimp","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fprbonn%2Fsimp","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fprbonn%2Fsimp/lists"}