├── .gitignore ├── LICENSE.txt ├── README.md ├── backend ├── multi_robot_utils_launch │ ├── CMakeLists.txt │ ├── launch │ │ ├── others │ │ │ ├── relay_topics_for_loop_closure.launch │ │ │ ├── relay_topics_for_loop_closure_semantic_meas_and_odom.launch │ │ │ ├── relay_topics_for_loop_closure_step2.launch │ │ │ ├── relay_topics_multi_robot.launch │ │ │ ├── relay_topics_multi_robot_2_robots.launch │ │ │ └── relay_topics_multi_robot_4_robots.launch │ │ ├── play_bag_for_object_modeller.launch │ │ ├── play_bag_for_object_modeller_robot2.launch │ │ ├── play_bag_with_remapping.launch │ │ ├── publish_tf.launch │ │ ├── record_bag_for_vis_pennovation_lidar_seg_pc.launch │ │ ├── record_bag_multi_robot.launch │ │ ├── record_bag_ns_robot.launch │ │ ├── record_bag_vems_slam_ground_station.launch │ │ ├── record_bag_vems_slam_robot.sh │ │ └── relay_topics_for_quad2_quad1.launch │ ├── package.xml │ └── script │ │ ├── tmux_multi_robot_indoor_outdoor-bkup.sh │ │ ├── tmux_multi_robot_with_bags.sh │ │ ├── tmux_multi_robot_with_bags_dcist.sh │ │ ├── tmux_multi_robot_with_bags_f250_scarab.sh │ │ ├── tmux_multi_robot_with_bags_forest.sh │ │ ├── tmux_multi_robot_with_bags_parking_lot.sh │ │ ├── tmux_multi_robot_with_bags_scarabs_indoor.sh │ │ ├── tmux_single_indoor_robot.sh │ │ ├── tmux_single_indoor_robot_open_vocab.sh │ │ ├── tmux_single_outdoor_kitti.sh │ │ └── tmux_single_outdoor_robot.sh ├── sloam │ ├── CMakeLists.txt │ ├── clipper_semantic_object │ │ ├── .gitignore │ │ ├── CMakeLists.txt │ │ ├── LICENSE │ │ ├── README.md │ │ ├── benchmarks │ │ │ ├── CMakeLists.txt │ │ │ ├── bm_utils.cpp │ │ │ ├── bm_utils.h │ │ │ └── main.cpp │ │ ├── bindings │ │ │ ├── matlab │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── euclideandistance_mex.cpp │ │ │ │ ├── finddensecluster_mex.cpp │ │ │ │ ├── mexutils.h │ │ │ │ └── pointnormaldistance_mex.cpp │ │ │ └── python │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── py_clipper.cpp │ │ │ │ ├── setup.py.in │ │ │ │ └── trampolines.h │ │ ├── cmake │ │ │ ├── FindMKL.cmake │ │ │ └── clipper-config.cmake.in │ │ ├── devel │ │ │ ├── .built_by │ │ │ ├── .private │ │ │ │ └── catkin_tools_prebuild │ │ │ │ │ ├── .catkin │ │ │ │ │ ├── .rosinstall │ │ │ │ │ ├── _setup_util.py │ │ │ │ │ ├── cmake.lock │ │ │ │ │ ├── env.sh │ │ │ │ │ ├── lib │ │ │ │ │ └── pkgconfig │ │ │ │ │ │ └── catkin_tools_prebuild.pc │ │ │ │ │ ├── local_setup.bash │ │ │ │ │ ├── local_setup.sh │ │ │ │ │ ├── local_setup.zsh │ │ │ │ │ ├── setup.bash │ │ │ │ │ ├── setup.sh │ │ │ │ │ ├── setup.zsh │ │ │ │ │ └── share │ │ │ │ │ └── catkin_tools_prebuild │ │ │ │ │ └── cmake │ │ │ │ │ ├── catkin_tools_prebuildConfig-version.cmake │ │ │ │ │ └── catkin_tools_prebuildConfig.cmake │ │ │ ├── _setup_util.py │ │ │ ├── cmake.lock │ │ │ ├── env.sh │ │ │ ├── lib │ │ │ │ └── pkgconfig │ │ │ │ │ └── catkin_tools_prebuild.pc │ │ │ ├── local_setup.bash │ │ │ ├── local_setup.sh │ │ │ ├── local_setup.zsh │ │ │ ├── setup.bash │ │ │ ├── setup.sh │ │ │ ├── setup.zsh │ │ │ └── share │ │ │ │ └── catkin_tools_prebuild │ │ │ │ └── cmake │ │ │ │ ├── catkin_tools_prebuildConfig-version.cmake │ │ │ │ └── catkin_tools_prebuildConfig.cmake │ │ ├── examples │ │ │ ├── c++ │ │ │ │ └── example.cpp │ │ │ ├── data │ │ │ │ ├── bun10k.ply │ │ │ │ ├── robot0Map_forest.txt │ │ │ │ ├── robot0Map_forest_2d.txt │ │ │ │ ├── robot0Map_indoor.txt │ │ │ │ ├── robot0Map_parking.txt │ │ │ │ ├── robot1Map_forest.txt │ │ │ │ ├── robot1Map_forest_2d.txt │ │ │ │ ├── robot1Map_indoor.txt │ │ │ │ ├── robot1Map_parking.txt │ │ │ │ ├── robot2Map_forest.txt │ │ │ │ ├── robot2Map_forest_2d.txt │ │ │ │ ├── robot2Map_indoor.txt │ │ │ │ └── robot2Map_parking.txt │ │ │ └── matlab │ │ │ │ ├── ex1_knownscalepointcloud.m │ │ │ │ ├── ex3_planecloud.m │ │ │ │ └── ex4_bunny.m │ │ ├── include │ │ │ ├── clipper │ │ │ │ ├── clipper.h │ │ │ │ ├── dsd.h │ │ │ │ ├── invariants │ │ │ │ │ ├── abstract.h │ │ │ │ │ ├── builtins.h │ │ │ │ │ ├── euclidean_distance.h │ │ │ │ │ └── pointnormal_distance.h │ │ │ │ ├── maxclique.h │ │ │ │ ├── sdp.h │ │ │ │ ├── types.h │ │ │ │ └── utils.h │ │ │ ├── semantic_clipper.h │ │ │ └── triangulation │ │ │ │ ├── descriptor.hpp │ │ │ │ ├── distance.hpp │ │ │ │ ├── observation.hpp │ │ │ │ └── polygon.hpp │ │ ├── matlab │ │ │ └── clipper.m │ │ ├── src │ │ │ ├── clipper.cpp │ │ │ ├── dsd.cpp │ │ │ ├── invariants │ │ │ │ ├── euclidean_distance.cpp │ │ │ │ └── pointnormal_distance.cpp │ │ │ ├── maxclique.cpp │ │ │ ├── sdp.cpp │ │ │ ├── semantic_clipper.cpp │ │ │ ├── triangulation │ │ │ │ ├── descriptor.cpp │ │ │ │ └── observation.cpp │ │ │ └── utils.cpp │ │ └── test │ │ │ ├── CMakeLists.txt │ │ │ ├── affinity_test.cpp │ │ │ ├── clipper_test.cpp │ │ │ ├── dsd_test.cpp │ │ │ ├── main.cpp │ │ │ ├── sdp_test.cpp │ │ │ └── test_semantic_clipper.cpp │ ├── cmake │ │ └── CMakeHelpers.cmake │ ├── include │ │ ├── core │ │ │ ├── cubeMapManager.h │ │ │ ├── cylinderMapManager.h │ │ │ ├── databaseManager.h │ │ │ ├── ellipsoidMapManager.h │ │ │ ├── place_recognition.h │ │ │ ├── robot.h │ │ │ ├── sloam.h │ │ │ ├── sloamNode.h │ │ │ └── sloamNodelet.h │ │ ├── factorgraph │ │ │ ├── cubeFactor.h │ │ │ ├── cylinderFactor.h │ │ │ ├── graph.h │ │ │ └── graphWrapper.h │ │ ├── helpers │ │ │ ├── definitions.h │ │ │ ├── serialization.h │ │ │ └── utils.h │ │ ├── objects │ │ │ ├── cube.h │ │ │ ├── cylinder.h │ │ │ ├── ellipsoid.h │ │ │ ├── plane.h │ │ │ └── semanticObject.h │ │ └── viz │ │ │ └── vizTools.h │ ├── launch │ │ ├── decentralized_sloam.launch │ │ ├── decentralized_sloam_indoor_outdoor.launch │ │ ├── decentralized_sloam_multi_robot_dcist.launch │ │ ├── decentralized_sloam_multi_robot_f250_scarab.launch │ │ ├── decentralized_sloam_multi_robot_forest.launch │ │ ├── decentralized_sloam_multi_robot_seven_robots_indoor_outdoor.launch │ │ ├── decentralized_sloam_scarab.launch │ │ ├── play_bag_indoor_scarab.launch │ │ ├── play_multi_robot_bags_with_remapping.launch │ │ ├── run.launch │ │ ├── run_indoor_large_scale_exploration.launch │ │ ├── run_pennovation.launch │ │ ├── run_sim.launch │ │ ├── rviz │ │ │ ├── decentralized_sloam.rviz │ │ │ └── sloam_active_slam.rviz │ │ ├── segmentation.launch │ │ ├── single_robot_sloam_test.launch │ │ ├── single_robot_sloam_test_LiDAR.launch │ │ ├── single_robot_sloam_test_benchmark.launch │ │ ├── single_robot_sloam_test_outdoor.launch │ │ ├── sloam.launch │ │ ├── sloam.launch.debug │ │ ├── sloam_bw_test.launch │ │ └── unit_tests │ │ │ └── place_recognition_test.launch │ ├── nodelet_plugins.xml │ ├── package.xml │ ├── params │ │ ├── sloam-forest-parking-lot.yaml │ │ └── sloam.yaml │ ├── resource │ │ ├── cabinet.stl │ │ ├── chair-no-back.dae │ │ ├── chair.stl │ │ ├── combine_cad_mode.py │ │ ├── door.stl │ │ ├── fridge.stl │ │ ├── microwave.stl │ │ ├── monitor.stl │ │ ├── move_chair.py │ │ ├── plant.stl │ │ ├── robot.stl │ │ ├── rotate_door.py │ │ ├── scale_object.py │ │ ├── table2.stl │ │ └── trash_can.stl │ └── src │ │ ├── core │ │ ├── cubeMapManager.cpp │ │ ├── cylinderMapManager.cpp │ │ ├── databaseManager.cpp │ │ ├── ellipsoidMapManager.cpp │ │ ├── inputNode.cpp │ │ ├── place_recognition.cpp │ │ ├── robot.cpp │ │ ├── sloam.cpp │ │ ├── sloamNode.cpp │ │ └── sloamNodelet.cpp │ │ ├── factorgraph │ │ ├── cubeFactor.cpp │ │ ├── cylinderFactor.cpp │ │ ├── graph.cpp │ │ └── graphWrapper.cpp │ │ ├── objects │ │ ├── cube.cpp │ │ ├── cylinder.cpp │ │ ├── ellipsoid.cpp │ │ └── plane.cpp │ │ ├── tests │ │ ├── core_test.cpp │ │ ├── cube_factor_test.cpp │ │ ├── cylinder_factor_test.cpp │ │ ├── cylinder_test.cpp │ │ ├── loop_closure_test.cpp │ │ ├── place_recognition_test.cpp │ │ └── plane_test.cpp │ │ └── viz │ │ └── vizTools.cpp └── sloam_msgs │ ├── CMakeLists.txt │ ├── action │ ├── ActiveLoopClosure.action │ └── DetectLoopClosure.action │ ├── msg │ ├── LoopClosure.msg │ ├── MultiArrayPoseObjectEdges.msg │ ├── ObservationPair.msg │ ├── PoseMst.msg │ ├── PoseMstBundle.msg │ ├── ROSCube.msg │ ├── ROSCylinder.msg │ ├── ROSCylinderArray.msg │ ├── ROSEllipsoid.msg │ ├── ROSGround.msg │ ├── ROSObservation.msg │ ├── ROSRangeBearing.msg │ ├── ROSRangeBearingSyncOdom.msg │ ├── ROSScan.msg │ ├── ROSSubMap.msg │ ├── ROSSweep.msg │ ├── ROSSyncOdom.msg │ ├── SemanticLoopClosure.msg │ ├── SemanticMeasSyncOdom.msg │ ├── StampedRvizMarkerArray.msg │ ├── cubeMap.msg │ ├── cylinderMap.msg │ ├── interRobotTF.msg │ ├── keyPoses.msg │ ├── poseObjectEdges.msg │ ├── syncPcOdom.msg │ ├── vector4d.msg │ └── vector7d.msg │ ├── package.xml │ └── srv │ ├── EvaluateLoopClosure.srv │ └── graphTansmission.srv ├── frontend ├── object_modeller │ ├── CMakeLists.txt │ ├── config │ │ ├── cls_all.yaml │ │ ├── cylinder_plane_modeller_params.yaml │ │ ├── open_vocab_cls_all.yaml │ │ └── rgb_segmentation_open_vocab_params.yaml │ ├── include │ │ └── object_modeller │ │ │ └── cylinder_modeller.h │ ├── launch │ │ ├── rgb_segmentation_f250.launch │ │ ├── rgb_segmentation_open_vocab.launch │ │ └── sync_semantic_measurements.launch │ ├── models │ │ └── THIS_FOLDER_STORES_YOUR_MODELS.txt │ ├── object_detector_utils │ │ ├── detect.py │ │ └── detect_open_vocab.py │ ├── package.xml │ ├── rviz │ │ ├── default.rviz │ │ └── object_modeller.rviz │ ├── script │ │ ├── cylinder_plane_modeller.py │ │ ├── merge_synced_measurements.py │ │ ├── play_bag_remap_scripts │ │ │ └── play_bag_with_remapping.launch │ │ ├── sync_centroid_odom.py │ │ ├── sync_cuboid_odom.py │ │ ├── sync_cylinder_odom.py │ │ ├── tmux_benchmark.sh │ │ ├── tmux_indoor_outdoor_merge.sh │ │ ├── tmux_run_all_nodes.sh │ │ └── tmux_run_all_nodes_jiuzhou.sh │ └── src │ │ └── cylinder_modeller.cpp └── scan2shape │ ├── rviz │ ├── default.rviz │ └── faster-lio-sloam.rviz │ ├── scan2shape_launch │ ├── CMakeLists.txt │ ├── config │ │ ├── infer_node_params.yaml │ │ ├── process_cloud_node_indoor_cls_info.yaml │ │ ├── process_cloud_node_indoor_open_vocab_cls_info.yaml │ │ ├── process_cloud_node_indoor_open_vocab_params.yaml │ │ ├── process_cloud_node_indoor_params.yaml │ │ ├── process_cloud_node_outdoor_class_info.yaml │ │ ├── process_cloud_node_outdoor_class_info_dcist_demo.yaml │ │ ├── process_cloud_node_outdoor_kitti_class_info.yaml │ │ ├── process_cloud_node_outdoor_kitti_params.yaml │ │ ├── process_cloud_node_outdoor_params.yaml │ │ ├── process_cloud_node_real_robot.yaml │ │ ├── process_cloud_node_real_robot_full_mission.yaml │ │ ├── process_cloud_node_real_robot_full_mission_loop_closure.yaml │ │ └── process_cloud_node_sim.yaml │ ├── launch │ │ ├── closure_retrigger.launch │ │ ├── drivers_for_faster_lio.launch │ │ ├── infer_node.launch │ │ ├── llol_robot_ns.launch │ │ ├── ouster_decoder.launch │ │ ├── play_bag_with_remapping.sh │ │ ├── process_cloud_node_lidar_indoor_with_ns.launch │ │ ├── process_cloud_node_outdoor_kitti_with_ns.launch │ │ ├── process_cloud_node_outdoor_with_ns.launch │ │ ├── process_cloud_node_rgbd_indoor_open_vocab_with_ns.launch │ │ ├── process_cloud_node_rgbd_indoor_with_ns.launch │ │ ├── real_robot_process_cloud_node.launch │ │ ├── record_bag.launch │ │ ├── record_bag_for_confidence_stats.launch │ │ ├── record_bag_for_loop_closure.launch │ │ ├── record_bag_for_running_process_node.launch │ │ ├── run_flio_with_driver.launch │ │ ├── run_lidar_driver_decoder.launch │ │ ├── run_llol_with_driver.launch │ │ ├── sim_perturb_odom.launch │ │ ├── sim_process_cloud_node.launch │ │ ├── sloam_process_cloud_node.launch │ │ └── throttle_and_remap_kitti.launch │ ├── package.xml │ └── script │ │ ├── backbone │ │ ├── decoder │ │ ├── infer_node.py │ │ ├── process_cloud_node.py │ │ ├── process_cloud_node_lidar_indoor.py │ │ └── process_cloud_node_outdoor.py │ └── script │ ├── CRF.py │ ├── assignment.py │ ├── backbone │ ├── basic_darknet.py │ └── darknet.py │ ├── cuboid_utils_indoor.py │ ├── cuboid_utils_outdoor.py │ ├── decoder │ ├── basic_darknet.py │ └── darknet.py │ ├── infer_node.py │ ├── laserscan.py │ ├── load_model.py │ ├── object_tracker.py │ ├── object_tracker_utils.py │ ├── process_cloud_node.py │ ├── process_cloud_node_lidar_indoor.py │ ├── process_cloud_node_outdoor.py │ ├── segmentator.py │ ├── sklearn_matching_utils.py │ ├── tmux_no_docker.sh │ ├── tmux_pipeline.sh │ ├── tmux_script_multi_robot_sloam.sh │ ├── utils.py │ └── utils_outdoor.py └── run_slide_slam_docker.sh /.gitignore: -------------------------------------------------------------------------------- 1 | */build/* 2 | *.pyc 3 | */__pycache__/* 4 | /scan2shape/script/explo_plots/* 5 | /scan2shape/script/explo_model* 6 | /scan2shape/script/explo_plots2/* 7 | /scan2shape/script/stats_data/* 8 | ouster_example 9 | /scan2shape/script/plots/* 10 | scan2shape/script/max_scale.pkl 11 | /*/*/build/ 12 | *log*.txt 13 | *.bag 14 | *.pt 15 | *.npy 16 | .vscode 17 | *.png 18 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, KumarRobotics at University of Pennsylvania 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(multi_robot_utils_launch) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED) 7 | 8 | catkin_package() 9 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/others/relay_topics_for_loop_closure.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 10 | 12 | 14 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/others/relay_topics_for_loop_closure_semantic_meas_and_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8 | 9 | 11 | 13 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/others/relay_topics_for_loop_closure_step2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 10 | 12 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/others/relay_topics_multi_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 9 | 11 | 13 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/others/relay_topics_multi_robot_2_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 10 | 12 | 14 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/others/relay_topics_multi_robot_4_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 12 | 14 | 16 | 17 | 19 | 21 | 23 | 25 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/play_bag_for_object_modeller.launch: -------------------------------------------------------------------------------- 1 | 2 | 8 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/play_bag_for_object_modeller_robot2.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/play_bag_with_remapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/publish_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/record_bag_for_vis_pennovation_lidar_seg_pc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 17 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/record_bag_multi_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 29 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/record_bag_ns_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/record_bag_vems_slam_ground_station.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 61 | 62 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/record_bag_vems_slam_robot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | TOPICS=" 4 | /tf 5 | /tf_static 6 | /${AGENT}/camera/depth/image_rect_raw 7 | /${AGENT}/camera/depth/camera_info 8 | /${AGENT}/camera/color/image_raw 9 | /${AGENT}/camera/infra1/image_rect_raw 10 | /${AGENT}/camera/infra2/image_rect_raw 11 | /${AGENT}/camera/imu 12 | /${AGENT}/camera/aligned_depth_to_color/image_raw 13 | /${AGENT}/camera/aligned_depth_to_color/camera_info 14 | /scarab40/odom_laser 15 | /scarab41/odom_laser 16 | /scarab42/odom_laser 17 | /scarab43/odom_laser 18 | /scarab44/odom_laser 19 | /scarab45/odom_laser 20 | /scarab46/odom_laser 21 | /robot0/semantic_meas_sync_odom 22 | /robot1/semantic_meas_sync_odom 23 | /robot2/semantic_meas_sync_odom 24 | /robot3/semantic_meas_sync_odom 25 | /robot4/semantic_meas_sync_odom 26 | " 27 | 28 | ALL_TOPICS=$TOPICS 29 | 30 | BAG_STAMP=$(date +%F-%H-%M-%S-%Z) 31 | CURR_TIMEZONE=$(date +%Z) 32 | 33 | BAG_PREFIX=VEMS-SLAM-${AGENT}-${CURR_TIMEZONE} 34 | 35 | eval rosbag record -b512 $ALL_TOPICS -o $BAG_PREFIX -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/launch/relay_topics_for_quad2_quad1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 10 | 12 | 14 | -------------------------------------------------------------------------------- /backend/multi_robot_utils_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multi_robot_utils_launch 4 | 0.0.0 5 | The multi_robot_utils_launch package 6 | 7 | Xu Liu 8 | Jiuzhou Lei 9 | Ankit Prabhu 10 | Yuezhan Tao 11 | 12 | Xu Liu 13 | Jiuzhou Lei 14 | Ankit Prabhu 15 | Yuezhan Tao 16 | 17 | Penn Software License 18 | 19 | catkin 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | *.pyc 3 | *.m~ 4 | .vscode 5 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 MIT Aerospace Controls Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/README.md: -------------------------------------------------------------------------------- 1 | ## Note 2 | The repo is forked from CLIPPER and modified to extract triangulation descriptors from point landmarks and robustly estimate point correspondences betweentwo set of points. The code is put inside the slide_slam for the convenience of debugging and implementation. 3 | 4 | Original CLIPPER Git Repo: https://github.com/mit-acl/clipper 5 | 6 | CLIPPER citation 7 | ``` 8 | @inproceedings{lusk2021clipper, 9 | title={{CLIPPER}: A graph-theoretic framework for robust data association}, 10 | author={Lusk, Parker C and Fathian, Kaveh and How, Jonathan P}, 11 | booktitle={2021 IEEE International Conference on Robotics and Automation (ICRA)}, 12 | pages={13828--13834}, 13 | year={2021}, 14 | organization={IEEE} 15 | } 16 | ``` 17 | 18 | #### Dependencies for clipper and triangulation 19 | Eigen3, MKL, Boost, Opencv, qhull, pmc, scs 20 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/benchmarks/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | FetchContent_Declare(tinyply 3 | GIT_REPOSITORY https://github.com/ddiakopoulos/tinyply 4 | GIT_TAG 2.3.4) 5 | # set(BUILD_TESTS OFF) 6 | # FetchContent_MakeAvailable(tinyply) 7 | 8 | FetchContent_GetProperties(tinyply) 9 | if(NOT tinyply_POPULATED) 10 | FetchContent_Populate(tinyply) 11 | 12 | set(BUILD_TESTS OFF) 13 | add_subdirectory(${tinyply_SOURCE_DIR} ${tinyply_BINARY_DIR}) 14 | 15 | # fix for tinyply because it does not use target_include_directories 16 | target_include_directories(tinyply PUBLIC 17 | $) 18 | endif() 19 | 20 | include(FetchContent) 21 | FetchContent_Declare(nanoflann 22 | GIT_REPOSITORY https://github.com/jlblancoc/nanoflann 23 | GIT_TAG v1.4.2) 24 | set(NANOFLANN_BUILD_EXAMPLES OFF) 25 | set(NANOFLANN_BUILD_TESTS OFF) 26 | FetchContent_MakeAvailable(nanoflann) 27 | 28 | include(FetchContent) 29 | FetchContent_Declare(libfort 30 | GIT_REPOSITORY https://github.com/seleznevae/libfort 31 | GIT_TAG v0.4.2) 32 | set(FORT_ENABLE_TESTING OFF) 33 | FetchContent_MakeAvailable(libfort) 34 | 35 | include(FetchContent) 36 | FetchContent_Declare(indicators 37 | GIT_REPOSITORY https://github.com/p-ranav/indicators 38 | GIT_TAG v2.2) 39 | set(INDICATORS_BUILD_TESTS OFF) 40 | set(INDICATORS_SAMPLES OFF) 41 | set(INDICATORS_DEMO OFF) 42 | FetchContent_MakeAvailable(indicators) 43 | 44 | 45 | add_executable(benchmark) 46 | target_sources(benchmark PRIVATE main.cpp bm_utils.cpp) 47 | target_include_directories(benchmark PRIVATE $) 48 | target_link_libraries(benchmark clipper Eigen3::Eigen tinyply nanoflann fort indicators) 49 | 50 | # trying to make life easier when running ./benchmarks/benchmark 51 | add_custom_command(TARGET benchmark POST_BUILD 52 | COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_SOURCE_DIR}/examples/data/*.ply 53 | ${CMAKE_BINARY_DIR}/) -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/bindings/matlab/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Matlab COMPONENTS MX_LIBRARY) 2 | 3 | if(Matlab_FOUND) 4 | message(STATUS "MATLAB root directory found: ${Matlab_ROOT_DIR}") 5 | 6 | matlab_add_mex(NAME finddensecluster_mex SRC finddensecluster_mex.cpp LINK_TO Eigen3::Eigen clipper) 7 | set_target_properties(finddensecluster_mex PROPERTIES COMPILE_FLAGS "-fvisibility=default") 8 | set_target_properties(finddensecluster_mex PROPERTIES OUTPUT_NAME clipper_finddensecluster) 9 | 10 | matlab_add_mex(NAME euclideandistance_mex SRC euclideandistance_mex.cpp LINK_TO Eigen3::Eigen clipper) 11 | set_target_properties(euclideandistance_mex PROPERTIES COMPILE_FLAGS "-fvisibility=default") 12 | set_target_properties(euclideandistance_mex PROPERTIES OUTPUT_NAME clipper_euclideandistance) 13 | 14 | matlab_add_mex(NAME pointnormaldistance_mex SRC pointnormaldistance_mex.cpp LINK_TO Eigen3::Eigen clipper) 15 | set_target_properties(pointnormaldistance_mex PROPERTIES COMPILE_FLAGS "-fvisibility=default") 16 | set_target_properties(pointnormaldistance_mex PROPERTIES OUTPUT_NAME clipper_pointnormaldistance) 17 | else() 18 | message(WARNING "MATLAB root directory not found. Will not build MATLAB bindings.") 19 | set(BUILD_MATLAB_BINDINGS OFF) 20 | endif() 21 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/bindings/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(PYPKG_DIR "${CMAKE_CURRENT_BINARY_DIR}/clipperpy") 2 | 3 | pybind11_add_module(py_clipper py_clipper.cpp) 4 | target_link_libraries(py_clipper PUBLIC clipper) 5 | set_target_properties(py_clipper PROPERTIES OUTPUT_NAME "clipperpy") 6 | set_target_properties(py_clipper PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${PYPKG_DIR}") 7 | 8 | # copy setup.py file binary dir for install with: pip install . 9 | configure_file(setup.py.in ${CMAKE_CURRENT_BINARY_DIR}/setup.py) 10 | 11 | # Create the Python package -- Note that "." is used to conform to PEP 328 12 | file(WRITE "${PYPKG_DIR}/__init__.py" 13 | "from .clipperpy import *\n" 14 | "from .clipperpy import __version__\n" 15 | "from .clipperpy import __doc__") 16 | 17 | set(DIST "none") 18 | if(UNIX AND NOT APPLE) 19 | execute_process(COMMAND bash -c "lsb_release -cs" OUTPUT_VARIABLE UBUNTU_DIST) 20 | string(STRIP "${UBUNTU_DIST}" UBUNTU_DIST) 21 | set(DIST "${UBUNTU_DIST}") 22 | elseif(APPLE) 23 | # TODO: not very specific... 24 | set(DIST "macos") 25 | elseif(WIN32) 26 | # TODO: not very specific... 27 | set(DIST "win10") 28 | endif() 29 | 30 | set(PKGSTR clipperpy-py3-${DIST}-${PROJECT_VERSION}) 31 | add_custom_target(pypkg 32 | DEPENDS py_clipper 33 | COMMAND ${CMAKE_COMMAND} -E make_directory ${PKGSTR} 34 | COMMAND ${CMAKE_COMMAND} -E copy setup.py ${PKGSTR}/ 35 | COMMAND ${CMAKE_COMMAND} -E copy_directory ${PYPKG_DIR} ${PKGSTR}/clipperpy 36 | COMMAND ${CMAKE_COMMAND} -E tar zcvf ${PKGSTR}.tar.gz ${PKGSTR} 37 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) 38 | 39 | add_custom_target(pip-install 40 | DEPENDS pypkg 41 | COMMAND ${PYTHON_EXECUTABLE} -m pip install . 42 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/${PKGSTR} 43 | ) -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/bindings/python/setup.py.in: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='clipperpy', 5 | version='${PROJECT_VERSION}', 6 | author='Parker Lusk', 7 | author_email='plusk@mit.edu', 8 | description='Robust data association framework', 9 | package_dir={'': '${CMAKE_CURRENT_BINARY_DIR}'}, 10 | packages=['clipperpy'], 11 | package_data={'': ['*.so']} 12 | ) -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/bindings/python/trampolines.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file trampolines.h 3 | * @brief Trampolines to help overriding C++ in Python 4 | * @author Parker Lusk 5 | * @date 16 May 2021 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "clipper/invariants/abstract.h" 13 | 14 | template 15 | class PyInvariant : public InvariantBase { 16 | public: 17 | using InvariantBase::InvariantBase; // Inherit constructors 18 | }; 19 | 20 | template 21 | class PyPairwiseInvariant : public PyInvariant { 22 | public: 23 | using PyInvariant::PyInvariant; // Inherit constructors 24 | using Datum = clipper::invariants::Datum; // for convenience 25 | // trampoline for virtual function 26 | double operator()(const Datum& ai, const Datum& aj, const Datum& bi, const Datum& bj) override { 27 | pybind11::gil_scoped_acquire acquire; // Acquire GIL before calling Python code 28 | PYBIND11_OVERRIDE_PURE_NAME(double, PairwiseInvariantBase, "__call__", operator(), ai, aj, bi, bj); 29 | } 30 | }; -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/cmake/clipper-config.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the clipper package 2 | # It defines the following variables 3 | # CLIPPER_LIBRARIES - libraries to link against 4 | 5 | # Compute paths 6 | get_filename_component(ADAPTNOTCH_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 7 | 8 | # Our library dependencies (contains definitions for IMPORTED targets) 9 | if(NOT TARGET clipper) 10 | include("${ADAPTNOTCH_CMAKE_DIR}/clipper-targets.cmake") 11 | endif() 12 | 13 | # These are IMPORTED targets created by clipper-targets.cmake 14 | set(CLIPPER_LIBRARIES clipper) -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.built_by: -------------------------------------------------------------------------------- 1 | catkin build -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/.catkin: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/build/catkin_tools_prebuild -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/.rosinstall: -------------------------------------------------------------------------------- 1 | - setup-file: 2 | local-name: /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/setup.sh 3 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/cmake.lock: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/cmake.lock -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/templates/env.sh.in 3 | 4 | if [ $# -eq 0 ] ; then 5 | /bin/echo "Usage: env.sh COMMANDS" 6 | /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." 7 | exit 1 8 | fi 9 | 10 | # ensure to not use different shell type which was set before 11 | CATKIN_SHELL=sh 12 | 13 | # source setup.sh from same directory as this file 14 | _CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) 15 | . "$_CATKIN_SETUP_DIR/setup.sh" 16 | exec "$@" 17 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc: -------------------------------------------------------------------------------- 1 | prefix=/home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild 2 | 3 | Name: catkin_tools_prebuild 4 | Description: Description of catkin_tools_prebuild 5 | Version: 0.0.0 6 | Cflags: 7 | Libs: -L${prefix}/lib 8 | Requires: 9 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/local_setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/local_setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" --extend --local 9 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/local_setup.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/template/local_setup.sh.in 3 | 4 | # since this file is sourced either use the provided _CATKIN_SETUP_DIR 5 | # or fall back to the destination set at configure time 6 | : ${_CATKIN_SETUP_DIR:=/home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild} 7 | CATKIN_SETUP_UTIL_ARGS="--extend --local" 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | unset CATKIN_SETUP_UTIL_ARGS 10 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/local_setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/local_setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh" --extend --local' 9 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"' 9 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake: -------------------------------------------------------------------------------- 1 | # generated from catkin/cmake/template/pkgConfig-version.cmake.in 2 | set(PACKAGE_VERSION "0.0.0") 3 | 4 | set(PACKAGE_VERSION_EXACT False) 5 | set(PACKAGE_VERSION_COMPATIBLE False) 6 | 7 | if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") 8 | set(PACKAGE_VERSION_EXACT True) 9 | set(PACKAGE_VERSION_COMPATIBLE True) 10 | endif() 11 | 12 | if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") 13 | set(PACKAGE_VERSION_COMPATIBLE True) 14 | endif() 15 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/_setup_util.py: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/_setup_util.py -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/cmake.lock: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/cmake.lock -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/env.sh: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/env.sh -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/lib/pkgconfig/catkin_tools_prebuild.pc: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/local_setup.bash: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/local_setup.bash -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/local_setup.sh: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/local_setup.sh -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/local_setup.zsh: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/local_setup.zsh -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/setup.bash: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/setup.bash -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/setup.sh: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/setup.sh -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/setup.zsh: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/setup.zsh -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake: -------------------------------------------------------------------------------- 1 | /home/jiuzhou/clipper_semantic_object/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/examples/data/bun10k.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/clipper_semantic_object/examples/data/bun10k.ply -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/examples/data/robot0Map_indoor.txt: -------------------------------------------------------------------------------- 1 | 3 6.41903 -0.346591 0.605761 2 | 1 6.41602 2.5084 0.140431 3 | 1 7.20971 1.31182 0.000203529 4 | 1 6.77072 -3.40847 0.00187095 5 | 1 4.69451 -2.83212 0.0519033 6 | 1 2.83934 -1.87861 -0.0608559 7 | 2 5.18586 -0.662365 -0.0555485 8 | 1 3.97671 -4.72411 0.0356192 9 | 3 4.63306 -2.91857 0.964001 10 | 1 5.29141 -1.08711 -0.0355758 11 | 1 4.54943 0.650984 -0.0313159 12 | 1 2.33172 1.20601 -0.0237492 13 | 3 -3.92076 -3.48758 1.23033 14 | 1 1.53793 -4.55869 -0.0130436 15 | 3 13.8174 -5.89769 0.103694 16 | 3 11.3295 -2.64406 0.94652 17 | 1 14.0145 -2.97051 -0.0308608 18 | 1 11.7614 -13.6643 0.0114579 19 | 3 9.82803 -17.0212 1.31943 20 | 3 11.4281 -14.1887 0.35213 21 | 1 11.2235 -11.4765 0.0362473 22 | 1 6.91769 -16.5407 -0.184619 23 | 2 11.479 -12.5559 -0.0325268 24 | 1 5.85361 -11.929 -0.0556598 25 | 1 9.73723 -11.8781 -0.0566215 26 | 1 2.39288 -10.9363 0.00777467 27 | 3 5.8612 -16.9482 1.25288 28 | 1 0.570472 -7.80347 -0.0374796 29 | 1 -2.20189 -19.0989 0.378749 30 | 1 -2.04522 -17.3162 0.326958 31 | 3 -1.74581 -17.5877 0.370282 32 | 3 -1.8644 -19.5628 0.301241 33 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/examples/data/robot1Map_indoor.txt: -------------------------------------------------------------------------------- 1 | 1 4.36256 -2.86669 -0.0363254 2 | 1 3.66272 -4.21781 -0.0149842 3 | 1 5.09733 -0.964295 -0.0378865 4 | 2 3.34634 -1.33718 -0.197003 5 | 1 2.08358 5.4637 -0.0644543 6 | 1 6.73703 3.42892 -0.0397598 7 | 1 3.35749 7.61567 0.00108001 8 | 3 -0.147777 5.42552 1.3611 9 | 1 1.63495 -1.41596 0.0188353 10 | 1 2.48336 0.0664812 -0.0180247 11 | 3 3.63549 -1.67245 0.3306 12 | 1 0.611101 -4.85208 0.0226985 13 | 2 2.59122 -3.16468 -0.0720823 14 | 1 2.51721 -3.41103 -0.0158438 15 | 1 0.436468 -6.60099 0.00187924 16 | 1 -2.96265 -0.822504 -0.00824841 17 | 3 0.582904 -2.95753 0.925894 18 | 2 0.771968 -6.39815 -0.16327 19 | 1 0.585116 -8.27963 0.0521746 20 | 1 -4.94987 -12.1257 -0.0864293 21 | 1 -9.77966 -10.0189 -0.0113719 22 | 1 -8.17321 -9.31268 -0.00964654 23 | 2 -8.73956 -9.82727 -0.0358115 24 | 3 -13.004 -8.57177 1.17484 25 | 1 -12.601 -6.25474 -0.020846 26 | 1 0.805256 -12.5098 -0.0391472 27 | 1 -0.834472 0.350466 -0.0424411 28 | 1 1.02748 5.47296 0.0161506 29 | 1 2.47358 7.50933 -0.14224 30 | 1 0.674991 8.1755 -0.081143 31 | 1 2.54236 11.9787 -0.0655328 32 | 1 2.42949 10.0787 -0.00465212 33 | 3 -0.303691 8.71331 1.38179 34 | 1 0.906162 10.0148 -0.0137986 35 | 1 -4.24122 0.0234059 0.000502329 36 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/examples/data/robot2Map_indoor.txt: -------------------------------------------------------------------------------- 1 | 1 4.87694 -3.50548 0.00581943 2 | 1 6.52997 -1.44357 -0.0146996 3 | 2 5.69076 -2.22827 -0.0969788 4 | 1 2.95647 -2.94865 -0.0257299 5 | 3 4.12508 -1.94785 0.272436 6 | 1 3.31044 -0.869268 -0.0148898 7 | 3 -2.195 -5.58573 1.55165 8 | 1 -3.6781 -1.15421 -0.104992 9 | 1 -1.85847 -2.91896 -0.0407093 10 | 1 -1.18035 0.713851 -0.0308894 11 | 3 -6.29677 -5.32715 1.19462 12 | 1 -4.20375 -3.9642 -0.0935602 13 | 1 2.2139 -9.49344 -0.0121432 14 | 1 3.17959 -8.02131 0.0205073 15 | 1 12.303 -16.1166 0.00626077 16 | 3 7.64674 -18.0727 1.14465 17 | 3 10.7894 -19.0394 1.22846 18 | 1 12.9825 -27.0931 0.154325 19 | 1 11.3148 -24.0579 0.073585 20 | 1 4.86721 -23.8552 0.244067 21 | 1 16.2788 -21.5127 0.21982 22 | 1 15.5252 -5.34611 0.0199957 23 | 1 8.43794 -4.85963 0.0423192 24 | 1 4.86404 -0.386962 -0.0491706 25 | 1 0.48252 1.28017 -0.0118155 26 | 3 -1.39512 -4.43837 1.24793 27 | 1 3.36659 -5.97624 0.0912229 28 | 3 -4.56683 -3.71748 1.09683 29 | 1 -8.05273 -1.05183 -0.090258 30 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/dsd.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dsd.h 3 | * @brief Dense subgraph discovery using Goldberg's algorithm 4 | * @brief See https://github.com/MengLiuPurdue/find_densest_subgraph 5 | * @author Parker Lusk 6 | * @date 13 June 2023 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "clipper/types.h" 21 | 22 | namespace clipper { 23 | namespace dsd { 24 | 25 | /** 26 | * @brief Find the densest subgraph in an unweighted edge-weighted graph 27 | * using Goldberg's flow-based polynomial-time algorithm. This 28 | * solves the problem exactly. 29 | * 30 | * The optimization formulation is as follows 31 | * 32 | * max u' * A * u / (u' * u) 33 | * s.t. u \in {0, 1}^n 34 | * 35 | * In the graph networks community, the objective is frequently 36 | * written as f(S) = w(S) / |S|, where S is the subgraph, w(S) 37 | * is the sum of the edges in the subgraph S, and |S| is the 38 | * number of vertices in the subgraph. For example, see second 39 | * paragraph of https://arxiv.org/pdf/1809.04802.pdf. 40 | * 41 | * @param[in] A Weighted adjacency matrix of graph. Symmetric upper tri. 42 | * @param[in] S (Optional) restrict search to subgraph of A 43 | * 44 | * @return Nodes of densest subgraph 45 | */ 46 | std::vector solve(const SpAffinity& A, const std::vector& S = {}); 47 | 48 | /** 49 | * @brief Dense specialization of dsd::solve 50 | * 51 | * @param[in] A Dense weighted adjacency matrix of graph 52 | * @param[in] S (Optional) restrict search to subgraph of A 53 | * 54 | * @return Nodes of densest subgraph 55 | */ 56 | std::vector solve(const Eigen::MatrixXd& A, const std::vector& S = {}); 57 | 58 | } // ns dsd 59 | } // ns clipper -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/invariants/abstract.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file abstract.h 3 | * @brief Abstract base class for geometric invariants 4 | * @author Parker Lusk 5 | * @date 15 May 2021 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace clipper { 17 | namespace invariants { 18 | 19 | using Data = Eigen::MatrixXd; 20 | using Datum = Eigen::VectorXd; 21 | 22 | /** 23 | * @brief Abstract base clase for all invariant types. 24 | * An invariant is a function which yields the same output for 25 | * the same subset of elements in a dataset under transformation. 26 | * 27 | * For example, in R^n under SE(3), the most natural invariant is 28 | * the norm since ||q|| = ||T p||. So for two points in a dataset, 29 | * if ||qi - qj|| = ||Tpi - Tpj|| then there is a high degreee of 30 | * consistency between (qi, pi) and (qj, pj). 31 | * 32 | * The Invariant datatype implements an invariant for a specific 33 | * domain and expected transformation. Further, this datatype is 34 | * used to score consistency between data using the implemented 35 | * invariant. 36 | */ 37 | class Invariant { 38 | public: 39 | virtual ~Invariant() = default; 40 | }; 41 | 42 | using InvariantPtr = std::shared_ptr; 43 | 44 | /** 45 | * @brief A pairwise invariant uses pairs of points to score association 46 | * consistency. This class implements a real-valued invariant 47 | * scoring function that takes the form 48 | * 49 | * f : A x A x A x A -> R 50 | * 51 | * where A represents the specific domain of interest (e.g., R^n) 52 | * Note that f is not the invariant itself, but the invariant 53 | * _scoring_ function, which uses the invariant to assses 54 | * consistency. 55 | */ 56 | class PairwiseInvariant : public Invariant 57 | { 58 | public: 59 | virtual ~PairwiseInvariant() = default; 60 | 61 | /** 62 | * @brief Functor for pairwise invariant scoring function 63 | * 64 | * @param[in] ai Element i from dataset 1 65 | * @param[in] aj Element j from dataset 1 66 | * @param[in] bi Element i from dataset 2 67 | * @param[in] bj Element j from dataset 2 68 | * 69 | * @return The consistency score for the association of (ai,bi) and (aj,bj) 70 | */ 71 | virtual double operator()(const Datum& ai, const Datum& aj, const Datum& bi, const Datum& bj) = 0; 72 | }; 73 | 74 | using PairwiseInvariantPtr = std::shared_ptr; 75 | 76 | } // ns invariants 77 | } // ns clipper -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/invariants/builtins.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file builtins.h 3 | * @brief Common geometric invariants for CLIPPER 4 | * @author Parker Lusk 5 | * @date 3 October 2020 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "clipper/invariants/euclidean_distance.h" 11 | #include "clipper/invariants/pointnormal_distance.h" -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/invariants/euclidean_distance.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file euclidean_distance.h 3 | * @brief Pairwise Euclidean distance geometric invariant 4 | * @author Parker Lusk 5 | * @date 15 May 2021 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "clipper/invariants/abstract.h" 11 | 12 | namespace clipper { 13 | namespace invariants { 14 | 15 | /** 16 | * @brief Specialization of PairwiseInvariant to Euclidean distance in 17 | * the real numbers using the 2-norm as the invariant. 18 | */ 19 | class EuclideanDistance : public PairwiseInvariant 20 | { 21 | public: 22 | struct Params 23 | { 24 | double sigma = 0.01; ///< spread / "variance" of exponential kernel 25 | double epsilon = 0.06; ///< bound on consistency score, determines if inlier/outlier 26 | double mindist = 0; ///< minimum allowable distance between inlier points in the same dataset 27 | }; 28 | public: 29 | EuclideanDistance(const Params& params) 30 | : params_(params) {} 31 | ~EuclideanDistance() = default; 32 | 33 | /** 34 | * @brief Functor for pairwise invariant scoring function 35 | * 36 | * @param[in] ai Element i from dataset 1 37 | * @param[in] aj Element j from dataset 1 38 | * @param[in] bi Element i from dataset 2 39 | * @param[in] bj Element j from dataset 2 40 | * 41 | * @return The consistency score for the association of (ai,bi) and (aj,bj) 42 | */ 43 | double operator()(const Datum& ai, const Datum& aj, const Datum& bi, const Datum& bj) override; 44 | 45 | private: 46 | Params params_; 47 | }; 48 | 49 | using EuclideanDistancePtr = std::shared_ptr; 50 | 51 | } // ns invariants 52 | } // ns clipper -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/invariants/pointnormal_distance.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file pointnormal_distance.h 3 | * @brief Pairwise geometric invariant between point-normals (e.g., planes) 4 | * @author Parker Lusk 5 | * @date 15 May 2021 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "clipper/invariants/abstract.h" 11 | 12 | namespace clipper { 13 | namespace invariants { 14 | 15 | /** 16 | * @brief Specialization of PairwiseInvariant to be applied to sets 17 | * of planes, patches, or equivalently, points with normals. 18 | * 19 | * Data: 6xn matrix 20 | * Datum: 6x1 vector --> top 3x1 is point, bottom 3x1 is normal. 21 | */ 22 | class PointNormalDistance : public PairwiseInvariant 23 | { 24 | public: 25 | struct Params 26 | { 27 | double sigp = 0.5; ///< point - spread of exp kernel 28 | double epsp = 0.5; ///< point - bound on consistency score 29 | double sign = 0.10; ///< normal - spread of exp kernel 30 | double epsn = 0.35; ///< normal - bound on consistency score 31 | }; 32 | public: 33 | PointNormalDistance(const Params& params) 34 | : params_(params) {} 35 | ~PointNormalDistance() = default; 36 | 37 | /** 38 | * @brief Functor for pairwise invariant scoring function 39 | * 40 | * @param[in] ai Element i from dataset 1 41 | * @param[in] aj Element j from dataset 1 42 | * @param[in] bi Element i from dataset 2 43 | * @param[in] bj Element j from dataset 2 44 | * 45 | * @return The consistency score for the association of (ai,bi) and (aj,bj) 46 | */ 47 | double operator()(const Datum& ai, const Datum& aj, const Datum& bi, const Datum& bj) override; 48 | 49 | private: 50 | Params params_; 51 | }; 52 | 53 | using PointNormalDistancePtr = std::shared_ptr; 54 | 55 | } // ns invariants 56 | } // ns clipper -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/maxclique.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file maxclique.h 3 | * @brief Library for solving maximum clique 4 | * @author Parker Lusk 5 | * @date 20 December 2021 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace clipper { 13 | namespace maxclique { 14 | 15 | enum class Method { EXACT, HEU, KCORE }; 16 | 17 | struct Params 18 | { 19 | Method method = Method::EXACT; ///< EXACT is ROBIN*, KCORE is ROBIN 20 | size_t threads = 24; ///< num threads for OpenMP to use 21 | int time_limit = 3600; ///< [s], maximum time allotted for MCP solving 22 | bool verbose = false; 23 | }; 24 | 25 | std::vector solve(const Eigen::MatrixXd& A, const Params& params = {}); 26 | 27 | } // ns maxclique 28 | } // ns clipper 29 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/sdp.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file sdp.h 3 | * @brief Problem parser for CLIPPER SDR optimization 4 | * @author Parker Lusk 5 | * @date 23 Nov 2021 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace clipper { 13 | namespace sdp { 14 | 15 | struct Solution 16 | { 17 | Eigen::MatrixXd X; 18 | Eigen::VectorXd lambdas; 19 | Eigen::VectorXd evec1; 20 | 21 | double thr; ///< threshold for selecting nodes 22 | std::vector nodes; ///< indices of selected nodes 23 | 24 | int iters; ///< number of iterations 25 | float pobj; ///< primal objective value 26 | float dobj; ///< dual objective value 27 | 28 | double t; ///< total tile: parsing, solving, extraction 29 | double t_parse; ///< time spent setting up the problem data 30 | double t_scs; ///< total SCS time 31 | double t_scs_setup; ///< SCS setup time 32 | double t_scs_solve; ///< SCS solve time 33 | double t_scs_linsys; ///< SCS total time spent in the linear system solver 34 | double t_scs_cone; ///< total time spent on cone projections 35 | double t_scs_accel; ///< total time spent in the accel routine 36 | double t_extract; ///< time spent extracting which nodes to select 37 | }; 38 | 39 | struct Params 40 | { 41 | bool verbose = false; 42 | 43 | int max_iters = 2000; 44 | int acceleration_interval = 10; 45 | int acceleration_lookback = 10; 46 | 47 | float eps_abs = 1e-3; 48 | float eps_rel = 1e-3; 49 | float eps_infeas = 1e-7; 50 | 51 | float time_limit_secs = 0; 52 | }; 53 | 54 | Solution solve(const Eigen::MatrixXd& M, const Eigen::MatrixXd& C, 55 | const Params& params = Params{}); 56 | 57 | } // ns sdp 58 | } // ns clipper -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/clipper/types.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file types.h 3 | * @brief Types used in CLIPPER framework 4 | * @author Parker Lusk 5 | * @date 19 March 2022 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace clipper { 14 | 15 | using SpMat = Eigen::SparseMatrix; 16 | using SpTriplet = Eigen::Triplet; 17 | 18 | using Association = Eigen::Matrix; 19 | using Affinity = Eigen::MatrixXd; 20 | using Constraint = Eigen::MatrixXd; 21 | 22 | using SpAffinity = SpMat; 23 | using SpConstraint = SpMat; 24 | 25 | } // ns clipper -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/semantic_clipper.h: -------------------------------------------------------------------------------- 1 | #ifndef SEMANTIC_CLIPPER_H 2 | #define SEMANTIC_CLIPPER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "clipper/clipper.h" 14 | #include "clipper/utils.h" 15 | #include "triangulation/observation.hpp" 16 | #include 17 | 18 | // namespace Eigen { 19 | // typedef Matrix Vector7d; 20 | // } 21 | 22 | namespace semantic_clipper{ 23 | 24 | Eigen::Matrix2Xd read_2d_points(std::string txt_file); 25 | 26 | Eigen::Matrix2Xd transform_2d_points(Eigen::Matrix2Xd points, Eigen::Matrix2d rotation, Eigen::Vector2d translation); 27 | 28 | std::vector argsort(const std::vector& v); 29 | 30 | void compute_triangle_diff(const DelaunayTriangulation::Polygon& triangle_model, const DelaunayTriangulation::Polygon& triangle_data, std::vector& diffs, std::vector>& matched_points_model, std::vector>& matched_points_data, double threshold); 31 | 32 | void match_triangles(const std::vector& triangles_model, const std::vector& triangles_data, std::vector& diffs, std::vector>& matched_points_model, std::vector>& matched_points_data, double threshold); 33 | 34 | void clipper_data_association(const Eigen::Matrix2Xd& matched_points_model, const Eigen::Matrix2Xd& matched_points_data, double threshold, clipper::Association& A); 35 | 36 | void clipper_semantic_object(const Eigen::Matrix2Xd& model, const Eigen::Matrix2Xd& data, double threshold, clipper::Association& A); 37 | 38 | bool run_semantic_clipper(const std::vector>& reference_map, const std::vector>& query_map, Eigen::Matrix4d& tfFromQuery2Ref, double sigma, double epsilon, int min_num_pairs, double matching_threshold); 39 | } 40 | 41 | #endif // SEMANTIC_CLIPPER_H -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/triangulation/descriptor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace descriptor{ 6 | // Samples points around the perimeter of a polygon and returns a new set of points. 7 | // The size of the returned PointVector depends on step. 8 | // Smaller steps means more points will be sampled, 9 | // e.g. step=0.05 means a point every 5% of the perimeter, resulting in 20 points. 10 | PointVector samplePoints(PointVector points, double step); 11 | // Uses sampled points to compute a centroid distance descriptor 12 | std::vector centroidDist(PointVector points, PointVector sampledPoints); 13 | // Uses OpenCV to compute the magnitude of the DFT of the centroid descriptor 14 | std::vector invariantFourier(std::vector centroidDesc); 15 | // Runs the entire descriptor computation pipeline 16 | std::vector compute(PointVector points); 17 | } -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/triangulation/distance.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using vecPtT = std::vector; 12 | using EdgeT = std::pair; 13 | using PointVector = std::vector; 14 | 15 | inline float pow_2(const float &x) { return x * x; } 16 | 17 | inline double euclideanDistance(std::vector A, std::vector B) 18 | { 19 | double d = 0; 20 | for (size_t i = 0; i < A.size(); ++i) 21 | { 22 | d += pow_2(A[i] - B[i]); 23 | } 24 | return std::sqrt(d); 25 | } 26 | 27 | inline double euclideanDistance2D(std::vector A, std::vector B) 28 | { 29 | double d = 0; 30 | for (size_t i = 0; i < 2; ++i) 31 | { 32 | d += pow_2(A[i] - B[i]); 33 | } 34 | return std::sqrt(d); 35 | } 36 | 37 | inline size_t cantorPairing(size_t a, size_t b) 38 | { 39 | // a is always the smallest number 40 | size_t aux = a; 41 | if(b < a) 42 | { 43 | a = b; 44 | b = aux; 45 | } 46 | return (a + b) * (a + b + 1) / 2 + a; 47 | } 48 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/triangulation/observation.hpp: -------------------------------------------------------------------------------- 1 | // the code is adapted from https://github.com/gnardari/urquhart 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "libqhullcpp/RboxPoints.h" 9 | #include "libqhullcpp/QhullError.h" 10 | #include "libqhullcpp/QhullQh.h" 11 | #include "libqhullcpp/QhullFacet.h" 12 | #include "libqhullcpp/QhullFacetList.h" 13 | #include "libqhullcpp/QhullFacetSet.h" 14 | #include "libqhullcpp/QhullLinkedList.h" 15 | #include "libqhullcpp/QhullPoint.h" 16 | #include "libqhullcpp/QhullVertex.h" 17 | #include "libqhullcpp/QhullVertexSet.h" 18 | #include "libqhullcpp/Qhull.h" 19 | 20 | namespace DelaunayTriangulation{ 21 | class Observation { 22 | public: 23 | explicit Observation(PointVector& landmarks); 24 | // Computes a Delaunay triangulation using QHull from a set of landmarks. 25 | void delaunayTriangulation(PointVector& points, std::vector& polygons); 26 | std::vector triangles; 27 | private: 28 | PointVector landmarks; 29 | 30 | // // Uses the triangles of the delaunay triangulation to compute an urquhart tessellation 31 | // void urquhartTesselation_(); 32 | 33 | // std::vector::iterator findEdge_(Polygon& x, EdgeT commonEdge); 34 | // // Merges two polygons into a new one, this is used inside the urquhart tessellation computation 35 | // // p will be merged with n, which shares the longest edge of p, indexed (relative to p) by commonEdgeIdx 36 | // Polygon combinePolygonData_(const Polygon& p, const Polygon& n); 37 | // Polygon mergePolygons_(Polygon& p, Polygon& n, EdgeT commonEdge); 38 | }; 39 | } -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/include/triangulation/polygon.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace DelaunayTriangulation { 6 | class Polygon { 7 | public: 8 | explicit Polygon(){ 9 | points = {}; 10 | neighbors = {}; 11 | edges = {}; 12 | edgeLengths = {}; 13 | descriptor = {}; 14 | } 15 | 16 | explicit Polygon(PointVector pp, std::vector nn, 17 | std::vector ee, std::vector el){ 18 | points = pp; 19 | neighbors = nn; 20 | edges = ee; 21 | edgeLengths = el; 22 | descriptor = descriptor::compute(pp); 23 | // std::cout << "Creating new polygon\n Neighbors:" << std::endl; 24 | // for(auto n : neighbors){ 25 | // std::cout << n << ", "; 26 | // } 27 | // std::cout << std::endl; 28 | // for(auto i = 0; i < ee.size(); ++i){ 29 | // std::cout << "Len: " << edgeLengths[i] << "| " << ee[i].first << " -> " << ee[i].second << std::endl; 30 | // } 31 | } 32 | 33 | // Puts the element in idx at the first position of each vector. Used during merging operations. 34 | void rotate(const size_t idx){ 35 | std::rotate(neighbors.begin(), neighbors.begin()+idx, neighbors.end()); 36 | std::rotate(points.begin(), points.begin()+idx, points.end()); 37 | std::rotate(edges.begin(), edges.begin()+idx, edges.end()); 38 | std::rotate(edgeLengths.begin(), edgeLengths.begin()+idx, edgeLengths.end()); 39 | } 40 | 41 | PointVector points; 42 | std::vector neighbors; 43 | // edges (x,y), undirected and their respective lengths 44 | std::vector edges; 45 | std::vector edgeLengths; 46 | std::vector descriptor; 47 | }; 48 | } -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/src/invariants/euclidean_distance.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file euclidean_distance.cpp 3 | * @brief Pairwise Euclidean distance geometric invariant 4 | * @author Parker Lusk 5 | * @date 15 May 2021 6 | */ 7 | 8 | #include "clipper/invariants/euclidean_distance.h" 9 | 10 | namespace clipper { 11 | namespace invariants { 12 | 13 | double EuclideanDistance::operator()(const Datum& ai, const Datum& aj, 14 | const Datum& bi, const Datum& bj) 15 | { 16 | 17 | // distance between two points in the same cloud 18 | const double l1 = (ai - aj).norm(); 19 | const double l2 = (bi - bj).norm(); 20 | 21 | // enforce minimum distance criterion -- if points in the same dataset 22 | // are too close, then this pair of associations cannot be selected 23 | if (params_.mindist > 0 && (l1 < params_.mindist || l2 < params_.mindist)) { 24 | return 0.0; 25 | } 26 | 27 | // consistency score 28 | const double c = std::abs(l1 - l2); 29 | 30 | return (c 5 | * @date 15 May 2021 6 | */ 7 | 8 | #include "clipper/invariants/pointnormal_distance.h" 9 | 10 | namespace clipper { 11 | namespace invariants { 12 | 13 | double PointNormalDistance::operator()(const Datum& ai, const Datum& aj, 14 | const Datum& bi, const Datum& bj) 15 | { 16 | // point distance 17 | const double l1 = (ai.head<3>() - aj.head<3>()).norm(); 18 | const double l2 = (bi.head<3>() - bj.head<3>()).norm(); 19 | 20 | // normal distance 21 | const double alpha1 = std::acos(ai.tail<3>().transpose() * aj.tail<3>()); 22 | const double alpha2 = std::acos(bi.tail<3>().transpose() * bj.tail<3>()); 23 | 24 | // check consistency 25 | const double dp = std::abs(l1 - l2); 26 | const double dn = std::abs(alpha1 - alpha2); 27 | 28 | if (dp < params_.epsp && dn < params_.epsn) { 29 | const double sp = std::exp(-0.5*dp*dp/(params_.sigp*params_.sigp)); 30 | const double sn = std::exp(-0.5*dn*dn/(params_.sign*params_.sign)); 31 | return sp * sn; 32 | } else { 33 | return 0.0; 34 | } 35 | } 36 | 37 | } // ns invariants 38 | } // ns clipper 39 | -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(GoogleTest) 2 | 3 | add_executable(tests main.cpp affinity_test.cpp clipper_test.cpp sdp_test.cpp dsd_test.cpp) 4 | target_link_libraries(tests clipper gtest pthread Eigen3::Eigen) 5 | gtest_add_tests(TARGET tests TEST_LIST all_tests) 6 | set_tests_properties(${all_tests} PROPERTIES TIMEOUT 10) -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/test/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @brief Entry point for all CLIPPER test suites 4 | * @author Parker Lusk 5 | * @date 19 July 2020 6 | */ 7 | 8 | #include 9 | 10 | int main(int argc, char** argv) { 11 | testing::InitGoogleTest(&argc, argv); 12 | return RUN_ALL_TESTS(); 13 | } -------------------------------------------------------------------------------- /backend/sloam/clipper_semantic_object/test/test_semantic_clipper.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_clipper.h" 2 | 3 | int main(){ 4 | /* 5 | Data preparation 6 | */ 7 | // read from data file 1 8 | std::string data1_file_name = "/home/jiuzhou/clipper_semantic_object/examples/data/robot1Map_forest_2d.txt"; 9 | Eigen::Matrix2Xd model = semantic_clipper::read_2d_points(data1_file_name); 10 | // convert to the format of run_semantic_clipper argument which is vector of vector 11 | std::vector> model_points; 12 | for (int i = 0; i < model.cols(); i++) { 13 | model_points.push_back({0, model(0, i), model(1, i), 0, 0, 0, 0, 0}); 14 | } 15 | 16 | // read from data file 2 17 | std::string data2_file_name = "/home/jiuzhou/clipper_semantic_object/examples/data/robot2Map_forest_2d.txt"; 18 | Eigen::Matrix2Xd data = semantic_clipper::read_2d_points(data2_file_name); 19 | // convert to the format of run_semantic_clipper argument which is vector of vector 20 | std::vector> data_points; 21 | 22 | for (int i = 0; i < data.cols(); i++) { 23 | data_points.push_back({0, data(0, i), data(1, i), 0, 0, 0, 0, 0}); 24 | } 25 | 26 | // run semantic clipper 27 | Eigen::Matrix4d tfFromQuery2Ref; 28 | double sigma = 0.1; 29 | double epsilon = 0.3; 30 | int min_num_pairs = 3; 31 | double matching_threshold = 0.1; 32 | bool found = semantic_clipper::run_semantic_clipper(model_points, data_points, tfFromQuery2Ref, sigma, epsilon, min_num_pairs, matching_threshold); 33 | 34 | // print the transformation 35 | std::cout << "Transformation matrix: " << std::endl; 36 | std::cout << tfFromQuery2Ref << std::endl; 37 | return 0; 38 | } -------------------------------------------------------------------------------- /backend/sloam/include/core/cubeMapManager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | class CubeMapManager { 18 | public: 19 | explicit CubeMapManager(); 20 | // get map for visualization 21 | std::vector getFinalMap(const int &num_min_observations = 3); 22 | const std::vector &getConstMap() const; 23 | std::vector &getRawMap(); 24 | std::map getMatchesMap() const; 25 | void getSubmap(const SE3 &pose, std::vector &submap); 26 | 27 | void updateMap(const SE3 &pose, std::vector &obs_tms, 28 | const std::vector &matches, const int &robotID); 29 | void getkeyPoseSubmap(const SE3 &pose, std::vector &submap, 30 | double submap_radius, const int robotID); 31 | 32 | private: 33 | // landmarks_ variable only records the 3D positions of semantic landmarks for 34 | // purposes such as to get a sub map 35 | CloudT::Ptr cube_landmarks_; 36 | std::vector cube_hits_; 37 | std::vector cube_models_; 38 | std::map cubeMatchesMap_; 39 | }; -------------------------------------------------------------------------------- /backend/sloam/include/core/cylinderMapManager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | struct KeyFrames { 20 | std::vector poses; 21 | std::vector> landmark_idxs; 22 | }; 23 | 24 | class CylinderMapManager { 25 | public: 26 | explicit CylinderMapManager(int num_of_robots = 1, 27 | const float searchRadius = 50); 28 | std::vector getFinalMap(const int &num_min_observations = 3); 29 | const std::vector &getConstMap() const; 30 | std::vector &getRawMap(); 31 | std::map getMatchesMap() const; 32 | void getSubmap(const SE3 &pose, std::vector &submap); 33 | const std::vector &getTrajectory(const int &robotID) const; 34 | void updateMap(const SE3 &pose, std::vector &obs_tms, 35 | const std::vector &matches, const int &robotID); 36 | int getLatestPoseIdx(const int robotID); 37 | SE3 getPose(const size_t idx, const int robotID); 38 | void getkeyPoseSubmap(const SE3 &pose, std::vector &submap, 39 | double submap_radius, const int robotID); 40 | // TODO: merge these two functions 41 | bool getLoopCandidateIdx(const double max_dist, const size_t poseIdx, 42 | size_t &candidateIdx, const int robotID, 43 | const size_t &at_least_num_of_poses_old = 50); 44 | bool InLoopClosureRegion(const double &max_dist_xy, const double &max_dist_z, 45 | const SE3 &inputPose, const int robotID, 46 | const size_t &at_least_num_of_poses_old); 47 | 48 | private: 49 | int numRobots; 50 | float sqSearchRadius; 51 | CloudT::Ptr landmarks_; 52 | std::vector robotPoseCloud_; 53 | 54 | std::vector treeModels_; 55 | std::map matchesMap_; 56 | std::vector treeHits_; 57 | std::vector robotKeyFrames_; 58 | }; -------------------------------------------------------------------------------- /backend/sloam/include/core/ellipsoidMapManager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | class EllipsoidMapManager { 18 | public: 19 | explicit EllipsoidMapManager(); 20 | std::vector getFinalMap(const int &num_min_observations = 3); 21 | const std::vector &getConstMap() const; 22 | std::vector &getRawMap(); 23 | std::map getMatchesMap() const; 24 | void getSubmap(const SE3 &pose, std::vector &submap); 25 | void updateMap(const SE3 &pose, std::vector &obs_tms, 26 | const std::vector &matches, const int &robotID); 27 | void getkeyPoseSubmap(const SE3 &pose, std::vector &submap, 28 | double submap_radius, const int robotID); 29 | 30 | private: 31 | // landmarks_ variable only records the 3D positions of semantic landmarks for 32 | // purposes such as get sub map 33 | CloudT::Ptr ellipsoid_landmarks_; 34 | std::vector ellipsoid_hits_; 35 | std::vector ellipsoid_models_; 36 | std::map ellipsoidMatchesMap_; 37 | }; -------------------------------------------------------------------------------- /backend/sloam/include/core/sloamNodelet.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Guilherme Nardari, Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | 15 | namespace sloam { 16 | class SLOAMNodelet : public nodelet::Nodelet { 17 | public: 18 | SLOAMNodelet() {} 19 | ~SLOAMNodelet() {} 20 | virtual void onInit(); 21 | 22 | private: 23 | SLOAMNode::Ptr sloamNode; 24 | }; 25 | } // namespace sloam 26 | -------------------------------------------------------------------------------- /backend/sloam/include/helpers/definitions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // #include 4 | #include /* isnan, sqrt */ 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #define PIDEF 3.14159265 26 | 27 | using namespace Eigen; 28 | using namespace boost; 29 | 30 | using SE3 = Sophus::SE3d; 31 | using SO3 = Sophus::SO3d; 32 | using Matrix3 = Matrix3d; 33 | using Matrix4 = Matrix4d; 34 | using MatrixX = MatrixXd; 35 | using VectorX = VectorXd; 36 | using Vector4 = Vector4d; 37 | using Vector3 = Vector3d; 38 | using Affine3 = Affine3d; 39 | using PointT = pcl::PointXYZI; 40 | using CloudT = pcl::PointCloud; 41 | using KDTree = pcl::KdTreeFLANN; 42 | using Quat = Quaterniond; 43 | using Tran = Translation3d; 44 | 45 | typedef std::vector> VectorType; 46 | typedef std::vector> SE3VectorType; 47 | using Slash = VectorType; 48 | using Scalar = double; 49 | 50 | /* 51 | * --------------- Trellis Structures ------------------ 52 | */ 53 | struct TreeVertex { 54 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | int treeId; 56 | int beam; 57 | int prevVertexSize; 58 | Scalar radius; 59 | bool isValid; 60 | PointT coords; 61 | Slash points; 62 | }; 63 | 64 | using graph_t = adjacency_list>; 66 | using vertex_t = graph_traits::vertex_descriptor; 67 | using edge_t = graph_traits::edge_descriptor; 68 | 69 | /* 70 | * --------------- Param Structures ------------------ 71 | */ 72 | struct FeatureModelParams { 73 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 74 | 75 | int scansPerSweep; 76 | 77 | Scalar minGroundModels; 78 | Scalar maxLidarDist; 79 | Scalar maxGroundLidarDist; 80 | Scalar minGroundLidarDist; 81 | bool twoStepOptim; 82 | 83 | int groundRadiiBins; 84 | int groundThetaBins; 85 | Scalar groundRetainThresh; 86 | Scalar groundMatchThresh; 87 | Scalar cylinderMatchThresh; 88 | Scalar cuboidMatchThresh; 89 | Scalar ellipsoidMatchThresh; 90 | Scalar maxTreeRadius; 91 | Scalar maxAxisTheta; 92 | Scalar maxFocusOutlierDistance; 93 | 94 | int featuresPerTree; 95 | int numGroundFeatures; 96 | 97 | Scalar defaultCylinderRadius; 98 | }; 99 | -------------------------------------------------------------------------------- /backend/sloam/include/helpers/serialization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace boost { 10 | namespace serialization { 11 | 12 | template 13 | void serialize(Archive &ar, PointT &g, const unsigned int version) { 14 | ar &g.getVector3fMap().data()[0]; 15 | ar &g.getVector3fMap().data()[1]; 16 | ar &g.getVector3fMap().data()[2]; 17 | } 18 | 19 | template 20 | void serialize(Archive &ar, TreeVertex &vtx, const unsigned int version) { 21 | ar &vtx.treeId; 22 | ar &vtx.beam; 23 | ar &vtx.prevVertexSize; 24 | ar &vtx.radius; 25 | ar &vtx.isValid; 26 | ar &vtx.coords; 27 | ar &vtx.points; 28 | } 29 | 30 | } // namespace serialization 31 | } // namespace boost -------------------------------------------------------------------------------- /backend/sloam/include/objects/cube.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | // Cube object stores information about cuboid landmarks in the semantic map 19 | struct CubeParameters { 20 | gtsam::Pose3 pose; 21 | gtsam::Point3 scale; 22 | int semantic_label; 23 | }; 24 | 25 | class Cube : public SemanticObject { 26 | public: 27 | explicit Cube(const gtsam::Pose3 &pose, const gtsam::Point3 &scale, 28 | const int &label = -2); 29 | 30 | Scalar distance(const CubeParameters &model) const; 31 | Scalar distance(const PointT &point) const; 32 | // tf means tf_sensor_to_map, not tf_map_to_sensor 33 | void project(const SE3 &tf); 34 | Cube projectAndReturn(const SE3 &tf); 35 | Scalar weightedDistance(const CubeParameters &model, double dim_weight= 0.0) const; 36 | Scalar IoU(const CubeParameters &model) const; 37 | }; 38 | -------------------------------------------------------------------------------- /backend/sloam/include/objects/ellipsoid.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | struct EllipsoidParameters { 22 | gtsam::Pose3 pose; 23 | gtsam::Point3 scale; // x y z 24 | int semantic_label; 25 | }; 26 | 27 | class Ellipsoid : public SemanticObject { 28 | public: 29 | explicit Ellipsoid(const gtsam::Pose3 &pose, const gtsam::Point3 &scale, 30 | const int &label = 1); 31 | 32 | Scalar distance(const EllipsoidParameters &model) const; 33 | Scalar distance(const PointT &point) const; 34 | // tf means tf_sensor_to_map, not tf_map_to_sensor 35 | void project(const SE3 &tf); 36 | Ellipsoid projectAndReturn(const SE3 &tf); 37 | Scalar weightedDistance(const EllipsoidParameters &model, 38 | double dim_weight = 0.0) const; 39 | }; 40 | -------------------------------------------------------------------------------- /backend/sloam/include/objects/semanticObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Guilherme Nardari, Xu Liu, Jiuzhou Lei, Ankit Prabhu, 5 | * Yuezhan Tao 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | template 15 | class SemanticObject { 16 | public: 17 | // pure virtual function 18 | virtual Scalar distance(const T& model) const = 0; 19 | virtual Scalar distance(const PointT& point) const = 0; 20 | virtual void project(const SE3& tf) = 0; 21 | T getModel() const { return model; }; 22 | VectorType getFeatures() const { return features; }; 23 | size_t id; 24 | bool isValid; 25 | VectorType features; 26 | T model; // object model 27 | }; 28 | 29 | template 30 | void projectObjects(const SE3& tf, std::vector& objs) { 31 | for (T& o : objs) o.project(tf); 32 | } 33 | 34 | template 35 | void projectObjects(const SE3& tf, const std::vector& objs, 36 | std::vector& projected) { 37 | for (T o : objs) { 38 | o.project(tf); 39 | projected.push_back(o); 40 | } 41 | } -------------------------------------------------------------------------------- /backend/sloam/launch/decentralized_sloam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /backend/sloam/launch/decentralized_sloam_indoor_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /backend/sloam/launch/decentralized_sloam_multi_robot_dcist.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /backend/sloam/launch/decentralized_sloam_multi_robot_f250_scarab.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /backend/sloam/launch/decentralized_sloam_multi_robot_forest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /backend/sloam/launch/play_bag_indoor_scarab.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /backend/sloam/launch/play_multi_robot_bags_with_remapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /backend/sloam/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /backend/sloam/launch/run_indoor_large_scale_exploration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | -------------------------------------------------------------------------------- /backend/sloam/launch/run_pennovation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /backend/sloam/launch/run_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /backend/sloam/launch/segmentation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /backend/sloam/launch/single_robot_sloam_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /backend/sloam/launch/single_robot_sloam_test_LiDAR.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /backend/sloam/launch/single_robot_sloam_test_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /backend/sloam/launch/sloam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /backend/sloam/launch/sloam.launch.debug: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /backend/sloam/launch/sloam_bw_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /backend/sloam/launch/unit_tests/place_recognition_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /backend/sloam/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ODOM SLOAM 5 | 6 | 7 | 8 | 9 | MAP SLOAM 10 | 11 | 12 | 13 | 14 | SLOAM 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /backend/sloam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sloam 4 | 0.0.0 5 | The SLOAM package 6 | 7 | Guilherme Nardari 8 | Xu Liu 9 | Ankit Prabhu 10 | Jiuzhou Lei 11 | Yuezhan Tao 12 | 13 | Xu Liu 14 | Ankit Prabhu 15 | Jiuzhou Lei 16 | 17 | TODO 18 | 19 | catkin 20 | message_generation 21 | message_runtime 22 | roscpp 23 | Eigen 24 | sensor_msgs 25 | geometry_msgs 26 | std_msgs 27 | sloam_msgs 28 | nodelet 29 | tf2 30 | tf2_ros 31 | tf2_eigen 32 | tf2_geometry_msgs 33 | tf 34 | pcl_ros 35 | pcl_conversions 36 | loop_closure 37 | actionlib 38 | actionlib_msgs 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /backend/sloam/resource/cabinet.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/cabinet.stl -------------------------------------------------------------------------------- /backend/sloam/resource/chair.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/chair.stl -------------------------------------------------------------------------------- /backend/sloam/resource/combine_cad_mode.py: -------------------------------------------------------------------------------- 1 | from stl import mesh 2 | import numpy as np 3 | import glob 4 | 5 | # Load the meshes from the STL files 6 | # load all files in urrent directory 7 | files = glob.glob("*.stl") 8 | meshes = [mesh.Mesh.from_file(f) for f in files] 9 | 10 | # Create a new mesh that contains all the other meshes 11 | combined_mesh_data = np.concatenate([m.data for m in meshes]) 12 | 13 | combined_mesh = mesh.Mesh(combined_mesh_data) 14 | 15 | # Save the combined mesh to a new file 16 | combined_mesh.save('combined.stl') 17 | 18 | -------------------------------------------------------------------------------- /backend/sloam/resource/door.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/door.stl -------------------------------------------------------------------------------- /backend/sloam/resource/fridge.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/fridge.stl -------------------------------------------------------------------------------- /backend/sloam/resource/microwave.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/microwave.stl -------------------------------------------------------------------------------- /backend/sloam/resource/monitor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/monitor.stl -------------------------------------------------------------------------------- /backend/sloam/resource/move_chair.py: -------------------------------------------------------------------------------- 1 | 2 | import open3d as o3d 3 | 4 | # load chair.stl and translate along y -1.0 m 5 | chair = o3d.io.read_triangle_mesh("chair_input.stl") 6 | # translate to 0,0,0 7 | chair.translate([-chair.get_center()[0], -chair.get_center()[1], -chair.get_center()[2]]) 8 | # visualize origin frame 9 | mesh = o3d.geometry.TriangleMesh.create_coordinate_frame() 10 | o3d.visualization.draw_geometries([chair, mesh], mesh_show_wireframe=True, mesh_show_back_face=True) 11 | # the new chair using write_triangle_mesh as .stl 12 | # compute normals first 13 | chair.compute_vertex_normals() 14 | o3d.io.write_triangle_mesh("chair.stl", chair) -------------------------------------------------------------------------------- /backend/sloam/resource/plant.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/plant.stl -------------------------------------------------------------------------------- /backend/sloam/resource/robot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/robot.stl -------------------------------------------------------------------------------- /backend/sloam/resource/rotate_door.py: -------------------------------------------------------------------------------- 1 | 2 | import open3d as o3d 3 | 4 | # load object.stl and translate along y -1.0 m 5 | object = o3d.io.read_triangle_mesh("door_input.stl") 6 | # translate to 0,0,0 7 | object.translate([-object.get_center()[0], -object.get_center()[1], -object.get_center()[2]]) 8 | # rotate 90 degrees to face up 9 | R = object.get_rotation_matrix_from_xyz((0, 1.57, 0)) 10 | object.rotate(R, center=(0, 0, 0)) 11 | 12 | # visualize origin frame 13 | mesh = o3d.geometry.TriangleMesh.create_coordinate_frame() 14 | o3d.visualization.draw_geometries([object, mesh], mesh_show_wireframe=True, mesh_show_back_face=True) 15 | # the new object using write_triangle_mesh as .stl 16 | # compute normals first 17 | object.compute_vertex_normals() 18 | o3d.io.write_triangle_mesh("door.stl", object) -------------------------------------------------------------------------------- /backend/sloam/resource/scale_object.py: -------------------------------------------------------------------------------- 1 | 2 | import open3d as o3d 3 | 4 | # fname = "trash_can" 5 | # fname = "cabinet" 6 | fname = "robot" 7 | # fname = "door" 8 | # load object.stl and translate along y -1.0 m 9 | object = o3d.io.read_triangle_mesh(fname+"_input.stl") 10 | # translate to 0,0,0 11 | object = object.translate([-object.get_center()[0], -object.get_center()[1], -object.get_center()[2]]) 12 | # scale 13 | # object = object.scale(5, center=(0, 0, 0)) 14 | # scale to 0.25 15 | # object = object.scale(0.25, center=(0, 0, 0)) 16 | # scale to 0.6 17 | object = object.scale(0.5, center=(0, 0, 0)) 18 | 19 | # visualize origin frame 20 | mesh = o3d.geometry.TriangleMesh.create_coordinate_frame() 21 | o3d.visualization.draw_geometries([object, mesh], mesh_show_wireframe=True, mesh_show_back_face=True) 22 | # the new object using write_triangle_mesh as .stl 23 | # compute normals first 24 | object.compute_vertex_normals() 25 | o3d.io.write_triangle_mesh(fname+".stl", object) -------------------------------------------------------------------------------- /backend/sloam/resource/table2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/table2.stl -------------------------------------------------------------------------------- /backend/sloam/resource/trash_can.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/backend/sloam/resource/trash_can.stl -------------------------------------------------------------------------------- /backend/sloam/src/core/sloamNodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace sloam { 5 | void SLOAMNodelet::onInit() { 6 | // sloam.reset(new SLOAM(getMTPrivateNodeHandle())); 7 | sloamNode.reset(new SLOAMNode(getPrivateNodeHandle())); 8 | ROS_INFO("Created Sloam Nodelet"); 9 | } 10 | 11 | } // namespace sloam 12 | 13 | PLUGINLIB_EXPORT_CLASS(sloam::SLOAMNodelet, nodelet::Nodelet); 14 | -------------------------------------------------------------------------------- /backend/sloam/src/factorgraph/cubeFactor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | 15 | namespace gtsam_cube { 16 | 17 | gtsam::Vector CubeFactor::evaluateError( 18 | const gtsam::Pose3 &p, const CubeMeasurement &cube_lmrk, 19 | boost::optional H1, 20 | boost::optional H2) const { 21 | // TODO(xu:) implement analytic Jacobian for cube factors 22 | 23 | // (1) our measurement error has to be expressed in the map frame to 24 | // get it working properly, and 25 | // (2)the error is h(x) - z, i.e., 26 | // the error = cube^map_landmark - cube^map_measure, i.e., log map of: 27 | // cube^map_measure.inverse()*(tf_sensor_to_map*cube^sensor_landmark) 28 | 29 | // p is tf_sensor_to_map, not tf_map_to_sensor 30 | // m_ is the cube measurement in the sensor frame, cube^sensor_measure 31 | // cube_lmrk is the cube landmark in the map frame, cube^map_landmark 32 | // The first 6 term in our error is the SE3 error 33 | 34 | // all errors are in map frame, not in the sensor frame 35 | gtsam::Vector9 error = m_.project(p).localCoordinates(cube_lmrk); 36 | 37 | boost::function 38 | funPtr(boost::bind(&CubeFactor::evaluateError, this, _1, _2, boost::none, 39 | boost::none)); 40 | // Jacobian of error wrt pose 41 | if (H1) { 42 | Eigen::Matrix de_dx = 43 | gtsam::numericalDerivative21(funPtr, p, cube_lmrk, 1e-6); 44 | *H1 = de_dx; 45 | } 46 | // Jacobian of error wrt cube measurement 47 | if (H2) { 48 | Eigen::Matrix de_dc = 49 | gtsam::numericalDerivative22(funPtr, p, cube_lmrk, 1e-6); 50 | *H2 = de_dc; 51 | } 52 | return error; 53 | } 54 | } // namespace gtsam_cube -------------------------------------------------------------------------------- /backend/sloam/src/objects/ellipsoid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of SlideSLAM 3 | * 4 | * Copyright (C) 2024 Xu Liu, Jiuzhou Lei, Ankit Prabhu, Yuezhan Tao, Guilherme Nardari 5 | * 6 | * TODO: License information 7 | * 8 | */ 9 | 10 | #include 11 | 12 | Ellipsoid::Ellipsoid(const gtsam::Pose3 &pose, const gtsam::Point3 &scale, 13 | const int &label) { 14 | // simplified assumption: pose will be centroid + upright orientation (aligned 15 | // with world frame z-axis) 16 | model.pose = pose; 17 | // scale will contain x (length), y (width), z(height) 18 | // simplified assumption: scale x = scale y since we do not use the 19 | // orientation in the factor graph 20 | model.scale = scale; 21 | model.semantic_label = label; 22 | } 23 | 24 | Scalar Ellipsoid::distance(const EllipsoidParameters &input) const { 25 | return (input.pose.translation() - model.pose.translation()).norm(); 26 | } 27 | 28 | Scalar Ellipsoid::distance(const PointT &point) const { 29 | gtsam::Point3 point_vec = gtsam::Point3(point.x, point.y, point.z); 30 | return (model.pose.translation() - point_vec).norm(); 31 | } 32 | 33 | void Ellipsoid::project(const SE3 &tf) { 34 | // tf means tf_sensor_to_map, not tf_map_to_sensor 35 | gtsam::Pose3 pose_new = gtsam::Pose3(tf.matrix()); 36 | model.pose = pose_new * model.pose; 37 | model.scale = model.scale; 38 | } 39 | 40 | Ellipsoid Ellipsoid::projectAndReturn(const SE3 &tf) { 41 | gtsam::Pose3 pose_new = gtsam::Pose3(tf.matrix()); 42 | gtsam::Pose3 newPose = pose_new * model.pose; 43 | gtsam::Point3 newScale = model.scale; 44 | int newLabel = model.semantic_label; 45 | return Ellipsoid(newPose, newScale, newLabel); 46 | } 47 | 48 | Scalar Ellipsoid::weightedDistance(const EllipsoidParameters &input, 49 | double dim_weight) const { 50 | if (input.semantic_label != model.semantic_label) { 51 | return 1000; 52 | } 53 | 54 | Scalar position_similarity = 55 | (input.pose.translation() - model.pose.translation()).norm(); 56 | Scalar scale_similarity = (input.scale - model.scale).norm() / 3.0; 57 | 58 | Scalar weighted_distance = 59 | (1 - dim_weight) * position_similarity + dim_weight * scale_similarity; 60 | return weighted_distance; 61 | } -------------------------------------------------------------------------------- /backend/sloam/src/tests/plane_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "gtest/gtest.h" 8 | using ::testing::EmptyTestEventListener; 9 | using ::testing::InitGoogleTest; 10 | using ::testing::Test; 11 | using ::testing::TestEventListeners; 12 | using ::testing::TestInfo; 13 | using ::testing::TestPartResult; 14 | using ::testing::UnitTest; 15 | 16 | class PlaneTest : public ::testing::Test { 17 | protected: 18 | void SetUp() override { 19 | setupParams(); 20 | 21 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 22 | ros::console::levels::Debug)) 23 | ros::console::notifyLoggerLevelsChanged(); 24 | } 25 | 26 | void setupParams() { 27 | params.scansPerSweep = 1; 28 | params.maxLidarDist = 15.0; 29 | params.maxGroundLidarDist = 30.0; 30 | params.minGroundLidarDist = 0.0; 31 | 32 | params.groundRadiiBins = 1; 33 | params.groundThetaBins = 18; 34 | params.groundMatchThresh = 2.0; 35 | params.groundRetainThresh = 0.1; 36 | params.numGroundFeatures = 1; 37 | 38 | params.maxTreeRadius = 0.3; 39 | params.maxAxisTheta = 10; 40 | params.maxFocusOutlierDistance = 0.5; 41 | params.roughcylinderMatchThresh = 3.0; 42 | params.cylinderMatchThresh = 1.0; 43 | 44 | params.AddNewTreeThreshDist = 2.0; 45 | 46 | params.featuresPerTree = 2; 47 | params.defaultCylinderRadius = 0.1; 48 | } 49 | 50 | FeatureModelParams params; 51 | }; 52 | 53 | TEST_F(PlaneTest, Initalizes) { 54 | PointT pta, ptb, ptc; 55 | pta.x = 0; 56 | pta.y = 0; 57 | pta.z = 0; 58 | ptb.x = 0; 59 | ptb.y = 1; 60 | ptb.z = 0; 61 | ptc.x = 1; 62 | ptc.y = 0; 63 | ptc.z = 0; 64 | VectorType gfeatures{pta, ptb, ptc}; 65 | Plane plane(gfeatures, params); 66 | EXPECT_TRUE(plane.isValid); 67 | } 68 | 69 | TEST_F(PlaneTest, DistanceToFeature) { 70 | PointT pta, ptb, ptc; 71 | pta.x = 0; 72 | pta.y = 0; 73 | pta.z = 0; 74 | ptb.x = 0; 75 | ptb.y = 1; 76 | ptb.z = 0; 77 | ptc.x = 1; 78 | ptc.y = 0; 79 | ptc.z = 0; 80 | VectorType gfeatures{pta, ptb, ptc}; 81 | Plane plane(gfeatures, params); 82 | 83 | float d = plane.distance(pta); 84 | EXPECT_NEAR(0.0, d, 0.1); 85 | } 86 | 87 | TEST_F(PlaneTest, TranslateModel) { 88 | PointT pta, ptb, ptc; 89 | pta.x = 0; 90 | pta.y = 0; 91 | pta.z = 0; 92 | ptb.x = 0; 93 | ptb.y = 1; 94 | ptb.z = 0; 95 | ptc.x = 1; 96 | ptc.y = 0; 97 | ptc.z = 0; 98 | VectorType gfeatures{pta, ptb, ptc}; 99 | Plane plane(gfeatures, params); 100 | 101 | SE3 tf; 102 | tf.translation()[2] = 1; 103 | auto centroid = plane.model.centroid; 104 | plane.project(tf); 105 | EXPECT_EQ(centroid[2] + 1, plane.model.centroid[2]); 106 | } -------------------------------------------------------------------------------- /backend/sloam_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(sloam_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | geometry_msgs 8 | sensor_msgs 9 | nav_msgs 10 | actionlib_msgs 11 | visualization_msgs 12 | ) 13 | 14 | add_message_files(DIRECTORY msg FILES 15 | interRobotTF.msg 16 | ROSCylinder.msg 17 | ROSEllipsoid.msg 18 | ROSGround.msg 19 | ROSScan.msg 20 | ROSSweep.msg 21 | ROSSubMap.msg 22 | ROSObservation.msg 23 | ObservationPair.msg 24 | LoopClosure.msg 25 | keyPoses.msg 26 | cubeMap.msg 27 | cylinderMap.msg 28 | MultiArrayPoseObjectEdges.msg 29 | poseObjectEdges.msg 30 | ROSCube.msg 31 | PoseMst.msg 32 | PoseMstBundle.msg 33 | ROSRangeBearing.msg 34 | ROSRangeBearingSyncOdom.msg 35 | SemanticLoopClosure.msg 36 | ROSSyncOdom.msg 37 | ROSCylinderArray.msg 38 | SemanticMeasSyncOdom.msg 39 | StampedRvizMarkerArray.msg 40 | vector4d.msg 41 | vector7d.msg 42 | syncPcOdom.msg 43 | ) 44 | 45 | ## Generate actions in the 'action' folder 46 | add_action_files( 47 | FILES 48 | ActiveLoopClosure.action 49 | DetectLoopClosure.action 50 | ) 51 | 52 | add_service_files( 53 | FILES 54 | EvaluateLoopClosure.srv 55 | ) 56 | 57 | generate_messages(DEPENDENCIES std_msgs geometry_msgs sensor_msgs nav_msgs actionlib_msgs visualization_msgs) 58 | 59 | catkin_package( 60 | CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs nav_msgs actionlib_msgs visualization_msgs 61 | ) 62 | -------------------------------------------------------------------------------- /backend/sloam_msgs/action/ActiveLoopClosure.action: -------------------------------------------------------------------------------- 1 | # Action goal 2 | Header header 3 | std_msgs/UInt64 key_pose_idx 4 | geometry_msgs/Point[] submap 5 | # The loop closure server should use this list of semantic objects to find closure 6 | --- 7 | # Action result 8 | Header header 9 | bool success 10 | bool killed 11 | nav_msgs/Odometry optimized_key_pose 12 | nav_msgs/Odometry vio_pose 13 | nav_msgs/Odometry relativeMotion # output of map merging: relative transform from optimized_key_pose to current pose (in factor graph) upon loop closure 14 | nav_msgs/Odometry vio_to_sloam_pose 15 | --- 16 | # Action feedback 17 | Header header 18 | duration computation_time -------------------------------------------------------------------------------- /backend/sloam_msgs/action/DetectLoopClosure.action: -------------------------------------------------------------------------------- 1 | # Action goal 2 | Header header 3 | geometry_msgs/Point[] submap 4 | # The loop closure server should use this list of semantic objects to find closure 5 | --- 6 | # Action result 7 | Header header 8 | bool success 9 | bool killed 10 | int32 status # by default it is 0. If success=false, check status 11 | # status=1 for not enough measurements 12 | # status=2 for Sync time failed 13 | # status=3 for residual too large 14 | sloam_msgs/SemanticLoopClosure loop_closure_msg 15 | --- 16 | # Action feedback 17 | Header header 18 | duration computation_time -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/LoopClosure.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | int32 source_idx 4 | int32 target_idx -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/MultiArrayPoseObjectEdges.msg: -------------------------------------------------------------------------------- 1 | int32 robotID 2 | string objectType 3 | poseObjectEdges[] multiArrayEdges -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ObservationPair.msg: -------------------------------------------------------------------------------- 1 | ROSObservation source 2 | ROSObservation target 3 | int32 source_idx 4 | int32 target_idx -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/PoseMst.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose # 56 bytes 2 | geometry_msgs/Pose relativeRawOdom # 56 bytes 3 | ROSCube[] cubes # 69 bytes each 4 | ROSCylinder[] cylinders # 37 bytes each 5 | ROSEllipsoid[] ellipsoids # 69 bytes each -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/PoseMstBundle.msg: -------------------------------------------------------------------------------- 1 | int8 robotID 2 | PoseMst[] poseMstPair 3 | vector7d[] map_of_labelXYZ 4 | interRobotTF[] interRobotTFs -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSCube.msg: -------------------------------------------------------------------------------- 1 | float32[3] dim 2 | int8 semantic_label 3 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSCylinder.msg: -------------------------------------------------------------------------------- 1 | float32[3] root 2 | float32[3] ray 3 | float64[] radii 4 | float32 radius 5 | int64 id 6 | int8 semantic_label 7 | -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSCylinderArray.msg: -------------------------------------------------------------------------------- 1 | # Header should have the stamp 2 | std_msgs/Header header 3 | ROSCylinder[] cylinders -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSEllipsoid.msg: -------------------------------------------------------------------------------- 1 | float32[3] scale 2 | int8 semantic_label 3 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSGround.msg: -------------------------------------------------------------------------------- 1 | float32[4] coefs 2 | sensor_msgs/PointCloud2 features 3 | int64 id -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSObservation.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped pose 3 | geometry_msgs/PoseStamped initialGuess 4 | sensor_msgs/PointCloud2 pc 5 | ROSCylinder[] treeModels 6 | ROSGround ground 7 | int32[] matches 8 | bool success -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSRangeBearing.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Point[] bearing_factors 3 | float64[] range_factors 4 | geometry_msgs/Point[] landmark_body_positions 5 | std_msgs/UInt64[] meas_ids 6 | nav_msgs/Odometry odom 7 | -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSRangeBearingSyncOdom.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Point[] bearing_factors 3 | float64[] range_factors 4 | geometry_msgs/Point[] landmark_body_positions 5 | std_msgs/UInt64[] meas_ids 6 | nav_msgs/Odometry vio_odom 7 | nav_msgs/Odometry corrected_odom 8 | nav_msgs/Odometry optimized_key_pose 9 | std_msgs/UInt64 key_pose_idx 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSScan.msg: -------------------------------------------------------------------------------- 1 | ROSCylinder[] treeModels 2 | sensor_msgs/PointCloud2 treeFeatures 3 | sensor_msgs/PointCloud2 groundFeatures 4 | -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSSubMap.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | ROSCylinder[] treeModels 3 | sensor_msgs/PointCloud2 treeFeatures 4 | sensor_msgs/PointCloud2 groundFeatures 5 | geometry_msgs/PoseWithCovarianceStamped t_map_anchor 6 | -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSSweep.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | ROSScan[] scans 3 | geometry_msgs/PoseStamped t_anchor_curr 4 | geometry_msgs/PoseStamped t_global_curr 5 | -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/ROSSyncOdom.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | # Synced High Freq VIO and SLOAM Odom 3 | # VIO raw odometry 4 | nav_msgs/Odometry vio_odom 5 | # factor graph estimated pose 6 | nav_msgs/Odometry sloam_odom -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/SemanticLoopClosure.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | # VIO raw odometry 3 | nav_msgs/Odometry vio_odom 4 | # factor graph estimated pose 5 | nav_msgs/Odometry corrected_odom 6 | # history key pose index for loop closure 7 | std_msgs/UInt64 key_pose_idx 8 | # history key pose estimated by factor graph for loop closure 9 | nav_msgs/Odometry optimized_key_pose 10 | # output of map merging: relative transform from optimized_key_pose to current pose upon loop closure 11 | nav_msgs/Odometry optimized_relative_loop_closure_pose 12 | # output of map merging: relative transform from optimized_key_pose to current pose upon loop closure 13 | nav_msgs/Odometry vio_to_sloam_pose -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/SemanticMeasSyncOdom.msg: -------------------------------------------------------------------------------- 1 | # This message is the main message for semantic SLAM, where synced measurements are recorded. 2 | # If a certain type of measurements is absent, we just leave it as empty 3 | # Header should have the stamp 4 | # std_msgs/Header header 5 | # # For centroid semantic landmarks, the direction of this 3D vector is bearing while the magnitude is the range 6 | # geometry_msgs/Point[] centroid_factors 7 | # # For cylindrical landmarks. Contains root, radius and axis. Same as original SLOAM cylinder modeling 8 | # ROSCylinder[] cylinder_factors 9 | # # For cuboidal landmarks. 10 | # visualization_msgs/MarkerArray cuboid_factors 11 | # # For the odometry corresponding to these measurements 12 | # nav_msgs/Odometry odometry 13 | 14 | 15 | # new version 16 | # Header should have the stamp 17 | std_msgs/Header header 18 | # For centroid semantic landmarks, the direction of this 3D vector is bearing while the magnitude is the range 19 | ROSEllipsoid[] ellipsoid_factors 20 | # For cylindrical landmarks. Contains root, radius and axis. Same as original SLOAM cylinder modeling 21 | ROSCylinder[] cylinder_factors 22 | # For cuboidal landmarks. 23 | ROSCube[] cuboid_factors 24 | # For the odometry corresponding to these measurements 25 | nav_msgs/Odometry odometry -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/StampedRvizMarkerArray.msg: -------------------------------------------------------------------------------- 1 | # Header should have the stamp 2 | std_msgs/Header header 3 | # For cuboidal landmarks. 4 | visualization_msgs/MarkerArray cuboid_rviz_markers -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/cubeMap.msg: -------------------------------------------------------------------------------- 1 | int8 robotID 2 | ROSCube[] cubes -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/cylinderMap.msg: -------------------------------------------------------------------------------- 1 | int8 robotID 2 | ROSCylinder[] cylinders -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/interRobotTF.msg: -------------------------------------------------------------------------------- 1 | int8 hostRobotID 2 | int8 targetRobotID 3 | geometry_msgs/Pose TFfromTarget2Host -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/keyPoses.msg: -------------------------------------------------------------------------------- 1 | int8 robotID 2 | geometry_msgs/Pose[] poses -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/poseObjectEdges.msg: -------------------------------------------------------------------------------- 1 | #it contains an array of int, each element is the index of 2 | #the object seen at a certain keyPose 3 | int32[] edges -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/syncPcOdom.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | sensor_msgs/PointCloud2 cloud 3 | nav_msgs/Odometry odom 4 | # This message contains the synced odometry with the point cloud 5 | # The point cloud contains data in the following order: 6 | # points (x, y, z), 7 | # label (classification), 8 | # id (an unique id for different instance. Note: not global ID), 9 | # confidence (the confidence of the classification) 10 | # see process cloud node in sloam to parse the point cloud 11 | -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/vector4d.msg: -------------------------------------------------------------------------------- 1 | float64[4] labelXYZ -------------------------------------------------------------------------------- /backend/sloam_msgs/msg/vector7d.msg: -------------------------------------------------------------------------------- 1 | float64[7] labelXYZ -------------------------------------------------------------------------------- /backend/sloam_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | sloam_msgs 5 | 0.0.1 6 | Defines the messages that are used to communicate with the sloam package. 7 | 8 | TODO 9 | Yuezhao Tao 10 | Xu Liu 11 | Yuezhao Tao 12 | Xu Liu 13 | Guilherme Nardari 14 | 15 | catkin 16 | 17 | message_generation 18 | std_msgs 19 | geometry_msgs 20 | sensor_msgs 21 | nav_msgs 22 | actionlib_msgs 23 | visualization_msgs 24 | message_runtime 25 | -------------------------------------------------------------------------------- /backend/sloam_msgs/srv/EvaluateLoopClosure.srv: -------------------------------------------------------------------------------- 1 | Header header 2 | # bool add_factor # if is best action 3 | float64[] travel_distances # distance to travel 4 | uint64[] candidate_traj_pose_indices # candidate trajectory pose indices 5 | # std_msgs/UInt64 current_pose_id # current key pose id in factor graph 6 | # std_msgs/UInt64 closure_key_pose_id # history key pose id for candidate loop closure in factor graph 7 | # bool reset # if reset the factor graph, sync with the true factor graph 8 | --- 9 | float64 information_gain # information gain 10 | bool success # if success 11 | -------------------------------------------------------------------------------- /backend/sloam_msgs/srv/graphTansmission.srv: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /frontend/object_modeller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(object_modeller) 3 | 4 | 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | pcl_conversions 8 | pcl_ros 9 | roscpp 10 | sensor_msgs 11 | ) 12 | 13 | find_package(PCL 1.7 REQUIRED) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs sloam_msgs 18 | DEPENDS PCL 19 | ) 20 | 21 | include_directories( 22 | include 23 | ${catkin_INCLUDE_DIRS} 24 | ${PCL_INCLUDE_DIRS} 25 | ) 26 | 27 | link_directories(${PCL_LIBRARY_DIRS}) 28 | add_definitions(${PCL_DEFINITIONS}) 29 | 30 | 31 | add_executable(${PROJECT_NAME}_cylinder src/cylinder_modeller.cpp) 32 | target_link_libraries(${PROJECT_NAME}_cylinder ${catkin_LIBRARIES} ${PCL_LIBRARIES} -ldw) 33 | target_include_directories(${PROJECT_NAME}_cylinder PUBLIC include ${PCL_INCLUDE_DIRS}) 34 | -------------------------------------------------------------------------------- /frontend/object_modeller/config/cls_all.yaml: -------------------------------------------------------------------------------- 1 | # Classes used by YOLOv8 object detector 2 | # Background class is 0 so start class list from 1 3 | # Each class has the following attributes and new classes must be added in the same format: 4 | # id: Unique ID for the class 5 | # color: RGB color of the class 6 | # length_cutoff: Min and max length of the object to consider it as valid detection and fit model 7 | # height_cutoff: Min and max height of the object to consider it as valid detection and fit model 8 | # mesh_model_path: Path to the mesh model of the object. If it doesn't exist, leave the value as empty and don't write anything. An Ellipsoid model will be used in that case. 9 | # mesh_model_scale: Scale of the mesh model. This value will be multiplied by a fixed dimension which is 0.6m for all dimensions. Can be left blank if mesh model doesn't exist. 10 | # class_assignment_thresh: Distance threshold in meters for Hungarian Assignment to consider two object tracks as the same object. If two object model centroids are closer than this value, they will be considered as the same object. 11 | chair: 12 | id: 1 13 | color: [1.0, 0.0, 0.0] # Red Chair 14 | length_cutoff: [0.2, 2.0] 15 | height_cutoff: [0.2, 2.0] 16 | mesh_model_path: "package://sloam/resource/chair.stl" 17 | mesh_model_scale: 0.85 18 | class_assignment_thresh: 0.75 19 | table: 20 | id: 2 21 | color: [0.0, 1.0, 0.0] # Green Table 22 | length_cutoff: [1.0, 5.0] 23 | height_cutoff: [0.2, 3.0] 24 | mesh_model_path: "package://sloam/resource/table2.stl" 25 | mesh_model_scale: 0.5 26 | class_assignment_thresh: 10.0 27 | tv: 28 | id: 3 29 | color: [0.0, 0.0, 1.0] # Blue TV 30 | length_cutoff: [0.1, 2.0] 31 | height_cutoff: [0.1, 2.0] 32 | mesh_model_path: "package://sloam/resource/monitor.stl" 33 | mesh_model_scale: 0.3 34 | class_assignment_thresh: 1.5 -------------------------------------------------------------------------------- /frontend/object_modeller/config/cylinder_plane_modeller_params.yaml: -------------------------------------------------------------------------------- 1 | # This YAML file contains the parameters for the cylinder_plane_modeller.py script 2 | # which is used to fit cylinders and plane models to tree trunks and ground respectively. 3 | 4 | #--------------IMPORTANT PARAMETERS---------------- 5 | # The angle of the cylinder axis ray compared to the ground plane. 6 | # Cylinders with axis ray angle more than these values are discarded. 7 | # Used to avoid cylinder fitting to very slanted tree trunks (like fallen trees). 8 | angle_cutoff: 15 9 | 10 | # The minimum and maximum radius of the cylinder. 11 | # Cylinders with radius less than the minimum or more than the maximum have a default radius set for them which is default_radius. 12 | radius_cutoff: [0.05, 0.5] 13 | 14 | # The minimum number of points required for a point cloud to be considered as a valid cylinder fitting object. 15 | min_points_per_tree: 15 16 | 17 | # The minimum number of points required in the point cloud ring to calculate the radius of the cylinder. 18 | min_points_for_radius: 5 19 | 20 | # The minimum number of points required to fit a plane model to the ground points. 21 | min_points_per_ground_patch: 40 22 | 23 | # DBSCAN parameters for clustering of the tree point clouds to generate tree instance point clouds. 24 | clus_eps: 0.5 25 | clus_min_samples: 10 26 | 27 | # Rate at which the cylinder fitting function is run to process the tree point cloud data 28 | # Rate = 1/run_rate 29 | # This is done to reduce computational load. 30 | # If you have a lot of trees then reduce this number to run at a faster rate. 31 | run_rate: 20 32 | #--------------------------------------------------- 33 | 34 | # Size of the ground plane patch to display in RViz 35 | ground_plane_patch_size: 10 36 | 37 | # Number of ground plane models to keep in the memory to retrieve the nearest available ground plane model 38 | num_ground_plane_model_to_keep: 50 39 | 40 | # Height above the ground to measure the diameter of the tree trunk. 1.37 is the standard Diameter-at-Breast-Height measurement in forestry research. 41 | diameter_measure_height_above_ground: 1.3716 42 | 43 | # Default radius to set for cylinders with radius less than the minimum or more than the maximum radius. 44 | default_radius: 0.2 45 | -------------------------------------------------------------------------------- /frontend/object_modeller/config/rgb_segmentation_open_vocab_params.yaml: -------------------------------------------------------------------------------- 1 | sim: false 2 | desired_rate: 2 3 | confidence_threshold: 0.4 4 | rgb_topic: "camera/color/image_raw/" 5 | depth_topic: "camera/depth/image_rect_raw" 6 | aligned_depth_topic: "camera/aligned_depth_to_color/image_raw" 7 | odom_topic: "$(arg odom_topic)" 8 | sync_odom_measurements: true 9 | sync_pc_odom_topic: "/robot$(arg robot_ID)/sem_detection/sync_pc_odom" 10 | pc_topic: "/robot$(arg robot_ID)/sem_detection/pointcloud" 11 | 12 | # For scarab 13 | # fx: 390.97088623046875 14 | # fy: 390.970886230468 15 | # cx: 324.1226806640625 16 | # cy: 243.95254516601562 17 | 18 | # For f250 435i 19 | fx: 603.7166748046875 20 | fy: 603.9064331054688 21 | cx: 314.62518310546875 22 | cy: 244.9166717529297 23 | k_depth_scaling_factor: 1000.0 24 | -------------------------------------------------------------------------------- /frontend/object_modeller/launch/rgb_segmentation_f250.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 34 | 35 | 36 | 37 | 38 | 39 | --> 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /frontend/object_modeller/launch/rgb_segmentation_open_vocab.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /frontend/object_modeller/launch/sync_semantic_measurements.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /frontend/object_modeller/models/THIS_FOLDER_STORES_YOUR_MODELS.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/SLIDE_SLAM/9d510f075273605bb1c36b4321d5185e25ff4a39/frontend/object_modeller/models/THIS_FOLDER_STORES_YOUR_MODELS.txt -------------------------------------------------------------------------------- /frontend/object_modeller/script/play_bag_remap_scripts/play_bag_with_remapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /frontend/object_modeller/script/tmux_indoor_outdoor_merge.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | 4 | SESSION_NAME=sync_nodes 5 | 6 | CURRENT_DISPLAY=${DISPLAY} 7 | if [ -z ${DISPLAY} ]; 8 | then 9 | echo "DISPLAY is not set" 10 | CURRENT_DISPLAY=:0 11 | fi 12 | 13 | if [ -z ${TMUX} ]; 14 | then 15 | TMUX= tmux new-session -s $SESSION_NAME -d 16 | echo "Starting new session." 17 | else 18 | echo "Already in tmux, leave it first." 19 | exit 20 | fi 21 | 22 | SETUP_ROS_STRING="source ~/sloam_ws/devel/setup.bash; export ROS_MASTER_URI=http://localhost:11311" 23 | 24 | # Python args 25 | ODOM_TOPIC="/dragonfly67/quadrotor_ukf/control_odom" 26 | # ODOM_TOPIC="/Odometry" 27 | # ODOM_TOPIC="/scarab41/odom_laser" 28 | # ODOM_TOPIC="/quadrotor1/lidar_odom" 29 | # ODOM_TOPIC="/quadrotor1/lidar_odom" 30 | # POINT_CLOUD_NS="/quadrotor1/" 31 | POINT_CLOUD_NS="/" 32 | 33 | 34 | # Make mouse useful in copy mode 35 | tmux setw -g mouse on 36 | 37 | 38 | tmux rename-window -t $SESSION_NAME "Core" 39 | tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time True" Enter 40 | 41 | 42 | 43 | # tmux new-window -t $SESSION_NAME -n "Bag" 44 | # # tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; rosbag play --clock /home/sam/bags/pennovation-bags/generic_sloam_2_robots_multi_robot_MOST_IMPORTANT_2022-06-30-22-50-33.bag -s 30" 45 | # # tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; rosbag play --clock /home/sam/bags/xmas-slam-bags/test-indoor-sloam-and-SLC-cylinder-odom-only-bag-2023-10-26-17-58-19.bag -s 30" 46 | # # tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; rosbag play --clock /home/jiuzl/bags/outdoor_bag/outdoor-car-detection-drifted-5m-shorter-range-second-part-partking-lot-2023-12-05-14-21-56.bag" 47 | # tmux split-window -t $SESSION_NAME 48 | # tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; rosparam get /use_sim_time; python3 ./merge_synced_measurements.py" Enter 49 | # tmux select-layout -t $SESSION_NAME tiled 50 | 51 | 52 | tmux new-window -t $SESSION_NAME -n "Sync" 53 | tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch object_modeller sync_semantic_measurements.launch" Enter 54 | 55 | # Add window to easily kill all processes 56 | tmux new-window -t $SESSION_NAME -n "rviz" 57 | tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; rviz -d ../rviz/object_modeller.rviz" 58 | 59 | # sloam 60 | tmux new-window -t $SESSION_NAME -n "sloam" 61 | tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam single_robot_sloam_test.launch" Enter 62 | 63 | 64 | # Add window to easily kill all processes 65 | tmux new-window -t $SESSION_NAME -n "Kill" 66 | tmux send-keys -t $SESSION_NAME "tmux kill-session -t ${SESSION_NAME}" 67 | 68 | 69 | tmux select-window -t $SESSION_NAME:5 70 | tmux -2 attach-session -t $SESSION_NAME 71 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(scan2shape_launch) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED) 7 | 8 | catkin_package() 9 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/infer_node_params.yaml: -------------------------------------------------------------------------------- 1 | # Description: Parameters for the infer_node.py file 2 | #------------------------------------------------- 3 | # namespace: namespace used by the segmented point cloud publisher to publish under 4 | 5 | # model_dir: path to the directory containing the trained RangeNet++ model. The path should end with a '/' 6 | # Also make sure that all model files do not have a .zip extension. If they do, then edit the file names and just remove the .zip extension. 7 | 8 | # desired_frequency: frequency at which inference should be performed by the infer_node.py file. 9 | # This can be used to skip inferences on some point clouds to speed up operations especially when running on a CPU. 10 | 11 | # num_cpu_threads: number of CPU threads to be used by PyTorch to perform CPU inference. 12 | 13 | # pc_range_threshold: threshold for the range of the point cloud in meters. If the range of the points in the point cloud is greater than this threshold, 14 | # those points are considered to be out of range and the out_of_range_default_position parameter is used as the position of those points. 15 | 16 | # pc_point_step: length of a point in bytes in the point cloud. 17 | #------------------------------------------------- 18 | namespace: /os_node 19 | model_dir: /opt/bags/vems-slam-bags/penn_smallest/ 20 | desired_frequency: 2 21 | num_cpu_threads: 10 22 | pc_range_threshold: 40 23 | pc_point_step: 16 24 | out_of_range_default_position: [0, 0, 0] -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_indoor_cls_info.yaml: -------------------------------------------------------------------------------- 1 | # Classes used by YOLOv8 object detector 2 | # Background class is 0 so start class list from 1 3 | # Each class has the following attributes and new classes must be added in the same format: 4 | # id: Unique ID for the class 5 | # color: RGB color of the class 6 | # length_cutoff: Min and max length of the object to consider it as valid detection and fit model 7 | # height_cutoff: Min and max height of the object to consider it as valid detection and fit model 8 | # mesh_model_path: Path to the mesh model of the object. If it doesn't exist, leave the value as empty and don't write anything. An Ellipsoid model will be used in that case. 9 | # mesh_model_scale: Scale of the mesh model. This value will be multiplied by a fixed dimension which is 0.6m for all dimensions. Can be left blank if mesh model doesn't exist. 10 | # class_assignment_thresh: Distance threshold in meters for Hungarian Assignment to consider two object tracks as the same object. If two object model centroids are closer than this value, they will be considered as the same object. 11 | chair: 12 | id: 1 13 | color: [1.0, 0.0, 0.0] # Red Chair 14 | length_cutoff: [0.2, 2.0] 15 | height_cutoff: [0.2, 2.0] 16 | mesh_model_path: "package://sloam/resource/chair.stl" 17 | mesh_model_scale: 0.85 18 | class_assignment_thresh: 0.75 19 | table: 20 | id: 2 21 | color: [0.0, 1.0, 0.0] # Green Table 22 | length_cutoff: [0.5, 5.0] 23 | height_cutoff: [0.2, 3.0] 24 | mesh_model_path: "package://sloam/resource/table2.stl" 25 | mesh_model_scale: 0.5 26 | class_assignment_thresh: 2.0 27 | tv: 28 | id: 3 29 | color: [0.0, 0.0, 1.0] # Blue TV 30 | length_cutoff: [0.1, 2.0] 31 | height_cutoff: [0.1, 2.0] 32 | mesh_model_path: "package://sloam/resource/monitor.stl" 33 | mesh_model_scale: 0.3 34 | class_assignment_thresh: 1.5 -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_indoor_open_vocab_params.yaml: -------------------------------------------------------------------------------- 1 | # Description: Parameters for the process_cloud_node_indoor.py file 2 | 3 | #------------------Important Parameters START------------------ 4 | # Object detection confidence threshold 5 | confidence_threshold: 0.4 6 | # threshold for segmented lidar points range in meters 7 | valid_range_threshold: 40.0 8 | # expected segmentation frequency in Hz 9 | expected_segmentation_frequency: 2.0 10 | # Minimum time in seconds to track a object for before model fitting 11 | time_to_initialize_cuboid: 0.75 12 | # Lower and upper percentile of depth values to consider for object detection 13 | # When using the open-vocab object detector, use more conservative values to avoid faulty depth since the model cannot do instance segmentation 14 | depth_percentile_lower: 35 15 | depth_percentile_upper: 45 # open-vocab: 45 16 | # input point cloud parameters 17 | pc_width: 1024 18 | pc_heigh: 64 19 | pc_point_step: 16 20 | #------------------Important Parameters END------------------ 21 | 22 | # Rate at which object models are published 23 | desired_acc_obj_pub_rate: 1.0 24 | # use simulator or run on real robot data. SHOULD ALWAYS BE FALSE for running on real robot data 25 | use_sim: False 26 | # Cuboid dim threshold used to start initial object tracking process 27 | fit_cuboid_dim_thresh: 0.2 28 | # Resolution to perform voxel downsampling for accumulated instance point cloud. -1 means no downsample 29 | downsample_res: -1 30 | # only keep the most recent num_points_limit_per_instance for any segmented point cloud instance while tracking it 31 | num_instance_point_lim: 10000 32 | # time to delete a track if it is not updated for a long time 33 | time_to_delete_lost_track_cuboid: 30 34 | # save figures of object clustering results from DBSCAN 35 | visualize_DBSCAN_results: False 36 | output_dir_to_save_figs: "./" -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_indoor_params.yaml: -------------------------------------------------------------------------------- 1 | # Description: Parameters for the process_cloud_node_indoor.py file 2 | 3 | #------------------Important Parameters START------------------ 4 | # Object detection confidence threshold 5 | confidence_threshold: 0.6 6 | # threshold for segmented lidar points range in meters 7 | valid_range_threshold: 40.0 8 | # expected segmentation frequency in Hz 9 | expected_segmentation_frequency: 2.0 10 | # Minimum time in seconds to track a object for before model fitting 11 | time_to_initialize_cuboid: 0.75 12 | # Lower and upper percentile of depth values to consider for object detection 13 | # When using the open-vocab object detector, use more conservative values to avoid faulty depth since the model cannot do instance segmentation 14 | depth_percentile_lower: 35 15 | depth_percentile_upper: 45 # open-vocab: 45 16 | # input point cloud parameters 17 | pc_width: 1024 18 | pc_heigh: 64 19 | pc_point_step: 16 20 | #------------------Important Parameters END------------------ 21 | 22 | # Rate at which object models are published 23 | desired_acc_obj_pub_rate: 1.0 24 | # use simulator or run on real robot data. SHOULD ALWAYS BE FALSE for running on real robot data 25 | use_sim: False 26 | # Cuboid dim threshold used to start initial object tracking process 27 | fit_cuboid_dim_thresh: 0.2 28 | # Resolution to perform voxel downsampling for accumulated instance point cloud. -1 means no downsample 29 | downsample_res: -1 30 | # only keep the most recent num_points_limit_per_instance for any segmented point cloud instance while tracking it 31 | num_instance_point_lim: 10000 32 | # time to delete a track if it is not updated for a long time 33 | time_to_delete_lost_track_cuboid: 30 34 | # save figures of object clustering results from DBSCAN 35 | visualize_DBSCAN_results: False 36 | output_dir_to_save_figs: "./" -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_outdoor_class_info.yaml: -------------------------------------------------------------------------------- 1 | # All classes must have atleast the following fields: 2 | # id: unique integer id for the class 3 | # model: model type for the class. Currently supported models are "cuboid" and "cylinder". For ground class, leave the value empty. 4 | 5 | # For cuboid model, the following fields are required: 6 | # track_assignment_threshold: threshold for assigning and merging two cuboid tracks, during Hungarian Assignment, as one, based on their centroid distance. 7 | # length_cutoff: minimum and maximum length of the cuboid 8 | # width_cutoff: minimum and maximum width of the cuboid 9 | # height_cutoff: minimum and maximum height of the cuboid 10 | 11 | # TODO(ankit): Add instructions for cylinder model 12 | 13 | # All ground planes must be named as "ground" in the class_info.yaml file. 14 | # This "ground" string is used to determine if a ground plane can be extracted from segmented point cloud. 15 | ground: 16 | id: 1 17 | model: 18 | car: 19 | id: 5 20 | model: cuboid 21 | track_assignment_threshold: 1.0 22 | track_age_threshold: 4 23 | clustering_params: [0.5, 25] 24 | length_cutoff: [2.0, 5.5] 25 | width_cutoff: [1.0, 2.5] 26 | height_cutoff: [0.5, 2.0] 27 | tree: 28 | id: 8 29 | model: cylinder 30 | track_assignment_threshold: 1.0 31 | lightpole: 32 | id: 9 33 | model: cylinder 34 | track_assignment_threshold: 1.0 -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_outdoor_class_info_dcist_demo.yaml: -------------------------------------------------------------------------------- 1 | # All classes must have atleast the following fields: 2 | # id: unique integer id for the class 3 | # model: model type for the class. Currently supported models are "cuboid" and "cylinder". For ground class, leave the value empty. 4 | 5 | # For cuboid model, the following fields are required: 6 | # track_assignment_threshold: threshold for assigning and merging two cuboid tracks, during Hungarian Assignment, as one, based on their centroid distance. 7 | # track_age_threshold: minimum age of a track (obtained from Hungarian tracking) before it can be used for cuboid fitting. This value is multipled by the expected segmentation frequency to get the actual age threshold. 8 | # length_cutoff: minimum and maximum length of the cuboid 9 | # width_cutoff: minimum and maximum width of the cuboid 10 | # height_cutoff: minimum and maximum height of the cuboid 11 | # clustering_params: DBSCAN clustering parameters for the class. The first parameter is epsilon and the second parameter is min_samples. 12 | 13 | # TODO(ankit): Add instructions for cylinder model 14 | 15 | # All ground planes must be named as "ground" in the class_info.yaml file. 16 | # This "ground" string is used to determine if a ground plane can be extracted from segmented point cloud. 17 | ground: 18 | id: 0 19 | model: 20 | building: 21 | id: 3 22 | model: cuboid 23 | track_assignment_threshold: 8.0 24 | track_age_threshold: 16 25 | length_cutoff: [3.0, 15.0] 26 | width_cutoff: [3.0, 15.0] 27 | height_cutoff: [0.5, 10.0] 28 | clustering_params: [4.0, 80] #85 29 | box: 30 | id: 4 31 | model: cuboid 32 | track_assignment_threshold: 1.0 33 | track_age_threshold: 2 34 | length_cutoff: [0.05, 0.1] 35 | width_cutoff: [0.05, 0.1] 36 | height_cutoff: [0.05, 0.1] 37 | clustering_params: [0.5, 5] 38 | car: 39 | id: 5 40 | model: cuboid 41 | track_assignment_threshold: 5.0 42 | track_age_threshold: 4 43 | length_cutoff: [3.0, 7.5] 44 | width_cutoff: [1.5, 7.5] 45 | height_cutoff: [0.5, 4.0] 46 | clustering_params: [0.5, 20] #20 47 | big_bench: 48 | id: 7 49 | model: cuboid 50 | track_assignment_threshold: 3.0 #4.0 51 | track_age_threshold: 6 52 | length_cutoff: [1.0, 10.0] 53 | width_cutoff: [0.5, 10.0] 54 | height_cutoff: [0.5, 5.0] 55 | clustering_params: [2.0, 35] #40 56 | tree: 57 | id: 8 58 | model: cylinder 59 | track_assignment_threshold: 1.0 -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_outdoor_kitti_class_info.yaml: -------------------------------------------------------------------------------- 1 | # All classes must have atleast the following fields: 2 | # id: unique integer id for the class 3 | # model: model type for the class. Currently supported models are "cuboid" and "cylinder". For ground class, leave the value empty. 4 | 5 | # For cuboid model, the following fields are required: 6 | # track_assignment_threshold: threshold for assigning and merging two cuboid tracks, during Hungarian Assignment, as one, based on their centroid distance. 7 | # length_cutoff: minimum and maximum length of the cuboid 8 | # width_cutoff: minimum and maximum width of the cuboid 9 | # height_cutoff: minimum and maximum height of the cuboid 10 | 11 | # TODO(ankit): Add instructions for cylinder model 12 | 13 | # All ground planes must be named as "ground" in the class_info.yaml file. 14 | # This "ground" string is used to determine if a ground plane can be extracted from segmented point cloud. 15 | ground: 16 | id: 40 17 | model: 18 | car: 19 | id: 10 20 | model: cuboid 21 | track_assignment_threshold: 1.0 22 | track_age_threshold: 0.1 23 | clustering_params: [0.5, 10] 24 | length_cutoff: [0.5, 7.5] 25 | width_cutoff: [0.5, 7.5] 26 | height_cutoff: [0.2, 4.0] 27 | tree: 28 | id: 71 29 | model: cylinder 30 | track_assignment_threshold: 1.0 31 | lightpole: 32 | id: 80 33 | model: cylinder 34 | track_assignment_threshold: 1.0 35 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_outdoor_kitti_params.yaml: -------------------------------------------------------------------------------- 1 | # Description: Parameters for the process_cloud_node_outdoor.py file 2 | 3 | #------------------Important Parameters START------------------ 4 | # threshold for segmented lidar points range in meters 5 | valid_range_threshold: 100.0 6 | # expected segmentation frequency in Hz 7 | expected_segmentation_frequency: 2.0 #2.0 8 | # Minimum time in seconds to track a cuboid for before cuboid fitting 9 | time_to_initialize_cuboid: 3.0 10 | # TODO(ankit): Change the use_1st_layer_clustering param names to something more intuitive 11 | # DBSCAN parameters for clustering the segmented point cloud 12 | # The first layer of clustering is done to throw away small noisy points. Hence the parameters need to be conservative 13 | # The second layer of clustering is done to cluster the remaining points into objects. Hence the parameters can be more aggressive 14 | use_1st_layer_clustering: True 15 | use_2nd_layer_clustering: True 16 | epsilon_scan_1st_layer: 0.1 17 | min_samples_scan_1st_layer: 7 18 | epsilon_scan_2nd_layer: 0.5 19 | min_samples_scan_2nd_layer: 25 20 | # Flag to determine whether to estimate facing direction of car by comparing front and rear heights 21 | estimate_facing_dir_car: False 22 | # Flag to determine whether to cluster and fix cuboid orientation by taking a consensus of the orientations of the cuboids in the cluster 23 | cluster_and_fix_cuboid_orientation: True 24 | # Flag for running KITTI dataset benchmarking code 25 | run_kitti: True # default False 26 | # input point cloud parameters 27 | pc_width: 1024 28 | pc_heigh: 64 29 | pc_point_step: 16 30 | #------------------Important Parameters END------------------ 31 | 32 | # Minimum number of ground points required to perform RANSAC and fit a plane 33 | ransac_n_points: 50 34 | # the height above the ground plane to consider a point as an object point. All points below this height are considered as ground points and not used for object fitting 35 | ground_median_increment: 0.25 36 | # use simulator or run on real robot data. SHOULD ALWAYS BE FALSE for running on real robot data 37 | use_sim: False 38 | # visualize all cuboid instances (keep track of all and never remove tracks), should be set as False unless debugging 39 | track_always_visualize: False 40 | # Cuboid dim threshold used to start initial object tracking process 41 | fit_cuboid_dim_thresh: 0.5 42 | # Resolution to perform voxel downsampling for accumulated instance point cloud. -1 means no downsample 43 | downsample_res: -1 44 | # only keep the most recent num_points_limit_per_instance for any segmented point cloud instance while tracking it 45 | num_instance_point_lim: 10000 46 | # time to delete a track if it is not updated for a long time 47 | time_to_delete_lost_track_cuboid: 45 48 | # save figures of object clustering results from DBSCAN 49 | visualize_DBSCAN_results: False 50 | output_dir_to_save_figs: "./" 51 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_outdoor_params.yaml: -------------------------------------------------------------------------------- 1 | # Description: Parameters for the process_cloud_node_outdoor.py file 2 | 3 | #------------------Important Parameters START------------------ 4 | # threshold for segmented lidar points range in meters 5 | valid_range_threshold: 40.0 6 | # expected segmentation frequency in Hz 7 | expected_segmentation_frequency: 2.0 8 | # Minimum time in seconds to track a cuboid for before cuboid fitting 9 | time_to_initialize_cuboid: 3.0 10 | # TODO(ankit): Change the use_1st_layer_clustering param names to something more intuitive 11 | # DBSCAN parameters for clustering the segmented point cloud 12 | # The first layer of clustering is done to throw away small noisy points. Hence the parameters need to be conservative 13 | # The second layer of clustering is done to cluster the remaining points into objects. Hence the parameters can be more aggressive 14 | use_1st_layer_clustering: True 15 | use_2nd_layer_clustering: True 16 | epsilon_scan_1st_layer: 0.1 17 | min_samples_scan_1st_layer: 7 18 | epsilon_scan_2nd_layer: 0.5 19 | min_samples_scan_2nd_layer: 25 20 | # Flag to determine whether to estimate facing direction of car by comparing front and rear heights 21 | estimate_facing_dir_car: False 22 | # Flag to determine whether to cluster and fix cuboid orientation by taking a consensus of the orientations of the cuboids in the cluster 23 | cluster_and_fix_cuboid_orientation: True 24 | # Flag for running KITTI dataset benchmarking code 25 | run_kitti: False 26 | # input point cloud parameters 27 | pc_width: 1024 28 | pc_heigh: 64 29 | pc_point_step: 16 30 | #------------------Important Parameters END------------------ 31 | 32 | # Minimum number of ground points required to perform RANSAC and fit a plane 33 | ransac_n_points: 50 34 | # the height above the ground plane to consider a point as an object point. All points below this height are considered as ground points and not used for object fitting 35 | ground_median_increment: 0.25 36 | # use simulator or run on real robot data. SHOULD ALWAYS BE FALSE for running on real robot data 37 | use_sim: False 38 | # visualize all cuboid instances (keep track of all and never remove tracks), should be set as False unless debugging 39 | track_always_visualize: False 40 | # Cuboid dim threshold used to start initial object tracking process 41 | fit_cuboid_dim_thresh: 0.5 42 | # Resolution to perform voxel downsampling for accumulated instance point cloud. -1 means no downsample 43 | downsample_res: -1 44 | # only keep the most recent num_points_limit_per_instance for any segmented point cloud instance while tracking it 45 | num_instance_point_lim: 10000 46 | # time to delete a track if it is not updated for a long time 47 | time_to_delete_lost_track_cuboid: 45 48 | # save figures of object clustering results from DBSCAN 49 | visualize_DBSCAN_results: False 50 | output_dir_to_save_figs: "./" 51 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_real_robot.yaml: -------------------------------------------------------------------------------- 1 | ground_class_label: 0 # the label corresponds to ground class 2 | car_class_label: 1 # the label corresponds to vehicle class 3 | tree_trunk_class_label: 8 # the label corresponds to tree class 4 | light_pole_class_label: 9 # the label corresponds to light pole class 5 | desired_acc_obj_pub_rate: 1 # in Hz 6 | valid_range_threshold: 6.0 # in meters 7 | epsilon_scan: 0.5 # epsilon_scan: determines the radius of clustering when considering neighboring points as single cluster. 8 | min_samples_scan: 100 # min_samples_scan : determines the minimum samples needed inside the epsilon radius of the cluster to be clustered together 9 | depth_percentile_lower: 30 # depth_percentile_lower: depth percentile for filtering out noisy points in depth camera 10 | depth_percentile_uppper: 50 # depth_percentile_uppper: depth percentile for filtering out noisy points in depth camera 11 | confidence_threshold: 0.5 # confidence_threshold: confidence threshold for filtering out noisy points in depth camera 12 | cuboid_length_cutoff_upper: 1.5 # cuboid_length_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 13 | cuboid_width_cutoff_upper: 1.5 # cuboid_width_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 14 | cuboid_height_cutoff_upper: 5 # cuboid_height_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids assignment_threshold: 1.0 # assignment_threshold: Hungarian assignment cost to associate 2 cuboid centroids as one 15 | downsample_res: 0.1 # Resolution to perform voxel downsampling for instance point cloud accumulation 16 | num_instance_point_lim: 30000 # only keep the most recent num_points_limit_per_instance for any instance 17 | for_forestry_bags: False # run forestry data or not 18 | sim: False 19 | ########### THREE PARAMS ARE LINKED TOGETHER ########### 20 | expected_segmentation_rate: 3 # in Hz 21 | time_to_initialize_cuboid: 0.3 # time_to_initialize_cuboid: Minimum time required to track a cuboid per lidar scan 22 | time_to_delete_lost_track_cuboid: 5 # time_to_delete_lost_track_cuboid: Lower will delete tracks faster and will cause new tracks to be initiated at the same location 23 | ######################################################## -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_real_robot_full_mission.yaml: -------------------------------------------------------------------------------- 1 | ground_class_label: 0 # the label corresponds to ground class 2 | car_class_label: 1 # the label corresponds to vehicle class 3 | tree_trunk_class_label: 8 # the label corresponds to tree class 4 | light_pole_class_label: 9 # the label corresponds to light pole class 5 | desired_acc_obj_pub_rate: 1 # in Hz 6 | valid_range_threshold: 6.5 # in meters 7 | epsilon_scan: 0.5 # epsilon_scan: determines the radius of clustering when considering neighboring points as single cluster. 8 | min_samples_scan: 200 # min_samples_scan : determines the minimum samples needed inside the epsilon radius of the cluster to be clustered together 9 | depth_percentile_lower: 15 # depth_percentile_lower: depth percentile for filtering out noisy points in depth camera 10 | depth_percentile_uppper: 40 # depth_percentile_uppper: depth percentile for filtering out noisy points in depth camera 11 | confidence_threshold: 0.4 # confidence_threshold: confidence threshold for filtering out noisy points in depth camera 12 | cuboid_length_cutoff_upper: 1.0 # cuboid_length_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 13 | cuboid_width_cutoff_upper: 1.0 # cuboid_width_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 14 | cuboid_height_cutoff_upper: 5 # cuboid_height_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 15 | assignment_threshold: 0.8 # assignment_threshold: Hungarian assignment cost to associate 2 cuboid centroids as one 16 | downsample_res: 0.1 # Resolution to perform voxel downsampling for instance point cloud accumulation 17 | num_instance_point_lim: 30000 # only keep the most recent num_points_limit_per_instance for any instance 18 | for_forestry_bags: False # run forestry data or not 19 | sim: False 20 | ########### THREE PARAMS ARE LINKED TOGETHER ########### 21 | expected_segmentation_rate: 10 # in Hz 22 | time_to_initialize_cuboid: 0.2 # time_to_initialize_cuboid: Minimum time required to track a cuboid per lidar scan 23 | time_to_delete_lost_track_cuboid: 5 # time_to_delete_lost_track_cuboid: Lower will delete tracks faster and will cause new tracks to be initiated at the same location 24 | ######################################################## 25 | 26 | 27 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_real_robot_full_mission_loop_closure.yaml: -------------------------------------------------------------------------------- 1 | ground_class_label: 0 # the label corresponds to ground class 2 | car_class_label: 1 # the label corresponds to vehicle class 3 | tree_trunk_class_label: 8 # the label corresponds to tree class 4 | light_pole_class_label: 9 # the label corresponds to light pole class 5 | desired_acc_obj_pub_rate: 1 # in Hz 6 | valid_range_threshold: 6.5 # in meters 7 | epsilon_scan: 0.5 # epsilon_scan: determines the radius of clustering when considering neighboring points as single cluster. 8 | min_samples_scan: 300 # min_samples_scan : determines the minimum samples needed inside the epsilon radius of the cluster to be clustered together 9 | depth_percentile_lower: 20 # depth_percentile_lower: depth percentile for filtering out noisy points in depth camera 10 | depth_percentile_uppper: 70 # depth_percentile_uppper: depth percentile for filtering out noisy points in depth camera 11 | confidence_threshold: 0.2 # confidence_threshold: confidence threshold for filtering out noisy points in depth camera 12 | cuboid_length_cutoff_upper: 1.0 # cuboid_length_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 13 | cuboid_width_cutoff_upper: 1.0 # cuboid_width_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 14 | cuboid_height_cutoff_upper: 5 # cuboid_height_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 15 | assignment_threshold: 0.5 # assignment_threshold: Hungarian assignment cost to associate 2 cuboid centroids as one 16 | downsample_res: 0.02 # Resolution to perform voxel downsampling for instance point cloud accumulation 17 | num_instance_point_lim: 30000 # only keep the most recent num_points_limit_per_instance for any instance 18 | for_forestry_bags: False # run forestry data or not 19 | sim: False 20 | ########### THREE PARAMS ARE LINKED TOGETHER ########### 21 | expected_segmentation_rate: 10 # in Hz 22 | time_to_initialize_cuboid: 0.02 # time_to_initialize_cuboid: Minimum time required to track a cuboid per lidar scan 23 | time_to_delete_lost_track_cuboid: 4 # time_to_delete_lost_track_cuboid: Lower will delete tracks faster and will cause new tracks to be initiated at the same location 24 | ######################################################## 25 | 26 | 27 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/config/process_cloud_node_sim.yaml: -------------------------------------------------------------------------------- 1 | ground_class_label: 0 # the label corresponds to ground class 2 | car_class_label: 1 # the label corresponds to vehicle class 3 | tree_trunk_class_label: 8 # the label corresponds to tree class 4 | light_pole_class_label: 9 # the label corresponds to light pole class 5 | desired_acc_obj_pub_rate: 1 # in Hz 6 | valid_range_threshold: 6.0 # in meters 7 | epsilon_scan: 0.5 # epsilon_scan: determines the radius of clustering when considering neighboring points as single cluster. 8 | min_samples_scan: 100 # min_samples_scan : determines the minimum samples needed inside the epsilon radius of the cluster to be clustered together 9 | depth_percentile_lower: 30 # depth_percentile_lower: depth percentile for filtering out noisy points in depth camera 10 | depth_percentile_uppper: 50 # depth_percentile_uppper: depth percentile for filtering out noisy points in depth camera 11 | confidence_threshold: 0.5 # confidence_threshold: confidence threshold for filtering out noisy points in depth camera 12 | cuboid_length_cutoff_upper: 1.5 # cuboid_length_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 13 | cuboid_width_cutoff_upper: 1.5 # cuboid_width_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids 14 | cuboid_height_cutoff_upper: 5 # cuboid_height_cutoff_upper: cutoff for filtering out the FINAL false positive cuboids assignment_threshold: 1.0 # assignment_threshold: Hungarian assignment cost to associate 2 cuboid centroids as one 15 | downsample_res: 0.1 # Resolution to perform voxel downsampling for instance point cloud accumulation 16 | num_instance_point_lim: 30000 # only keep the most recent num_points_limit_per_instance for any instance 17 | for_forestry_bags: False # run forestry data or not 18 | sim: True 19 | ########### THREE PARAMS ARE LINKED TOGETHER ########### 20 | expected_segmentation_rate: 3 # in Hz 21 | time_to_initialize_cuboid: 0.3 # time_to_initialize_cuboid: Minimum time required to track a cuboid per lidar scan 22 | time_to_delete_lost_track_cuboid: 5 # time_to_delete_lost_track_cuboid: Lower will delete tracks faster and will cause new tracks to be initiated at the same location 23 | ######################################################## -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/closure_retrigger.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/infer_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/llol_robot_ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/ouster_decoder.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/play_bag_with_remapping.sh: -------------------------------------------------------------------------------- 1 | rosbag play robot1-2023-09-28-10-35-14.bag --topics /quadrotor1/lidar_odom /semantic_meas_sync_odom --remap /quadrotor1/lidar_odom:=/quadrotor1/lidar_odom /semantic_meas_sync_odom:=/quadrotor1/semantic_meas_sync_odom 2 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_lidar_indoor_with_ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_outdoor_kitti_with_ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_outdoor_with_ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_rgbd_indoor_open_vocab_with_ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/process_cloud_node_rgbd_indoor_with_ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/real_robot_process_cloud_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/record_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 26 | 27 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/record_bag_for_confidence_stats.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 14 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/record_bag_for_loop_closure.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 12 | 13 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/record_bag_for_running_process_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 15 | 16 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/run_flio_with_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/run_lidar_driver_decoder.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/run_llol_with_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/sim_perturb_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/sim_process_cloud_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/sloam_process_cloud_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/launch/throttle_and_remap_kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | \ 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | scan2shape_launch 4 | 0.0.0 5 | The scan2shape_launch package 6 | 7 | Xu Liu 8 | Ankit Prabhu 9 | 10 | Penn Software License 11 | 12 | catkin 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/script/backbone: -------------------------------------------------------------------------------- 1 | ../../script/backbone -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/script/decoder: -------------------------------------------------------------------------------- 1 | ../../script/decoder -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/script/infer_node.py: -------------------------------------------------------------------------------- 1 | ../../script/infer_node.py -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/script/process_cloud_node.py: -------------------------------------------------------------------------------- 1 | ../../script/process_cloud_node.py -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/script/process_cloud_node_lidar_indoor.py: -------------------------------------------------------------------------------- 1 | ../../script/process_cloud_node_lidar_indoor.py -------------------------------------------------------------------------------- /frontend/scan2shape/scan2shape_launch/script/process_cloud_node_outdoor.py: -------------------------------------------------------------------------------- 1 | ../../script/process_cloud_node_outdoor.py -------------------------------------------------------------------------------- /frontend/scan2shape/script/assignment.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import numpy as np 4 | from sklearn_matching_utils import linear_assignment as skla 5 | 6 | 7 | def pad_cost_matrix(cost_matrix, unassigned_cost): 8 | """ 9 | Pad cost matrix to handle unassignment 10 | :param cost_matrix: M x N matrix, M - tracks, N - detections 11 | :param unassigned_cost: cost of unassignment 12 | :return: padded cost matrix 13 | """ 14 | if unassigned_cost == 0.0: 15 | return cost_matrix 16 | 17 | r, c = np.shape(cost_matrix) 18 | # padded size 19 | n = r + c 20 | float_max = np.finfo(float).max 21 | padded_cost_matrix = np.ones((n, n)) * float_max 22 | 23 | # fill the padded cost matrix 24 | padded_cost_matrix[:r, :c] = cost_matrix 25 | padded_cost_matrix[r:, c:] = 0.0 26 | padded_cost_matrix[range(r), range(c, n)] = unassigned_cost 27 | padded_cost_matrix[range(r, n), range(c)] = unassigned_cost 28 | return padded_cost_matrix 29 | 30 | 31 | def hungarian_assignment(cost_matrix, unassigned_cost=1.2): 32 | """ 33 | This is the equivalent of matlab's assignDetectionsToTracks 34 | :param cost_matrix: M x N matrix, M - tracks, N - detections 35 | :param unassigned_cost: cost of unassignment 36 | :return: matches, unassigned tracks, unassigned detections 37 | """ 38 | padded_cost_matrix = pad_cost_matrix(cost_matrix, unassigned_cost) 39 | assignment = skla(padded_cost_matrix) 40 | 41 | # Figure out matches, unassigned bboxes1 and unassigned bboxes2 42 | n1, n2 = np.shape(cost_matrix) 43 | ind1 = assignment[:, 0] 44 | ind2 = assignment[:, 1] 45 | 46 | # Correct matches 47 | match_inds = (ind1 < n1) & (ind2 < n2) 48 | matches = assignment[match_inds] 49 | matches = np.atleast_2d(matches) 50 | 51 | # Unassigned bboxes1 and bboxes2 52 | un_ind1 = (ind1 < n1) & (ind2 >= n2) 53 | un_ind2 = (ind1 >= n1) & (ind2 < n2) 54 | 55 | unassigned1 = ind1[un_ind1] 56 | unassigned2 = ind2[un_ind2] 57 | 58 | return matches, unassigned1, unassigned2 59 | -------------------------------------------------------------------------------- /frontend/scan2shape/script/load_model.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | from segmentator import Segmentator 3 | import torch 4 | 5 | 6 | class Load_Model(): 7 | def __init__(self, model_path): 8 | self.model_path = model_path 9 | self.arch_configs = yaml.safe_load( 10 | open(self.model_path+"arch_cfg.yaml", 'r')) 11 | self.data_configs = yaml.safe_load( 12 | open(self.model_path+"data_cfg.yaml", 'r')) 13 | self.n_classes = len(self.data_configs["learning_map_inv"]) 14 | 15 | def load_model(self): 16 | with torch.no_grad(): 17 | model = Segmentator(ARCH=self.arch_configs, 18 | nclasses=self.n_classes, path=self.model_path) 19 | 20 | return model 21 | -------------------------------------------------------------------------------- /frontend/scan2shape/script/tmux_no_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SESSION_NAME=tmux_pipeline 4 | 5 | ## Aliases are not expanded in non-intereactive bash 6 | shopt -s expand_aliases 7 | source ~/.bash_aliases 8 | 9 | ## Check alias set 10 | if [ "$(type -t runyuezhansloamdocker)" = 'alias' ]; then 11 | echo 'runyuezhansloamdocker is an alias' 12 | else 13 | echo 'runyuezhansloamdocker is not an alias, please set it first!' 14 | exit 15 | fi 16 | if [ "$(type -t rundetectnode)" = 'alias' ]; then 17 | echo 'rundetectnode is an alias' 18 | else 19 | echo 'rundetectnode is not an alias, please set it first!' 20 | exit 21 | fi 22 | if [ "$(type -t runprocesscloudnode)" = 'alias' ]; then 23 | echo 'runprocesscloudnode is an alias' 24 | else 25 | echo 'runprocesscloudnode is not an alias, please set it first!' 26 | exit 27 | fi 28 | 29 | if [ -z ${TMUX} ]; 30 | then 31 | tmux has-session -t $SESSION_NAME 2>/dev/null 32 | if [ "$?" -eq 1 ] ; then 33 | # Set up session 34 | TMUX= tmux new-session -s $SESSION_NAME -d 35 | echo "Starting new session." 36 | else 37 | echo "Session exist, kill it first." 38 | fi 39 | else 40 | echo "Already in tmux, leave it first." 41 | exit 42 | fi 43 | 44 | # runyuezhansloamdocker = run_sloam_node_xu.sh 45 | tmux setw -g mouse on 46 | 47 | tmux rename-window -t $SESSION_NAME "Core" 48 | tmux send-keys -t $SESSION_NAME "roscore" Enter 49 | tmux split-window -t $SESSION_NAME 50 | tmux send-keys -t $SESSION_NAME "sleep 1; rosparam set /use_sim_time True" Enter 51 | 52 | tmux new-window -t $SESSION_NAME -n "Main" 53 | tmux send-keys -t $SESSION_NAME "rosbag play /home/sam/bags/yuezhan-bags/first_floor_handcarry_2023-04-04-14-11-02.bag --clock -r 2" 54 | tmux split-window -t $SESSION_NAME 55 | tmux send-keys -t $SESSION_NAME "sleep 1; roslaunch sloam run_indoor_large_scale_exploration.launch" Enter 56 | tmux split-window -t $SESSION_NAME 57 | tmux send-keys -t $SESSION_NAME "sleep 1; roslaunch rgb_sem_segmentation rgb_segmentation_bag.launch" Enter 58 | tmux split-window -t $SESSION_NAME 59 | tmux send-keys -t $SESSION_NAME "sleep 1; roscd scan2shape_launch; python ../script/process_cloud_node.py" Enter 60 | tmux select-layout -t $SESSION_NAME tiled 61 | 62 | tmux new-window -t $SESSION_NAME -n "Closure" 63 | tmux send-keys -t $SESSION_NAME "sleep 1; rosrun loop_closure loop_closure_server_node" Enter 64 | tmux split-window -t $SESSION_NAME 65 | tmux send-keys -t $SESSION_NAME "roscd scan2shape_launch; python ../script/pub_loop_closure_trigger.py" 66 | tmux select-layout -t $SESSION_NAME tiled 67 | 68 | 69 | tmux new-window -t $SESSION_NAME -n "Kill" 70 | tmux send-keys -t $SESSION_NAME "tmux kill-session -t tmux_pipeline" 71 | 72 | tmux select-window -t $SESSION_NAME:1 73 | tmux -2 attach-session -t $SESSION_NAME 74 | 75 | clear -------------------------------------------------------------------------------- /frontend/scan2shape/script/tmux_pipeline.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SESSION_NAME=tmux_pipeline 4 | 5 | ## Aliases are not expanded in non-intereactive bash 6 | shopt -s expand_aliases 7 | source ~/.bash_aliases 8 | 9 | ## Check alias set 10 | if [ "$(type -t runyuezhansloamdocker)" = 'alias' ]; then 11 | echo 'runyuezhansloamdocker is an alias' 12 | else 13 | echo 'runyuezhansloamdocker is not an alias, please set it first!' 14 | exit 15 | fi 16 | if [ "$(type -t rundetectnode)" = 'alias' ]; then 17 | echo 'rundetectnode is an alias' 18 | else 19 | echo 'rundetectnode is not an alias, please set it first!' 20 | exit 21 | fi 22 | if [ "$(type -t runprocesscloudnode)" = 'alias' ]; then 23 | echo 'runprocesscloudnode is an alias' 24 | else 25 | echo 'runprocesscloudnode is not an alias, please set it first!' 26 | exit 27 | fi 28 | 29 | if [ -z ${TMUX} ]; 30 | then 31 | tmux has-session -t $SESSION_NAME 2>/dev/null 32 | if [ "$?" -eq 1 ] ; then 33 | # Set up session 34 | TMUX= tmux new-session -s $SESSION_NAME -d 35 | echo "Starting new session." 36 | else 37 | echo "Session exist, kill it first." 38 | fi 39 | else 40 | echo "Already in tmux, leave it first." 41 | exit 42 | fi 43 | 44 | # runyuezhansloamdocker = run_sloam_node_xu.sh 45 | tmux setw -g mouse on 46 | 47 | tmux rename-window -t $SESSION_NAME "Core" 48 | tmux send-keys -t $SESSION_NAME "roscore" Enter 49 | tmux split-window -t $SESSION_NAME 50 | tmux send-keys -t $SESSION_NAME "sleep 1; rosparam set /use_sim_time True" Enter 51 | 52 | tmux new-window -t $SESSION_NAME -n "Main" 53 | tmux send-keys -t $SESSION_NAME "rosbag play ~/bag/pennovation/ --clock -r 2" 54 | tmux split-window -t $SESSION_NAME 55 | tmux send-keys -t $SESSION_NAME "sleep 1; runyuezhansloamdocker" Enter 56 | tmux split-window -t $SESSION_NAME 57 | tmux send-keys -t $SESSION_NAME "sleep 1; rundetectnode" Enter 58 | tmux split-window -t $SESSION_NAME 59 | tmux send-keys -t $SESSION_NAME "sleep 1; runprocesscloudnode" Enter 60 | tmux select-layout -t $SESSION_NAME tiled 61 | 62 | tmux new-window -t $SESSION_NAME -n "Closure" 63 | tmux send-keys -t $SESSION_NAME "docker exec -it sloam_ros bash -c /opt/generic-sloam_ws/src/docker/run_map_merging_node.sh" 64 | 65 | tmux new-window -t $SESSION_NAME -n "Kill" 66 | tmux send-keys -t $SESSION_NAME "docker stop sloam_ros -t 1; tmux kill-session -t tmux_pipeline" 67 | 68 | tmux select-window -t $SESSION_NAME:1 69 | tmux -2 attach-session -t $SESSION_NAME 70 | 71 | clear -------------------------------------------------------------------------------- /frontend/scan2shape/script/tmux_script_multi_robot_sloam.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SESSION_NAME=tmux_pipeline 4 | 5 | ## Aliases are not expanded in non-intereactive bash 6 | shopt -s expand_aliases 7 | source ~/.bash_aliases 8 | 9 | ## Check alias set 10 | if [ "$(type -t ssloam)" = 'alias' ]; then 11 | echo 'ssloam is an alias' 12 | else 13 | echo 'ssloam is not an alias, please set it first!' 14 | exit 15 | fi 16 | if [ "$(type -t runinfernode)" = 'alias' ]; then 17 | echo 'runinfernode is an alias' 18 | else 19 | echo 'runinfernode is not an alias, please set it first!' 20 | exit 21 | fi 22 | if [ "$(type -t runprocesscloudnode)" = 'alias' ]; then 23 | echo 'runprocesscloudnode is an alias' 24 | else 25 | echo 'runprocesscloudnode is not an alias, please set it first!' 26 | exit 27 | fi 28 | 29 | if [ -z ${TMUX} ]; 30 | then 31 | tmux has-session -t $SESSION_NAME 2>/dev/null 32 | if [ "$?" -eq 1 ] ; then 33 | # Set up session 34 | TMUX= tmux new-session -s $SESSION_NAME -d 35 | echo "Starting new session." 36 | else 37 | echo "Session exist, kill it first." 38 | fi 39 | else 40 | echo "Already in tmux, leave it first." 41 | exit 42 | fi 43 | 44 | # runyuezhansloamdocker = run_sloam_node_xu.sh 45 | tmux setw -g mouse on 46 | 47 | tmux rename-window -t $SESSION_NAME "Core" 48 | tmux send-keys -t $SESSION_NAME "roscore" Enter 49 | tmux split-window -t $SESSION_NAME 50 | tmux send-keys -t $SESSION_NAME "sleep 1; rosparam set /use_sim_time True" Enter 51 | 52 | tmux new-window -t $SESSION_NAME -n "Main" 53 | tmux send-keys -t $SESSION_NAME "rosbag play /home/sam/bags/forest-map-merging-starting-same-location-left-direction-loop-falcon_pennovation_2022-11-29-13-55-01.bag --clock --topics /os_node/lidar_packets /os_node/imu_packets /os_node/metadata -s 20" 54 | tmux split-window -t $SESSION_NAME 55 | tmux send-keys -t $SESSION_NAME "sleep 1; ssloam; roslaunch sloam run_pennovation.launch" Enter 56 | tmux split-window -t $SESSION_NAME 57 | tmux send-keys -t $SESSION_NAME "sleep 1; ssloam; runinfernode" Enter 58 | tmux split-window -t $SESSION_NAME 59 | tmux send-keys -t $SESSION_NAME "sleep 1; ssloam; runprocesscloudnode" Enter 60 | tmux select-layout -t $SESSION_NAME tiled 61 | 62 | tmux new-window -t $SESSION_NAME -n "Faster LIO" 63 | tmux send-keys -t $SESSION_NAME "sleep 1; ssloam; roslaunch scan2shape_launch run_flio_with_driver.launch" Enter 64 | 65 | tmux new-window -t $SESSION_NAME -n "Relay Topics" 66 | tmux send-keys -t $SESSION_NAME "sleep 1; ssloam; roslaunch scan2shape_launch relay_topics_multi_robot.launch" Enter 67 | 68 | tmux new-window -t $SESSION_NAME -n "Kill" 69 | tmux send-keys -t $SESSION_NAME "tmux kill-session -t tmux_pipeline" 70 | 71 | tmux select-window -t $SESSION_NAME:1 72 | tmux -2 attach-session -t $SESSION_NAME 73 | 74 | clear -------------------------------------------------------------------------------- /run_slide_slam_docker.sh: -------------------------------------------------------------------------------- 1 | SlideSlamWs="/home/sam/slideslam_docker_ws" # point to your workspace directory 2 | SlideSlamCodeDir="/home/sam/slideslam_docker_ws/src/SLIDE_SLAM" # point to your code directory where you cloned the repository 3 | BAGS_DIR='/home/sam/bags' # point to your bags / data directory 4 | 5 | xhost +local:root # for the lazy and reckless 6 | docker run -it \ 7 | --name="slideslam_ros" \ 8 | --net="host" \ 9 | --privileged \ 10 | --gpus="all" \ 11 | --workdir="/opt/slideslam_docker_ws" \ 12 | --env="DISPLAY=$DISPLAY" \ 13 | --env="QT_X11_NO_MITSHM=1" \ 14 | --env="XAUTHORITY=$XAUTH" \ 15 | --volume="$SlideSlamWs:/opt/slideslam_docker_ws" \ 16 | --volume="$SlideSlamCodeDir:$SlideSlamCodeDir" \ 17 | --volume="$BAGS_DIR:/opt/bags" \ 18 | --volume="/home/$USER/.bash_aliases:/root/.bash_aliases" \ 19 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 20 | --volume="/home/$USER/repos:/home/$USER/repos" \ 21 | xurobotics/slide-slam:latest \ 22 | bash 23 | 24 | --------------------------------------------------------------------------------