├── HumbleDockerfile ├── LICENSE ├── Makefile ├── README.md ├── resources ├── Successrateenv.png ├── frame.jpg ├── frame.png └── motivation.png └── ros_ws ├── Demo.sh ├── data ├── Env1.png ├── Env1.yaml ├── Env2.png ├── Env2.yaml ├── Env3.png ├── Env3.yaml ├── cmcl_realA.config └── cmcl_realB.config └── src └── cmcl ├── cmcl_msgs ├── CMakeLists.txt ├── msg │ ├── Cluster.msg │ ├── Coreset.msg │ ├── DET.msg │ ├── DistributionExchange.msg │ ├── KMeansCluster.msg │ ├── ParticleFilter.msg │ ├── Prediction.msg │ ├── ProrokClusterAbstraction.msg │ └── RobotDetection.msg └── package.xml └── cmcl_ros ├── CMakeLists.txt ├── include └── RosUtils.h ├── launch └── cmcl_real.launch ├── ncore ├── CMakeLists.txt ├── data │ └── config │ │ ├── Env2.png │ │ ├── Env2.yaml │ │ └── cmcl.config ├── include │ ├── BeamEnd.h │ ├── Cluster.h │ ├── ClusterAbstraction.h │ ├── DensityEstimationTree.h │ ├── DistributionData.h │ ├── DistributionExchange.h │ ├── FSR.h │ ├── GMap.h │ ├── GoodPointsCompression.h │ ├── KMeansClustering.h │ ├── Lidar2D.h │ ├── LidarData.h │ ├── MCL.h │ ├── Particle.h │ ├── ParticleFilter.h │ ├── ProrokClustering.h │ ├── Resampling.h │ ├── SetStatistics.h │ ├── StandardThinning.h │ └── Utils.h ├── src │ ├── BeamEnd.cpp │ ├── CMakeLists.txt │ ├── Cluster.cpp │ ├── ClusterAbstraction.cpp │ ├── DensityEstimationTree.cpp │ ├── DistributionExchange.cpp │ ├── FSR.cpp │ ├── GMap.cpp │ ├── GoodPointsCompression.cpp │ ├── KMeansClustering.cpp │ ├── Lidar2D.cpp │ ├── MCL.cpp │ ├── Particle.cpp │ ├── ParticleFilter.cpp │ ├── ProrokClustering.cpp │ ├── Resampling.cpp │ ├── SetStatistics.cpp │ ├── StandardThinning.cpp │ ├── Utils.cpp │ └── dist_compress.py └── tst │ ├── BenchmarkClustering.cpp │ ├── CMakeLists.txt │ ├── Clustering.pdf │ ├── Clustering4.pdf │ ├── ClusteringNotSym.pdf │ ├── Clusteringfail.pdf │ ├── Clusteringsuccess.pdf │ ├── DumpClustering.cpp │ ├── TestUtils.cpp │ ├── TestUtils.h │ ├── UnitTests.cpp │ └── particle_test.py ├── package.xml ├── rviz └── cmcl_real.rviz └── src ├── Depth4DetectionNode.cpp ├── DetectionNoiseNode.cpp ├── EvalNode.cpp ├── LidarScan2PointCloudNode.cpp ├── MCLNode.cpp ├── MapLoaderNode.cpp ├── OprtitrackPublisherNode.cpp ├── RelativePosedNode.cpp ├── RosUtils.cpp └── UVDetection2WorldNode.cpp /HumbleDockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:humble 2 | ENV DEBIAN_FRONTEND noninteractive 3 | ENV NVIDIA_VISIBLE_DEVICES all 4 | ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute 5 | 6 | RUN apt-get update && apt-get install -y \ 7 | locales \ 8 | python3-pip \ 9 | ros-humble-xacro \ 10 | ros-humble-launch-xml \ 11 | ros-humble-cv-bridge \ 12 | ros-humble-launch-testing-ament-cmake \ 13 | ros-humble-robot-state-publisher \ 14 | ros-humble-joint-state-publisher \ 15 | ros-humble-joint-state-publisher-gui \ 16 | ros-humble-joy \ 17 | ros-humble-joy-teleop \ 18 | ros-humble-tf-transformations \ 19 | ros-humble-pcl-conversions \ 20 | ros-humble-pcl-msgs \ 21 | ros-humble-pcl-ros \ 22 | libopus-dev \ 23 | nano \ 24 | && rm -rf /var/lib/apt/lists/* 25 | 26 | RUN locale-gen en_US.UTF-8 27 | 28 | SHELL ["/bin/bash", "-c"] 29 | 30 | RUN python3 -m pip install -U numpy numpy-quaternion pyyaml 31 | RUN apt-get update && \ 32 | apt-get install -y nlohmann-json3-dev libgtest-dev libeigen3-dev libarmadillo-dev libensmallen-dev libcereal-dev libstb-dev g++ cmake && rm -rf /var/lib/apt/lists/* 33 | RUN mkdir -p /tmp && cd /tmp && git clone https://github.com/mlpack/mlpack.git && cd /tmp/mlpack && cmake -S . -B build && cmake --build build --target install && rm -rf /tmp 34 | 35 | RUN mkdir /tmp 36 | RUN chmod 1777 /tmp 37 | RUN apt-get update && apt-get install -y \ 38 | ros-humble-rviz2 \ 39 | ros-humble-navigation2 \ 40 | ros-humble-nav2-bringup \ 41 | ros-humble-slam-toolbox \ 42 | ros-humble-rqt \ 43 | ros-humble-rqt-image-view \ 44 | ros-humble-rqt-tf-tree \ 45 | ros-humble-joy \ 46 | ros-humble-joy-teleop \ 47 | ros-humble-rosbag2-storage-mcap \ 48 | && rm -rf /var/lib/apt/lists/* 49 | 50 | 51 | 52 | RUN python3 -m pip install goodpoints pybind11 pandas 53 | 54 | COPY ros_ws ros_ws 55 | 56 | RUN cd /usr/include/ && sudo ln -sf eigen3/Eigen Eigen 57 | 58 | RUN cd /ros_ws/src/cmcl/cmcl_ros/ncore && cmake -S . -B build && cmake --build build 59 | 60 | RUN cd /ros_ws && source /ros_entrypoint.sh && colcon build --packages-up-to robomaster_ros 61 | 62 | RUN cd /ros_ws && source /ros_entrypoint.sh && \ 63 | colcon build --packages-select cmcl_msgs cmcl_ros --cmake-clean-cache && rm -r build log 64 | 65 | 66 | WORKDIR /ros_ws 67 | 68 | 69 | #RUN chmod 1777 /tmp 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 IDSIA Robotics Group 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. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | build: 2 | ifdef foxy 3 | docker build -f Dockerfile -t mclfoxy . 4 | endif 5 | ifdef humble 6 | docker build -f HumbleDockerfile -t mclhumble . 7 | endif 8 | 9 | run: 10 | ifdef foxy 11 | docker run --rm -it --init --gpus=all --network host --pid host --ipc host -e "DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 12 | -v $(CURDIR)/ros_ws:/ros_ws mclfoxy 13 | endif 14 | ifdef humble 15 | docker run --rm -it --init --gpus=all --privileged --network host --pid host --ipc host -e "DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 16 | -v $(CURDIR)/ros_ws:/ros_ws mclhumble 17 | endif 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Resource-Aware Collaborative Monte Carlo Localization with Distribution Compression 2 | This repository contains the implementation of the following [publication](https://arxiv.org/abs/2404.02010): 3 | ```bibtex 4 | @misc{zimmerman2024arxiv, 5 | title={{Resource-Aware Collaborative Monte Carlo Localization with Distribution Compression}}, 6 | author={Nicky Zimmerman and Alessandro Giusti and Jérôme Guzzi}, 7 | year={2024}, 8 | eprint={2404.02010}, 9 | archivePrefix={arXiv}, 10 | primaryClass={cs.RO} 11 | } 12 | ``` 13 | Under review for IROS 2024. Our live demo: 14 | 15 | [![DEMO](resources/frame.png)](https://www.youtube.com/watch?v=dgssNAHPvvM) 16 | 17 | 18 | 19 | ## Abstract 20 | Global localization is essential in enabling robot autonomy, and collaborative localization is key for multi-robot systems. 21 | In this paper, we address the task of collaborative global localization under computational and communication constraints. We propose a method which reduces the amount of information exchanged and the computational cost. We also analyze, implement and open-source seminal approaches, which we believe to be a valuable contribution to the community. 22 | We exploit techniques for distribution compression in near-linear time, with error guarantees. 23 | We evaluate our approach and the implemented baselines on multiple challenging scenarios, simulated and real-world. Our approach can run online on an onboard computer. We release an open-source C++/ROS2 implementation of our approach, as well as the baselines. 24 |

25 | 26 |

27 | 28 | ## Results 29 | We conducted a thorough evaluation of the different approaches to cooperative localization. We present our experiments to show the capabilities of our method, Compress++ MCL. 30 | The results support the claims that our proposed approach (i) improves collaborative localization, (ii) decreases the required band-width, (iii) reduces the computational load, (iv) runs online 31 | on an onboard computer. 32 | 33 |

34 | 35 |

36 | 37 | ## Installation 38 | We provide Docker installations for ROS 2 Humble. Make sure you installed Docker Engine and NVidia container so you can run Dokcer with GPU support. 39 | Download the external resources in the host machine into the `/ros_ws/src` directory 40 | ```bash 41 | git clone git@github.com:jeguzzi/robomaster_ros.git 42 | ``` 43 | The package is necessarily for running the demo as the rosbag includes RoboMaster specific detection messages. However, they are not strictly necessary for any of the approaches, and the dependency can be removed with a bit of work on the ROS wrapper. In the root directory, run 44 | ```bash 45 | sudo make humble=1 46 | ``` 47 | To build the Docker. To enable RVIZ visualization from the Docker run in the host machine 48 | ```bash 49 | xhost +local:docker 50 | ``` 51 | To run the Docker 52 | ```bash 53 | sudo make run humble=1 54 | ``` 55 | The core code is C++ with no ROS dependency. To build the ncore library in the docker, run 56 | ```bash 57 | cd src/cmcl/cmcl_ros/ncore && mkdir build && cd build && cmake .. -DBUILD_TESTING=1 && make -j12 58 | ./bin/UnitTests 59 | ``` 60 | Run the unit tests to verify the installation is successful. Then to build the ROS 2 wrapper 61 | ```bash 62 | cd /ros_ws && . /opt/ros/humble/setup.bash && colcon build && . install/setup.bash 63 | ``` 64 | 65 | ## Running the Algo 66 | You can download the rosbag from [here](https://drive.switch.ch/index.php/s/Of0l9iPzjmQ8CXc). place it in the `/ros_ws/rosbags` directory and you can use the Demo.sh script to run it. 67 | Remember to enable RVIZ in your docker. 68 | 69 | ### MCLNode 70 | The main node which is a ROS 2 wrapper to the C++ implementation of the different approaches. 71 | Requires as input a minimum of 2 topics, Odometry and PointCloud2, for the single robot range-based MCL. To enable collaborative localization, RobotDetection messages are needed. 72 | These messages are agnostic to detection algorithm and if you wish to use your own detection pipeline, just write a ROS node that publishes these messages. 73 | The topic names are configurable through the launch file. This runs the correct and predict steps asynchronously, so correct is executed whenever we have odometry and predict is executed when an observation arrives. 74 | MCLNodE publishes PoseWithCovarianceStamped messages with the pose prediction. To see the predictions in RVIZ, run 75 | ```bash 76 | ros2 launch cmcl_ros cmcl_real.launch 77 | ``` 78 | 79 | ### LidarScan2PointCloudNode 80 | This nodes converts the LaserScan messages from our YDLIDAR Tmini Pro 2D LiDAR to a point cloud in base_link frame. 81 | 82 | ### DetectionNoiseNode 83 | This node is used to add noise to "perfect" detections generated by our simulation, to make the scenarios more realistic. 84 | 85 | ### UVDetection2WorldNode 86 | This node takes as input the RoboMaster Detection messages, which are 2D bounding boxes in image coordinates, and provides a 3D position of the detected robot in the base_link frame. 87 | The distance to the detected robot is computed using the known dimensions of the RoboMaster and the 2D bounding boxes. 88 | 89 | ### EvalNode 90 | This node was used to log the localization predictions for the evluation. 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /resources/Successrateenv.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/resources/Successrateenv.png -------------------------------------------------------------------------------- /resources/frame.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/resources/frame.jpg -------------------------------------------------------------------------------- /resources/frame.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/resources/frame.png -------------------------------------------------------------------------------- /resources/motivation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/resources/motivation.png -------------------------------------------------------------------------------- /ros_ws/Demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | 4 | #find . -name "*.csv" -type f -delete 5 | 6 | 7 | declare -a DIST 8 | DIST[0]="-1" 9 | DIST[1]="1" 10 | DIST[2]="3" 11 | DIST[3]="2" 12 | DIST[4]="4" 13 | DIST[5]="0" 14 | DIST[6]="0" 15 | 16 | declare -a DETECT 17 | DETECT[0]="false" 18 | DETECT[1]="true" 19 | DETECT[2]="true" 20 | DETECT[3]="true" 21 | DETECT[4]="true" 22 | DETECT[5]="true" 23 | DETECT[6]="true" 24 | 25 | declare -a ALGO 26 | ALGO[0]="MCL" 27 | ALGO[1]="FOX_CMCL" 28 | ALGO[2]="PROROK_CMCL" 29 | ALGO[3]="KMEANS_CMCL" 30 | ALGO[4]="GOODPOINTS_CMCL" 31 | ALGO[5]="THIN_CMCL" 32 | ALGO[6]="NAIVE_CMCL" 33 | 34 | declare -a MAP 35 | MAP[0]="Env1" 36 | MAP[1]="Env2" 37 | MAP[2]="Env3" 38 | 39 | 40 | particleNum=10000 41 | path1="/ros_ws/dump/" 42 | rosbagDir="/ros_ws/rosbags/" 43 | seq="demo" 44 | m=2 45 | mapName=${MAP[ $m ]} 46 | 47 | source /ros_ws/install/setup.bash 48 | 49 | a=4 50 | run=0 51 | detect=${DETECT[ $a ]} 52 | dist=${DIST[ $a ]} 53 | algo=${ALGO[ $a ]} 54 | csv="$path1/$algo/$mapName/S$s/" 55 | echo $csv 56 | 57 | 58 | ros2 launch cmcl_ros cmcl_real.launch mapName:="$mapName" detection:="$detect" distType:="$dist" csvPath:="$csv" run:="$run" & 59 | sleep 5 60 | ros2 lifecycle set /map_server activate 61 | rosbagPath="$rosbagDir/$seq/" 62 | echo $rosbagPath 63 | ros2 bag play $rosbagPath 64 | 65 | kill $(pgrep -f rviz2) 66 | kill $(pgrep -f DetectionNoiseNode) 67 | kill $(pgrep -f MCLNode) 68 | kill $(pgrep -f LidarScan2PointCloudNode) 69 | kill $(pgrep -f EvalNode) 70 | kill $(pgrep -f map_server) 71 | kill $(pgrep -f UVDetection2WorldNode) 72 | kill $(pgrep -f RelativePosedNode) 73 | kill $(pgrep -f cmcl.launch) 74 | kill $(pgrep -f OprtitrackPublisherNode) 75 | pkill -9 ros2 76 | ros2 daemon stop 77 | 78 | -------------------------------------------------------------------------------- /ros_ws/data/Env1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/data/Env1.png -------------------------------------------------------------------------------- /ros_ws/data/Env1.yaml: -------------------------------------------------------------------------------- 1 | image: Env1.png 2 | resolution: 0.05 3 | origin: [-16.0, -16.0, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.25 7 | mode: trinary 8 | -------------------------------------------------------------------------------- /ros_ws/data/Env2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/data/Env2.png -------------------------------------------------------------------------------- /ros_ws/data/Env2.yaml: -------------------------------------------------------------------------------- 1 | image: Env2.png 2 | resolution: 0.05 3 | origin: [-16, -16, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.25 7 | mode: trinary 8 | -------------------------------------------------------------------------------- /ros_ws/data/Env3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/data/Env3.png -------------------------------------------------------------------------------- /ros_ws/data/Env3.yaml: -------------------------------------------------------------------------------- 1 | image: Env3.png 2 | resolution: 0.05 3 | origin: [-10.0, -10.0, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.25 7 | mode: trinary 8 | -------------------------------------------------------------------------------- /ros_ws/data/cmcl_realA.config: -------------------------------------------------------------------------------- 1 | { 2 | "mapPath": "Env3.yaml", 3 | "numParticles": 2000, 4 | "sensorModel": { 5 | "likelihoodSigma": 0.15, 6 | "maxDist": 20.0, 7 | "type": "BeamEnd", 8 | "weightingScheme": 1 9 | }, 10 | "detectionModel": { 11 | "kmeans": 12 | { 13 | "clusters": 8, 14 | "iterations" : 5 15 | }, 16 | "det": 17 | { 18 | "minSize": 500, 19 | "maxSize": 1000 20 | }, 21 | "prorok": 22 | { 23 | "clusters": 8 24 | }, 25 | "thinning": 26 | { 27 | "clusters": 64 28 | }, 29 | "detectionNoise": 30 | { 31 | "x": 0.1, 32 | "y": 0.1 33 | }, 34 | "penalty" : 0.000000001, 35 | "alpha" : 0.06, 36 | "clusters": 10000, 37 | "type" : 3 38 | }, 39 | "tracking": 40 | { 41 | "mode" : false, 42 | "x" : [8.61341, 8.61341], 43 | "y" : [-3.3749, -3.3749], 44 | "yaw" : [0.03733, 0.03733] , 45 | "cov_x" : [0.5, 0.5], 46 | "cov_y" : [0.5, 0.5], 47 | "cov_yaw" : [0.5, 0.5] 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /ros_ws/data/cmcl_realB.config: -------------------------------------------------------------------------------- 1 | { 2 | "mapPath": "Env3.yaml", 3 | "numParticles": 2000, 4 | "sensorModel": { 5 | "likelihoodSigma": 0.3, 6 | "maxDist": 20.0, 7 | "type": "BeamEnd", 8 | "weightingScheme": 1 9 | }, 10 | "detectionModel": { 11 | "kmeans": 12 | { 13 | "clusters": 8, 14 | "iterations" : 5 15 | }, 16 | "det": 17 | { 18 | "minSize": 500, 19 | "maxSize": 1000 20 | }, 21 | "prorok": 22 | { 23 | "clusters": 8 24 | }, 25 | "thinning": 26 | { 27 | "clusters": 64 28 | }, 29 | "detectionNoise": 30 | { 31 | "x": 0.1, 32 | "y": 0.1 33 | }, 34 | "penalty" : 0.000000001, 35 | "alpha" : 0.06, 36 | "clusters": 10000, 37 | "type" : 3 38 | }, 39 | "tracking": 40 | { 41 | "mode" : false, 42 | "x" : [8.61341, 8.61341], 43 | "y" : [-3.3749, -3.3749], 44 | "yaw" : [0.03733, 0.03733] , 45 | "cov_x" : [0.5, 0.5], 46 | "cov_y" : [0.5, 0.5], 47 | "cov_yaw" : [0.5, 0.5] 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cmcl_msgs) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | find_package(std_msgs REQUIRED) 25 | find_package(geometry_msgs REQUIRED) 26 | find_package(rosidl_default_generators REQUIRED) 27 | 28 | rosidl_generate_interfaces(${PROJECT_NAME} 29 | "msg/ProrokClusterAbstraction.msg" 30 | "msg/KMeansCluster.msg" 31 | "msg/DET.msg" 32 | "msg/Coreset.msg" 33 | "msg/ParticleFilter.msg" 34 | "msg/Cluster.msg" 35 | "msg/Prediction.msg" 36 | "msg/DistributionExchange.msg" 37 | "msg/RobotDetection.msg" 38 | DEPENDENCIES geometry_msgs std_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg 39 | ) 40 | 41 | if(BUILD_TESTING) 42 | find_package(ament_lint_auto REQUIRED) 43 | # the following line skips the linter which checks for copyrights 44 | # uncomment the line when a copyright and license is not present in all source files 45 | #set(ament_cmake_copyright_FOUND TRUE) 46 | # the following line skips cpplint (only works in a git repo) 47 | # uncomment the line when this package is not in a git repo 48 | #set(ament_cmake_cpplint_FOUND TRUE) 49 | ament_lint_auto_find_test_dependencies() 50 | endif() 51 | 52 | ament_package() 53 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/Cluster.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float32[] centroid 3 | float32[] mu 4 | float32[] variance 5 | float32 weight -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/Coreset.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Pose[] particles 3 | float32[] weights 4 | float32[] variance 5 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/DET.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8[] tree 3 | float32[] variance 4 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/DistributionExchange.msg: -------------------------------------------------------------------------------- 1 | 2 | std_msgs/Header header 3 | uint8 type 4 | int32 observer_id 5 | int32 detected_id 6 | cmcl_msgs/ParticleFilter filter 7 | cmcl_msgs/Coreset coreset 8 | cmcl_msgs/DET det 9 | cmcl_msgs/Cluster[] clusters 10 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/KMeansCluster.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float32[] mean 3 | float32[] variance 4 | float32 weight 5 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/ParticleFilter.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Pose[] particles 3 | float32[] weights 4 | float32[] variance 5 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/Prediction.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 type 3 | geometry_msgs/PoseWithCovarianceStamped pose 4 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/ProrokClusterAbstraction.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float32[] centroid 3 | float32[] mu 4 | float32[] variance 5 | float32 weight -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/msg/RobotDetection.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | int32 observer_id 3 | int32 detected_id 4 | geometry_msgs/Pose pose 5 | uint8 type -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cmcl_msgs 5 | 0.0.0 6 | TODO: Package description 7 | nickybones 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | geometry_msgs 15 | std_msgs 16 | 17 | rosidl_default_generators 18 | 19 | rosidl_default_runtime 20 | 21 | rosidl_interface_packages 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cmcl_ros) 3 | 4 | SET(OpenCV_MIN_VERSION "4.2.0") 5 | 6 | 7 | #colcon build --allow-overriding cv_bridge image_geometry 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | #add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | 19 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) 20 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/) 21 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) 22 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/ncore/include/) 23 | 24 | 25 | set(RESULTS_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/results) 26 | file(MAKE_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}) 27 | 28 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 29 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 30 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/bin) 31 | link_directories(${CMAKE_CURRENT_SOURCE_DIR}/ncore/build/lib) 32 | 33 | 34 | 35 | 36 | # find dependencies 37 | find_package(ament_cmake REQUIRED) 38 | find_package(rclcpp REQUIRED) 39 | find_package(sensor_msgs REQUIRED) 40 | find_package(std_msgs REQUIRED) 41 | find_package(nav_msgs REQUIRED) 42 | find_package(geometry_msgs REQUIRED) 43 | find_package(message_filters REQUIRED) 44 | find_package(tf2 REQUIRED) 45 | find_package(tf2_ros REQUIRED) 46 | find_package(tf2_msgs REQUIRED) 47 | find_package(Eigen3 REQUIRED) 48 | find_package(OpenCV ${OpenCV_MIN_VERSION} REQUIRED) 49 | find_package(pcl_conversions REQUIRED) 50 | find_package(nav2_map_server REQUIRED) 51 | find_package(pcl_ros REQUIRED) 52 | find_package(cmcl_msgs REQUIRED) 53 | find_package(robomaster_msgs REQUIRED) 54 | find_package(cv_bridge REQUIRED) 55 | find_package(Boost REQUIRED COMPONENTS system) 56 | find_package(nlohmann_json 3.2.0 REQUIRED) 57 | find_package(OpenMP) 58 | if (OPENMP_FOUND) 59 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 60 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 61 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 62 | endif() 63 | 64 | set(pybind11_DIR /usr/local/lib/python3.10/dist-packages/) 65 | set(PYTHON_INCLUDE_DIR "/usr/include/python3.10") 66 | find_package (Python COMPONENTS Interpreter Development) 67 | if(Python_FOUND) 68 | message(STATUS "Found Python version ${Python_VERSION}") 69 | include_directories(${PYTHON_INCLUDE_DIR}) 70 | endif(Python_FOUND) 71 | find_package(pybind11 REQUIRED) 72 | 73 | find_package(Armadillo REQUIRED) 74 | if (ARMADILLO_FOUND) 75 | include_directories(${ARMADILLO_INCLUDE_DIRS}) 76 | endif() 77 | 78 | 79 | find_package(LAPACK REQUIRED) 80 | include_directories(${LAPACK_INCLUDE_DIRS}) 81 | 82 | if(BUILD_TESTING) 83 | find_package(ament_lint_auto REQUIRED) 84 | # the following line skips the linter which checks for copyrights 85 | # uncomment the line when a copyright and license is not present in all source files 86 | #set(ament_cmake_copyright_FOUND TRUE) 87 | # the following line skips cpplint (only works in a git repo) 88 | # uncomment the line when this package is not in a git repo 89 | #set(ament_cmake_cpplint_FOUND TRUE) 90 | ament_lint_auto_find_test_dependencies() 91 | endif() 92 | 93 | #add_subdirectory("src") 94 | add_library(NCORE STATIC IMPORTED) 95 | set_target_properties(NCORE PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/ncore/build/lib/libNCORE.a) 96 | 97 | add_executable(MCLNode src/MCLNode.cpp src/RosUtils.cpp) 98 | add_executable(LidarScan2PointCloudNode src/LidarScan2PointCloudNode.cpp src/RosUtils.cpp) 99 | add_executable(EvalNode src/EvalNode.cpp src/RosUtils.cpp) 100 | add_executable(Depth4DetectionNode src/Depth4DetectionNode.cpp src/RosUtils.cpp) 101 | add_executable(UVDetection2WorldNode src/UVDetection2WorldNode.cpp src/RosUtils.cpp) 102 | add_executable(DetectionNoiseNode src/DetectionNoiseNode.cpp src/RosUtils.cpp) 103 | add_executable(RelativePosedNode src/RelativePosedNode.cpp src/RosUtils.cpp) 104 | add_executable(OprtitrackPublisherNode src/OprtitrackPublisherNode.cpp src/RosUtils.cpp) 105 | add_executable(MapLoaderNode src/MapLoaderNode.cpp src/RosUtils.cpp) 106 | 107 | 108 | 109 | target_link_libraries(MCLNode NCORE ${Boost_SYSTEM_LIBRARY} ${ARMADILLO_LIBRARIES} ${LAPACK_LIBRARIES} pybind11::embed) 110 | target_link_libraries(LidarScan2PointCloudNode NCORE ${Boost_SYSTEM_LIBRARY} pybind11::embed) 111 | target_link_libraries(EvalNode NCORE ${Boost_SYSTEM_LIBRARY} pybind11::embed) 112 | target_link_libraries(Depth4DetectionNode NCORE ${Boost_SYSTEM_LIBRARY} pybind11::embed) 113 | target_link_libraries(UVDetection2WorldNode NCORE ${Boost_SYSTEM_LIBRARY} pybind11::embed) 114 | target_link_libraries(DetectionNoiseNode NCORE ${Boost_SYSTEM_LIBRARY} pybind11::embed) 115 | target_link_libraries(RelativePosedNode NCORE ${Boost_SYSTEM_LIBRARY} pybind11::embed) 116 | target_link_libraries(OprtitrackPublisherNode NCORE ${Boost_SYSTEM_LIBRARY} pybind11::embed) 117 | target_link_libraries(MapLoaderNode NCORE ${Boost_SYSTEM_LIBRARY}) 118 | 119 | 120 | ament_target_dependencies(MCLNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros pcl_conversions nlohmann_json OpenCV) 121 | 122 | ament_target_dependencies(LidarScan2PointCloudNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros pcl_conversions nlohmann_json OpenCV) 123 | 124 | 125 | ament_target_dependencies(EvalNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros pcl_conversions nlohmann_json OpenCV) 126 | 127 | ament_target_dependencies(Depth4DetectionNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs robomaster_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros pcl_conversions nlohmann_json OpenCV) 128 | 129 | ament_target_dependencies(UVDetection2WorldNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs robomaster_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros cv_bridge pcl_conversions nlohmann_json OpenCV) 130 | 131 | ament_target_dependencies(DetectionNoiseNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs robomaster_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros cv_bridge pcl_conversions nlohmann_json OpenCV) 132 | 133 | ament_target_dependencies(RelativePosedNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs robomaster_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros cv_bridge pcl_conversions nlohmann_json OpenCV) 134 | 135 | 136 | ament_target_dependencies(OprtitrackPublisherNode rclcpp std_msgs sensor_msgs nav_msgs cmcl_msgs robomaster_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros cv_bridge pcl_conversions nlohmann_json OpenCV) 137 | 138 | ament_target_dependencies(MapLoaderNode rclcpp std_msgs sensor_msgs nav2_map_server nav_msgs cmcl_msgs robomaster_msgs geometry_msgs message_filters tf2 tf2_ros tf2_msgs pcl_ros cv_bridge pcl_conversions nlohmann_json OpenCV) 139 | 140 | 141 | install(TARGETS 142 | MCLNode 143 | LidarScan2PointCloudNode 144 | EvalNode 145 | Depth4DetectionNode 146 | UVDetection2WorldNode 147 | DetectionNoiseNode 148 | RelativePosedNode 149 | OprtitrackPublisherNode 150 | MapLoaderNode 151 | DESTINATION lib/${PROJECT_NAME} 152 | ) 153 | 154 | 155 | install(DIRECTORY 156 | #python_cmcl 157 | DESTINATION lib/${PROJECT_NAME} 158 | ) 159 | 160 | install(DIRECTORY 161 | launch 162 | DESTINATION share/${PROJECT_NAME} 163 | ) 164 | 165 | install(DIRECTORY 166 | #config 167 | DESTINATION share/${PROJECT_NAME} 168 | ) 169 | 170 | install(DIRECTORY 171 | rviz 172 | DESTINATION share/${PROJECT_NAME} 173 | ) 174 | 175 | ament_package() 176 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/include/RosUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: RosUtils.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef ROSUTILS_H 14 | #define ROSUTILS_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | //#include 22 | //#include 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | // Eigen::Vector3f OdomMsg2Pose2D(const nav_msgs::msg::Odometry::ConstSharedPtr odom); 29 | 30 | // Eigen::Vector3f PoseMsg2Pose2D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& poseMsg); 31 | 32 | // std::vector OdomMsg2Pose3D(const nav_msgs::msg::Odometry::ConstSharedPtr odom); 33 | 34 | // std::vector PoseMsg2Pose3D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& odom); 35 | 36 | // geometry_msgs::msg::PoseStamped Pose2D2PoseMsg(Eigen::Vector3f pose2d); 37 | 38 | 39 | // //pcl::PointCloud Vec2PointCloud(const std::vector& points3d, Eigen::Vector3f rgb); 40 | 41 | 42 | // geometry_msgs::msg::PoseWithCovarianceStamped Pred2PoseWithCov(Eigen::Vector3d pred, Eigen::Matrix3d cov); 43 | 44 | 45 | Eigen::Vector3f OdomMsg2Pose2D(const nav_msgs::msg::Odometry::ConstSharedPtr& odom); 46 | 47 | Eigen::Vector3f PoseMsg2Pose2D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& poseMsg); 48 | 49 | std::vector OdomMsg2Pose3D(const nav_msgs::msg::Odometry::ConstSharedPtr& odom); 50 | 51 | std::vector PoseMsg2Pose3D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& odom); 52 | 53 | geometry_msgs::msg::PoseStamped Pose2D2PoseMsg(Eigen::Vector3f pose2d); 54 | 55 | 56 | pcl::PointCloud Vec2PointCloud(const std::vector& points3d, Eigen::Vector3f rgb); 57 | 58 | pcl::PointCloud Vec2PointCloud(const std::vector& points3d); 59 | 60 | pcl::PointCloud Vec2RGBPointCloud(const std::vector& points3d, std::vector rgb); 61 | 62 | 63 | pcl::PointCloud ManyVec2PointCloud(const std::vector>& points3d, std::vector rgb); 64 | 65 | 66 | geometry_msgs::msg::PoseWithCovarianceStamped Pred2PoseWithCov(Eigen::Vector3d pred, Eigen::Matrix3d cov); 67 | 68 | //sensor_msgs::msg::Image CVMat2ImgMsg(const cv::Mat& img, std_msgs::Header header, const std::string& type); 69 | 70 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/launch/cmcl_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 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 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | project(ncore) 3 | SET(OpenCV_MIN_VERSION "4.2.0") 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -fopenmp -Wall ${CMAKE_CXX_FLAGS}") 6 | set(CMAKE_CXX_STANDARD 14) 7 | 8 | find_package(OpenMP) 9 | if (OPENMP_FOUND) 10 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 11 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 12 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 13 | endif() 14 | 15 | 16 | 17 | find_package(Eigen3 REQUIRED) 18 | if(Eigen3_FOUND) 19 | message(STATUS "Found OpenCV version ${Eigen3}") 20 | message(STATUS "OpenCV directories: ${Eigen3_INCLUDE_DIRS}") 21 | include_directories(${Eigen3_INCLUDE_DIRS}) 22 | else() 23 | message(FATAL_ERROR "Eigen3 not found, please read the README.md") 24 | endif(Eigen3_FOUND) 25 | set(pybind11_DIR /usr/local/lib/python3.10/dist-packages/) 26 | set(PYTHON_INCLUDE_DIR "/usr/include/python3.10") 27 | find_package (Python COMPONENTS Interpreter Development) 28 | if(Python_FOUND) 29 | message(STATUS "Found Python version ${Python_VERSION}") 30 | include_directories(${PYTHON_INCLUDE_DIR}) 31 | endif(Python_FOUND) 32 | find_package(pybind11 REQUIRED) 33 | 34 | 35 | find_package(OpenCV ${OpenCV_MIN_VERSION} REQUIRED) 36 | if(OpenCV_FOUND) 37 | message(STATUS "Found OpenCV version ${OpenCV_VERSION}") 38 | message(STATUS "OpenCV directories: ${OpenCV_INCLUDE_DIRS}") 39 | include_directories(${OpenCV_INCLUDE_DIRS}) 40 | else() 41 | message(FATAL_ERROR "OpenCV not found, please read the README.md") 42 | endif(OpenCV_FOUND) 43 | 44 | 45 | find_package(Boost COMPONENTS serialization system filesystem) 46 | if(Boost_FOUND) 47 | message(STATUS "Found Boost version ${Boost_VERSION}") 48 | message(STATUS "Boost directories: ${Boost_INCLUDE_DIRS}") 49 | include_directories(${Boost_INCLUDE_DIRS}) 50 | else() 51 | message(FATAL_ERROR "Boost not found, please read the README.md") 52 | endif(Boost_FOUND) 53 | 54 | find_package(nlohmann_json 3.2.0 REQUIRED) 55 | if(nlohmann_json_FOUND) 56 | message(STATUS "Found nlohmann_json") 57 | else() 58 | message(FATAL_ERROR "nlohmann_json not found, please read the README.md") 59 | endif(nlohmann_json_FOUND) 60 | 61 | find_package(Armadillo REQUIRED) 62 | include_directories(${ARMADILLO_INCLUDE_DIRS}) 63 | find_package(LAPACK REQUIRED) 64 | include_directories(${LAPACK_INCLUDE_DIRS}) 65 | 66 | if(BUILD_TESTING) 67 | enable_testing() 68 | find_package(GTest REQUIRED) 69 | if(GTEST_FOUND) 70 | message(STATUS "GTest directories: ${GTEST_INCLUDE_DIRS}") 71 | include_directories(${GTEST_INCLUDE_DIRS}) 72 | else() 73 | message(FATAL_ERROR "GTEST_FOUND not found, please read the README.md") 74 | endif(GTEST_FOUND) 75 | endif() 76 | 77 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) 78 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/) 79 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) 80 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../nlohmann_json/include/) 81 | 82 | set(RESULTS_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/build) 83 | file(MAKE_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}) 84 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 85 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 86 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/bin) 87 | 88 | add_subdirectory("src") 89 | if(BUILD_TESTING) 90 | add_subdirectory("tst") 91 | endif() 92 | 93 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/data/config/Env2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/src/cmcl/cmcl_ros/ncore/data/config/Env2.png -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/data/config/Env2.yaml: -------------------------------------------------------------------------------- 1 | image: Env2.png 2 | resolution: 0.05 3 | origin: [-16, -16, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.25 7 | mode: trinary 8 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/data/config/cmcl.config: -------------------------------------------------------------------------------- 1 | { 2 | "mapPath": "Env2.yaml", 3 | "numParticles": 10000, 4 | "sensorModel": { 5 | "likelihoodSigma": 0.5, 6 | "maxDist": 20.0, 7 | "type": "BeamEnd", 8 | "weightingScheme": 1 9 | }, 10 | "detectionModel": { 11 | "kmeans": 12 | { 13 | "clusters": 8, 14 | "iterations" : 5 15 | }, 16 | "det": 17 | { 18 | "minSize": 500, 19 | "maxSize": 1000 20 | }, 21 | "prorok": 22 | { 23 | "clusters": 8 24 | }, 25 | "thinning": 26 | { 27 | "clusters": 64 28 | }, 29 | "detectionNoise": 30 | { 31 | "x": 0.1, 32 | "y": 0.1 33 | }, 34 | "penalty" : 0.000000001, 35 | "alpha" : 0.06, 36 | "clusters": 10000, 37 | "type" : 3 38 | }, 39 | "tracking": 40 | { 41 | "mode" : false, 42 | "x" : [8.61341, 8.61341], 43 | "y" : [-3.3749, -3.3749], 44 | "yaw" : [0.03733, 0.03733] , 45 | "cov_x" : [0.5, 0.5], 46 | "cov_y" : [0.5, 0.5], 47 | "cov_yaw" : [0.5, 0.5] 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/BeamEnd.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: BeamEnd.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef BEAMEND_H 14 | #define BEAMEND_H 15 | 16 | #include 17 | #include 18 | #include "GMap.h" 19 | #include 20 | #include 21 | #include "LidarData.h" 22 | #include "Particle.h" 23 | #include "GMap.h" 24 | 25 | class BeamEnd 26 | { 27 | public: 28 | 29 | enum class Weighting 30 | { 31 | NAIVE = 0, 32 | GEOMETRIC = 1 33 | }; 34 | 35 | 36 | //! A constructor 37 | /*! 38 | \param Gmap is a ptr to a GMAP object, which holds the gmapping map 39 | \param sigma is a float that determine how forgiving the model is (small sigma will give a very peaked likelihood) 40 | \param maxDist is a float specifying up to what distance from the sensor a reading is valid 41 | \param Weighting is an int specifying which weighting scheme to use 42 | */ 43 | 44 | BeamEnd(const std::shared_ptr& GMap, float sigma = 8, float maxDist = 15, Weighting weighting = Weighting::GEOMETRIC); 45 | 46 | //! Computes weights for all particles based on how well the observation matches the map 47 | /*! 48 | \param particles is a vector of Particle elements 49 | \param SensorData is an abstract container for sensor data. This function expects LidarData type 50 | */ 51 | 52 | void ComputeWeights(std::vector& particles, std::shared_ptr data); 53 | 54 | //! Returns truth if a particle is in an occupied grid cell, false otherwise. Notice that for particles in unknown areas the return is false. 55 | /*! 56 | \param Particle is a particle with pose and weight 57 | */ 58 | 59 | 60 | //void plotParticles(std::vector& particles, std::string title, bool show=true); 61 | 62 | const cv::Mat& DebugMap() 63 | { 64 | return o_debugMap; 65 | } 66 | 67 | 68 | private: 69 | 70 | 71 | float getLikelihood(float distance); 72 | 73 | void plotScan(Eigen::Vector3f laser, std::vector& zMap); 74 | 75 | std::vector scan2Map(const Particle& particle, const std::vector& scan); 76 | 77 | double naive(const Particle& particle, const std::vector& scan, std::vector scanMask); 78 | 79 | double geometric(const Particle& particle, const std::vector& scan, std::vector scanMask); 80 | 81 | 82 | std::shared_ptr o_gmap; 83 | cv::Mat o_debugMap; 84 | float o_maxDist = 15; 85 | float o_sigma = 8; 86 | float o_resolution = 0.05; 87 | 88 | cv::Mat o_edt; 89 | Weighting o_weighting; 90 | float o_coeff = 1; 91 | 92 | }; 93 | 94 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/Cluster.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Cluster.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef CLUSTER_H 13 | #define CLUSTER_H 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | class Cluster 20 | { 21 | public: 22 | 23 | Cluster(){} 24 | 25 | void ComputeMean(); 26 | 27 | void ComputeVariance(); 28 | 29 | void ComputeError(); 30 | 31 | void ComputeWeight(); 32 | 33 | 34 | std::vector points; 35 | std::vector weights; 36 | float totalWeight = 0; 37 | Eigen::Vector3f mean; 38 | Eigen::Vector3f variance; 39 | float error; 40 | 41 | 42 | }; 43 | 44 | 45 | 46 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/ClusterAbstraction.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | # ############################################################################## 4 | # Copyright (c) 2023- University of Lugano # 5 | # All rights reserved. # 6 | # # 7 | # Author: Nicky Zimmerman # 8 | # # 9 | # File: ClusterAbstraction.h # 10 | # ############################################################################## 11 | **/ 12 | 13 | 14 | #ifndef CLUSTERABSTRACTION_H 15 | #define CLUSTERABSTRACTION_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include "Cluster.h" 21 | #include "Utils.h" 22 | 23 | 24 | 25 | class ClusterAbstraction 26 | { 27 | public: 28 | 29 | ClusterAbstraction(); 30 | 31 | ClusterAbstraction(Cluster& cluster, Eigen::Vector3f trans); 32 | 33 | 34 | static Eigen::Vector2f computePolarDifference(Eigen::Vector3f pm, Eigen::Vector3f pn); 35 | 36 | static std::vector shiftPoints(std::vector& points, const Eigen::Vector3f& trans); 37 | 38 | static Eigen::Vector3f shiftPoint(Eigen::Vector3f& point, const Eigen::Vector2f& trans); 39 | 40 | void computePolarVariance(std::vector& polarPoints, std::vector& weights); 41 | 42 | 43 | 44 | 45 | 46 | 47 | Eigen::Vector3f centroid; 48 | float weight; 49 | Eigen::Vector2f mu; 50 | Eigen::Matrix2f covariance; 51 | }; 52 | 53 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/DensityEstimationTree.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: DensityEstimationTree.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef DENSITYESTIMATIONTREE_H 14 | #define DENSITYESTIMATIONTREE_H 15 | 16 | #include 17 | #include 18 | #include "DistributionData.h" 19 | #include 20 | #include "Resampling.h" 21 | 22 | class DensityEstimationTree 23 | { 24 | public: 25 | 26 | DensityEstimationTree(int minLeafSize, int maxLeafSize); 27 | 28 | std::shared_ptr Compress(const std::vector& particles, Eigen::Vector3f trans); 29 | 30 | std::shared_ptr> TrainDET(const std::vector& particles); 31 | 32 | double QueryDET(std::shared_ptr> tree, Particle particle); 33 | 34 | void Fuse(std::vector& particles, std::shared_ptr& distData); 35 | 36 | 37 | private: 38 | 39 | 40 | float o_penalty = 0.000000001; 41 | int o_minLeafSize = 0; 42 | int o_maxLeafSize = 0; 43 | std::shared_ptr o_resampler; 44 | 45 | 46 | }; 47 | 48 | 49 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/DistributionData.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: DistributionData.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef DISTRIBUTIONDATA_H 14 | #define DISTRIBUTIONDATA_H 15 | 16 | #include 17 | #include 18 | #include "Particle.h" 19 | #include "ClusterAbstraction.h" 20 | 21 | 22 | enum class DISTRIBUTION 23 | { 24 | PARTICLES = 0, 25 | DET = 1, 26 | KMEANS = 2, 27 | PROROK = 3, 28 | GOODPOINTS = 4 29 | }; 30 | 31 | class DistributionData 32 | { 33 | 34 | public: 35 | 36 | DistributionData(DISTRIBUTION distType, std::vector observerParticles, Eigen::Vector2f variance = Eigen::Vector2f(1.0, 1.0)) 37 | { 38 | o_distType = distType; 39 | o_particles = observerParticles; 40 | o_variance = variance; 41 | } 42 | 43 | DistributionData(DISTRIBUTION distType, std::vector det, Eigen::Vector2f variance = Eigen::Vector2f(1.0, 1.0)) 44 | { 45 | o_distType = distType; 46 | o_det = det; 47 | o_variance = variance; 48 | } 49 | 50 | DistributionData(DISTRIBUTION distType, std::vector kmeans) 51 | { 52 | o_distType = distType; 53 | o_kmeans = kmeans; 54 | o_variance = Eigen::Vector2f(0.0, 0.0); 55 | } 56 | 57 | DistributionData(DISTRIBUTION distType, std::vector prorok) 58 | { 59 | o_distType = distType; 60 | o_prorok = prorok; 61 | o_variance = Eigen::Vector2f(0.0, 0.0); 62 | } 63 | 64 | std::vector& Particles() 65 | { 66 | return o_particles; 67 | } 68 | 69 | std::vector& DET() 70 | { 71 | return o_det; 72 | } 73 | 74 | std::vector& KMeans() 75 | { 76 | return o_kmeans; 77 | } 78 | 79 | std::vector& Prorok() 80 | { 81 | return o_prorok; 82 | } 83 | 84 | DISTRIBUTION Type() 85 | { 86 | return o_distType; 87 | } 88 | 89 | Eigen::Vector2f Variance() 90 | { 91 | return o_variance; 92 | } 93 | 94 | 95 | 96 | private: 97 | 98 | DISTRIBUTION o_distType; 99 | std::vector o_particles; 100 | std::vector o_det; 101 | std::vector o_prorok; 102 | std::vector o_kmeans; 103 | Eigen::Vector2f o_variance; 104 | 105 | }; 106 | 107 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/DistributionExchange.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: DistributionExchange.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef DISTRIBUTIONEXCHANGE_H 13 | #define DISTRIBUTIONEXCHANGE_H 14 | 15 | #include 16 | #include "Particle.h" 17 | #include "DistributionData.h" 18 | 19 | #include "GoodPointsCompression.h" 20 | #include "ProrokClustering.h" 21 | #include "DensityEstimationTree.h" 22 | #include "KMeansClustering.h" 23 | #include "StandardThinning.h" 24 | 25 | 26 | class DistributionExchange 27 | { 28 | public: 29 | 30 | DistributionExchange(const std::string& jsonConfigPath); 31 | 32 | DistributionExchange(int k = 5, Eigen::Vector2f detectionNoise = Eigen::Vector2f(0.15, 0.15), float penalty = 0.000000001); 33 | 34 | void FuseDistribution(std::vector& particles, std::shared_ptr& distData); 35 | 36 | std::shared_ptr CreateDistribution(const std::vector& observerParticles, DISTRIBUTION distType, Eigen::Vector3f trans = Eigen::Vector3f()); 37 | 38 | 39 | 40 | 41 | private: 42 | 43 | Eigen::Vector2f o_detectionNoise; 44 | float o_penalty = 0.000000001; 45 | int o_k = 5; 46 | 47 | std::shared_ptr o_goodPoints; 48 | std::shared_ptr o_prorok; 49 | std::shared_ptr o_det; 50 | std::shared_ptr o_kmeans; 51 | std::shared_ptr o_thin; 52 | //std::shared_ptr o_naive; 53 | 54 | }; 55 | 56 | 57 | 58 | 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/FSR.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: FSR.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef FSR_H 14 | #define FSR_H 15 | 16 | #include 17 | #include 18 | 19 | class FSR 20 | { 21 | public: 22 | 23 | Eigen::Vector3f SampleMotion(const Eigen::Vector3f& p1, const std::vector& command, const std::vector& weights, const Eigen::Vector3f& noise); 24 | 25 | Eigen::Vector3f Forward(Eigen::Vector3f p1, Eigen::Vector3f u); 26 | 27 | Eigen::Vector3f Backward(Eigen::Vector3f p1, Eigen::Vector3f p2); 28 | 29 | 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/GMap.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023 University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: GMap.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef GMAP 14 | #define GMAP 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | class GMap 23 | { 24 | public: 25 | 26 | //! A constructor for handling the output of Gmapping, which include a metadata yaml and a .pgm map 27 | /*! 28 | \param origin is the 2D pose of the bottom right corner of the map (found in the yaml) 29 | \param resolution is the map resolution - distance in meters corresponding to 1 pixel (found in the yaml) 30 | \param gridMap is occupancy map built according to the scans 31 | */ 32 | GMap(cv::Mat& gridMap, Eigen::Vector3f origin, float resolution); 33 | 34 | 35 | //! A constructor for handling the output of Gmapping, which include a metadata yaml and a .pgm map 36 | /*! 37 | \param yamlPath is the path to the metadata yaml file produced by gmapping 38 | */ 39 | GMap(const std::string& mapFolder, const std::string& yamlName = "YouBotMap.yaml"); 40 | 41 | 42 | //! A getter top left corner of the actual occupied area, as the map usually has wide empty margins 43 | /*! 44 | \return Eigen::Vector2f = (u, v) pixel coordinates for the gridmap 45 | */ 46 | Eigen::Vector2f TopLeft() 47 | { 48 | return o_topLeft; 49 | } 50 | 51 | 52 | //! A getter bottom right corner of the actual occupied area, as the map usually has wide empty margins 53 | /*! 54 | \return Eigen::Vector2f = (u, v) pixel coordinates for the gridmap 55 | */ 56 | Eigen::Vector2f BottomRight() 57 | { 58 | return o_bottomRight; 59 | } 60 | 61 | 62 | //! Converts (x, y) from the map frame to the pixels coordinates 63 | /*! 64 | \param (x, y) position in map frame 65 | \return (u, v) pixel coordinates for the gridmap 66 | */ 67 | Eigen::Vector2f World2Map(Eigen::Vector2f xy) const; 68 | 69 | 70 | //! Converts (u, v) pixel coordinates to map frame (x, y) 71 | /*! 72 | \param (u, v) pixel coordinates for the gridmap 73 | \return (x, y) position in map frame 74 | */ 75 | Eigen::Vector2f Map2World(Eigen::Vector2f uv) const; 76 | 77 | 78 | bool IsInMap(Eigen::Vector2f mp) const; 79 | 80 | 81 | bool IsValid(Eigen::Vector3f pose) const; 82 | 83 | bool IsValid2D(Eigen::Vector2f mp) const; 84 | 85 | 86 | const cv::Mat& Map() const 87 | { 88 | return o_gridmap; 89 | } 90 | 91 | float Resolution() const 92 | { 93 | return o_resolution; 94 | } 95 | 96 | Eigen::Vector3f Origin() const 97 | { 98 | return o_origin; 99 | } 100 | 101 | 102 | private: 103 | 104 | //std::shared_ptr o_gridmap; 105 | cv::Mat o_gridmap; 106 | float o_resolution = 0; 107 | int o_maxy = 0; 108 | Eigen::Vector3f o_origin; 109 | Eigen::Vector2f o_bottomRight; 110 | Eigen::Vector2f o_topLeft; 111 | 112 | void getBorders(); 113 | 114 | }; 115 | 116 | #endif //GMap 117 | 118 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/GoodPointsCompression.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: GoodPointsCompression.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef GOODPOINTSCOMPRESSION_H 14 | #define GOODPOINTSCOMPRESSION_H 15 | 16 | #include 17 | #include 18 | #include "Particle.h" 19 | #include "DistributionData.h" 20 | #include "Resampling.h" 21 | 22 | class GoodPointsCompression 23 | { 24 | 25 | public: 26 | GoodPointsCompression(const std::string& modulePath, Eigen::Vector2f detectionNoise = Eigen::Vector2f(0.15, 0.15)); 27 | 28 | 29 | std::shared_ptr Compress(const std::vector& particles, Eigen::Vector3f trans); 30 | 31 | Eigen::MatrixXi Compress(Eigen::MatrixXd& data); 32 | 33 | 34 | void Fuse(std::vector& particles, std::shared_ptr& distData); 35 | 36 | void ReciprocalSampling(std::vector& particles, std::shared_ptr& distData); 37 | 38 | 39 | 40 | private: 41 | 42 | pybind11::object comp_kt; 43 | pybind11::object dc; 44 | float o_penalty = 0.000000001; //0.0000000000000001; 45 | Eigen::Vector2f o_detectionNoise; 46 | std::shared_ptr o_resampler; 47 | }; 48 | 49 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/KMeansClustering.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: KMeansClustering.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef KMEANSCLUSTERING_H 14 | #define KMEANSCLUSTERING_H 15 | 16 | #include 17 | #include 18 | #include "DistributionData.h" 19 | #include "Resampling.h" 20 | 21 | class KMeansClustering 22 | { 23 | public: 24 | 25 | KMeansClustering(int k, int epochs, Eigen::Vector2f detectionNoise); 26 | 27 | std::shared_ptr Compress(const std::vector& particles, Eigen::Vector3f trans); 28 | 29 | void Fuse(std::vector& particles, std::shared_ptr& distData); 30 | 31 | void AssignClusters(std::vector& points, std::vector& weights, std::vector& clusters); 32 | 33 | void RecomputeCentroids(std::vector& clusters); 34 | 35 | void ReciprocalSampling(std::vector& particles, std::shared_ptr& distData); 36 | 37 | 38 | private: 39 | 40 | float o_penalty = 0.000000001; 41 | int o_k = 0; 42 | int o_epochs = 0; 43 | Eigen::Vector2f o_detectionNoise; 44 | std::shared_ptr o_resampler; 45 | }; 46 | 47 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/Lidar2D.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Lidar2D.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef LIDAR2D_H 13 | #define LIDAR2D_H 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | class Lidar2D 21 | { 22 | public: 23 | 24 | Lidar2D(std::string name_, Eigen::Vector3f origin, int numBeams, float maxAngle, float minAngle); 25 | 26 | Lidar2D(std::string name_, std::string yamlFolder); 27 | 28 | Lidar2D(std::string jsonPath); 29 | 30 | //const std::vector& Heading() const 31 | std::vector Heading() 32 | { 33 | return o_heading; 34 | } 35 | 36 | std::string Name() 37 | { 38 | return o_name; 39 | } 40 | 41 | Eigen::Matrix3f Trans() 42 | { 43 | return o_trans; 44 | } 45 | 46 | 47 | 48 | std::vector Center(const std::vector& ranges); 49 | 50 | 51 | 52 | private: 53 | 54 | std::vector o_heading; 55 | std::string o_name; 56 | Eigen::Matrix3f o_trans; 57 | 58 | }; 59 | 60 | 61 | 62 | 63 | 64 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/LidarData.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: LidarData.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef LIDARDATA_H 14 | #define LIDARDATA_H 15 | 16 | #include 17 | #include 18 | 19 | class LidarData 20 | { 21 | 22 | public: 23 | 24 | LidarData(const std::vector& scan, const std::vector& mask) 25 | { 26 | o_scan = scan; 27 | o_mask = mask; 28 | } 29 | 30 | //virtual ~LidarData(){}; 31 | 32 | std::vector& Scan() 33 | { 34 | return o_scan; 35 | } 36 | 37 | std::vector& Mask() 38 | { 39 | return o_mask; 40 | } 41 | 42 | private: 43 | 44 | std::vector o_scan; 45 | std::vector o_mask; 46 | 47 | }; 48 | 49 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/MCL.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: MCL.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef MCL_H 13 | #define MCL_H 14 | 15 | #include "FSR.h" 16 | #include "BeamEnd.h" 17 | #include "Resampling.h" 18 | #include "SetStatistics.h" 19 | #include "GMap.h" 20 | #include 21 | #include "LidarData.h" 22 | #include "ParticleFilter.h" 23 | #include "DistributionExchange.h" 24 | 25 | class MCL 26 | { 27 | public: 28 | 29 | 30 | //! A constructor 31 | /*! 32 | \param fm is a ptr to a GMap object 33 | \param mm is a ptr to a MotionModel object, which is an abstract class. FSR is the implementation 34 | \param sm is a ptr to a BeamEnd object, which is an abstract class. BeamEnd is the implementation 35 | \param rs is a ptr to a Resampling object, which is an abstract class. LowVarianceResampling is the implementation 36 | \param n_particles is an int, and it defines how many particles the particle filter will use 37 | */ 38 | MCL(std::shared_ptr gmap, std::shared_ptr mm, std::shared_ptr sm, 39 | std::shared_ptr rs, int n_particles); 40 | 41 | 42 | //! A constructor 43 | /*! 44 | \param fm is a ptr to a GMap object 45 | */ 46 | MCL(const std::string& jsonConfigPath); 47 | 48 | 49 | //! A constructor 50 | /*! 51 | * \param fm is a ptr to a GMap object 52 | \param mm is a ptr to a MotionModel object, which is an abstract class. FSR is the implementation 53 | \param sm is a ptr to a BeamEnd object, which is an abstract class. BeamEnd is the implementation 54 | \param rs is a ptr to a Resampling object, which is an abstract class. LowVarianceResampling is the implementation 55 | \param n_particles is an int, and it defines how many particles the particle filter will use 56 | \param initGuess is a vector of initial guess for the location of the robots 57 | \param covariances is a vector of covariances (uncertainties) corresponding to the initial guesses 58 | \param injectionRatio is an float, and it determines which portions of the particles are replaced when relocalizing 59 | */ 60 | MCL(std::shared_ptr gmap, std::shared_ptr mm, std::shared_ptr sm, 61 | std::shared_ptr rs, int n_particles, std::vector initGuess, 62 | std::vector covariances); 63 | 64 | 65 | //! A getter for the mean and covariance of the particles 66 | /*! 67 | \return an object SetStatistics, that has fields for mean and covariance 68 | */ 69 | SetStatistics Stats() 70 | { 71 | return o_stats; 72 | } 73 | 74 | 75 | //! A getter particles representing the pose hypotheses 76 | /*! 77 | \return A vector of points, where each is Eigen::Vector3f = (x, y, theta) 78 | */ 79 | std::vector Particles() 80 | { 81 | return o_particles; 82 | } 83 | 84 | 85 | //! Advanced all particles according to the control and noise, using the chosen MotionModel's forward function 86 | /*! 87 | \param control is a 3d control command. In the FSR model it's (forward, sideways, rotation) 88 | \param odomWeights is the corresponding weight to each odometry source 89 | \param noise is the corresponding noise to each control component 90 | */ 91 | void Predict(const std::vector& control, const std::vector& odomWeights, const Eigen::Vector3f& noise); 92 | 93 | //! Considers the beamend likelihood of observation for all hypotheses, and then performs resampling 94 | /*! 95 | \param scan is a vector of homogeneous points (x, y, 1), in the sensor's frame. So the sensor location is (0, 0, 0) 96 | Notice that for the LaserScan messages you need to first transform the ranges to homo points, and then center them 97 | */ 98 | void Correct(std::shared_ptr data); 99 | 100 | 101 | void FuseDistribution(std::shared_ptr& distData); 102 | 103 | 104 | std::shared_ptr CreateDistribution(DISTRIBUTION distType, const Eigen::Vector3f& relTrans); 105 | 106 | 107 | 108 | //! Initializes filter with new particles upon localization failure 109 | void Recover(); 110 | 111 | Eigen::Vector3f Backward(Eigen::Vector3f p1, Eigen::Vector3f p2) 112 | { 113 | return o_motionModel->Backward(p1, p2); 114 | } 115 | 116 | //! A setter for the injection ration for particle, in case of text spotting. The ratio represents the fraction of particles that will be removed, and the same number 117 | // of particles will be injected at the location of detected text. The removed particles are always the ones with the lowest weights. 118 | 119 | 120 | const cv::Mat& DebugMap() 121 | { 122 | return o_beamEndModel->DebugMap(); 123 | } 124 | 125 | 126 | std::shared_ptr Map() 127 | { 128 | return o_gmap; 129 | } 130 | 131 | private: 132 | 133 | void dumpParticles(); 134 | int traceInMap(Eigen::Vector3f p1, Eigen::Vector3f p2); 135 | void detectionReweight(Eigen::Vector3f trans); 136 | //std::vector computeDetectedParticles(const Eigen::Vector3f& relTrans); 137 | 138 | std::shared_ptr o_particleFilter; 139 | unsigned int o_frame = 0; 140 | std::shared_ptr o_motionModel; 141 | std::shared_ptr o_beamEndModel; 142 | std::shared_ptr o_resampler; 143 | std::shared_ptr o_distExcange; 144 | std::shared_ptr o_gmap; 145 | int o_numParticles = 0; 146 | std::vector o_particles; 147 | SetStatistics o_stats; 148 | Eigen::Vector2f o_detectionNoise; 149 | }; 150 | 151 | #endif 152 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/Particle.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Particle.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef PARTICLE_H 13 | #define PARTICLE_H 14 | 15 | 16 | #include 17 | #include 18 | 19 | 20 | class Particle 21 | { 22 | public: 23 | 24 | Particle(Eigen::Vector3f p = Eigen::Vector3f(0, 0, 0), double w = 0); 25 | 26 | Eigen::Vector3f pose; 27 | double weight; 28 | 29 | }; 30 | 31 | 32 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/ParticleFilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: ParticleFilter.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef PARTICLEFILTER_H 14 | #define PARTICLEFILTER_H 15 | 16 | #include "Particle.h" 17 | #include "GMap.h" 18 | #include "SetStatistics.h" 19 | 20 | class ParticleFilter 21 | { 22 | public: 23 | 24 | //multi-map 25 | ParticleFilter(std::shared_ptr GMap); 26 | 27 | 28 | 29 | void InitUniform(std::vector& particles, int n_particles); 30 | void InitGaussian(std::vector& particles, int n_particles, const std::vector& initGuess, const std::vector& covariances); 31 | 32 | SetStatistics ComputeStatistics(const std::vector& particles); 33 | 34 | void NormalizeWeights(std::vector& particles); 35 | 36 | 37 | std::vector& Particles() 38 | { 39 | return o_particles; 40 | } 41 | 42 | void SetParticle(int id, Particle p) 43 | { 44 | o_particles[id] = p; 45 | } 46 | 47 | 48 | SetStatistics Statistics() 49 | { 50 | return o_stats; 51 | } 52 | 53 | 54 | 55 | private: 56 | 57 | 58 | 59 | 60 | std::shared_ptr o_gmap; 61 | std::vector o_particles; 62 | SetStatistics o_stats; 63 | }; 64 | 65 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/ProrokClustering.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: ProrokClustering.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef PROROKCLUSTERING_H 14 | #define PROROKCLUSTERING_H 15 | 16 | #include 17 | #include 18 | #include "Cluster.h" 19 | #include "DistributionData.h" 20 | #include "Resampling.h" 21 | 22 | 23 | 24 | class ProrokClustering 25 | { 26 | public: 27 | 28 | ProrokClustering(int k, Eigen::Vector2f detectionNoise = Eigen::Vector2f(0.15, 0.15)); 29 | 30 | std::shared_ptr Compress(const std::vector& particles, Eigen::Vector3f trans); 31 | 32 | void Fuse(std::vector& particles, std::shared_ptr& distData); 33 | 34 | 35 | std::vector ComputeClusters(const std::vector& particles); 36 | 37 | void findHighestVarianceCluster(std::vector& clusters, int& index, int& dim); 38 | 39 | void splitClusters(std::vector& clusters, int& index, int& dim); 40 | 41 | void ReciprocalSampling(std::vector& particles, std::shared_ptr& distData); 42 | 43 | //std::vector ProrokClustering(std::vector& points, std::vector& weights, int k); 44 | 45 | 46 | private: 47 | 48 | 49 | float o_penalty = 0.000000001; 50 | int o_k = 0; 51 | Eigen::Vector2f o_detectionNoise; 52 | std::shared_ptr o_resampler; 53 | }; 54 | 55 | 56 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/Resampling.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: LowVarianceResampling.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef RESAMPLING_H 14 | #define RESAMPLING_H 15 | 16 | 17 | #include 18 | #include 19 | #include "Particle.h" 20 | 21 | class Resampling 22 | { 23 | public: 24 | 25 | void Resample(std::vector& particles); 26 | 27 | void SetTH(float th) 28 | { 29 | o_th = th; 30 | } 31 | 32 | 33 | private: 34 | 35 | float o_th = 0.5; 36 | 37 | }; 38 | 39 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/SetStatistics.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: SetStatistics.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef SETSTATISTICS_H 14 | #define SETSTATISTICS_H 15 | 16 | #include 17 | #include 18 | #include "Particle.h" 19 | 20 | class SetStatistics 21 | { 22 | public: 23 | 24 | SetStatistics(Eigen::Vector3d m = Eigen::Vector3d::Zero(), Eigen::Matrix3d c = Eigen::Matrix3d::Zero()) 25 | { 26 | mean = m; 27 | cov = c; 28 | } 29 | 30 | Eigen::Vector3d Mean() 31 | { 32 | return mean; 33 | } 34 | 35 | Eigen::Matrix3d Cov() 36 | { 37 | return cov; 38 | } 39 | 40 | 41 | static SetStatistics ComputeParticleSetStatistics(const std::vector& particles); 42 | 43 | private: 44 | 45 | Eigen::Vector3d mean; 46 | Eigen::Matrix3d cov; 47 | 48 | }; 49 | 50 | 51 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/StandardThinning.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: StandardThinning.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef STANDARDTHINNING_H 14 | #define STANDARDTHINNING_H 15 | 16 | 17 | #include "DistributionData.h" 18 | #include "Resampling.h" 19 | 20 | class StandardThinning 21 | { 22 | public: 23 | StandardThinning(int subSize, Eigen::Vector2f detectionNoise = Eigen::Vector2f(0.15, 0.15)); 24 | 25 | std::shared_ptr Compress(const std::vector& particles, Eigen::Vector3f trans); 26 | 27 | void Fuse(std::vector& particles, std::shared_ptr& distData); 28 | 29 | void ReciprocalSampling(std::vector& particles, std::shared_ptr& distData); 30 | 31 | 32 | 33 | private: 34 | float o_penalty = 0.000000001; 35 | Eigen::Vector2f o_detectionNoise; 36 | int o_n = 0; 37 | std::shared_ptr o_resampler; 38 | 39 | 40 | }; 41 | 42 | 43 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/include/Utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Utils.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef UTILS_H 13 | #define UTILS_H 14 | 15 | 16 | #include 17 | #include 18 | #include 19 | #include "Particle.h" 20 | 21 | std::vector computeDetectedParticles(const std::vector& particles, const Eigen::Vector3f& relTrans); 22 | 23 | 24 | Eigen::Matrix3f Vec2Trans(Eigen::Vector3f v); 25 | 26 | float CosineSimilarity(float yaw1, float yaw2); 27 | 28 | float Wrap2Pi(float angle); 29 | 30 | float GetYaw(float qz, float qw); 31 | 32 | std::vector Ranges2Points(const std::vector& ranges, const std::vector& angles); 33 | 34 | std::vector Downsample(const std::vector& ranges, int N); 35 | 36 | std::vector Downsample(const std::vector& ranges, int N); 37 | 38 | std::vector StringToVec(std::string seq); 39 | 40 | std::vector File2Lines(std::string filePath); 41 | 42 | float SampleGuassian(float sigma); 43 | 44 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/BeamEnd.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: BeamEnd.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "BeamEnd.h" 13 | #include "Utils.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | BeamEnd::BeamEnd(const std::shared_ptr& GMap, float sigma, float maxDist, Weighting weighting ) 21 | { 22 | o_gmap = GMap; 23 | o_maxDist = maxDist; 24 | o_sigma = sigma; //value of 8 for map resolution 0.05, 40 for 0.01 25 | o_weighting = weighting; 26 | o_coeff = 1.0 / sqrt(2 * M_PI * o_sigma); 27 | o_resolution = o_gmap->Resolution(); 28 | 29 | cv::Mat edt; 30 | cv::threshold(o_gmap->Map(), o_edt, 127, 255, 0); 31 | cv::imwrite("TH.png", o_edt); 32 | o_edt = 255 - o_edt; 33 | cv::distanceTransform(o_edt, o_edt, cv::DIST_L2, cv::DIST_MASK_3); 34 | //cv::threshold(o_edt, o_edt, o_maxDist, o_maxDist, 2); //Threshold Truncated 35 | 36 | o_debugMap = cv::Mat::zeros(o_edt.size(), CV_32SC1); 37 | cv::imwrite("edt.png", o_edt); 38 | 39 | } 40 | 41 | void BeamEnd::ComputeWeights(std::vector& particles, std::shared_ptr data) 42 | { 43 | const std::vector& scan = data->Scan(); 44 | const std::vector& scanMask = data->Mask(); 45 | 46 | double acc_w = 0; 47 | 48 | // for(int m = 0; m < o_debugMaps.size(); ++m) 49 | // { 50 | // o_debugMaps[m] = cv::Mat::zeros(o_edts[0][0].size(), CV_32SC1); 51 | // } 52 | 53 | #pragma omp parallel for 54 | for(long unsigned int i = 0; i < particles.size(); ++i) 55 | { 56 | //auto t1 = std::chrono::high_resolution_clock::now(); 57 | double w = 0; 58 | switch(o_weighting) 59 | { 60 | case Weighting::NAIVE : 61 | w = naive(particles[i], scan, scanMask); 62 | break; 63 | case Weighting::GEOMETRIC : 64 | w = geometric(particles[i], scan, scanMask); 65 | break; 66 | 67 | } 68 | particles[i].weight *= w; 69 | acc_w += w; 70 | 71 | //auto t2 = std::chrono::high_resolution_clock::now(); 72 | //auto ns = std::chrono::duration_cast(t2 - t1); 73 | //std::cout << "BeamEnd::Timing of compute : " << ns.count() << std::endl; 74 | } 75 | 76 | //std::cout << acc_w/particles.size() << std::endl; 77 | } 78 | 79 | 80 | 81 | double BeamEnd::naive(const Particle& particle, const std::vector& scan, std::vector scanMask) 82 | { 83 | std::vector mapPoints = scan2Map(particle, scan); 84 | //plotScan(particle.pose, mapPoints); 85 | 86 | Eigen::Vector2f br = o_gmap->BottomRight(); 87 | 88 | double weight = 1.0; 89 | int nonValid = 0; 90 | 91 | for(long unsigned int i = 0; i < mapPoints.size(); ++i) 92 | { 93 | Eigen::Vector2f mp = mapPoints[i]; 94 | 95 | if(scanMask[i] > 0.0) 96 | { 97 | if ((mp(0) < 0) || (mp(1) < 0) || (mp(0) > br(0)) || (mp(1) > br(1))) 98 | { 99 | ++nonValid; 100 | } 101 | else 102 | { 103 | float dist = o_edt.at(mp(1) ,mp(0)); 104 | double w = getLikelihood(dist); 105 | weight *= w; 106 | } 107 | } 108 | } 109 | //std::cout << nonValid << std::endl; 110 | 111 | float penalty = pow(getLikelihood(o_maxDist), nonValid); 112 | weight *= penalty; 113 | 114 | return weight; 115 | } 116 | 117 | static int plot = 0; 118 | 119 | double BeamEnd::geometric(const Particle& particle, const std::vector& scan, std::vector scanMask) 120 | { 121 | std::vector mapPoints = scan2Map(particle, scan); 122 | //plotScan(particle.pose, mapPoints); 123 | //plot++; 124 | 125 | double weight = 1.0; 126 | float geoW = 1.0 / scan.size(); 127 | int debug_id = 0; 128 | 129 | Eigen::Vector2f br = o_gmap->BottomRight(); 130 | int nonValid = 0; 131 | int valid = 0; 132 | double tot_dist = 0; 133 | 134 | for(long unsigned int i = 0; i < mapPoints.size(); ++i) 135 | { 136 | Eigen::Vector2f mp = mapPoints[i]; 137 | 138 | if(scanMask[i] > 0.0) 139 | { 140 | if ((mp(0) < 0) || (mp(1) < 0) || (mp(0) > br(0)) || (mp(1) > br(1))) 141 | { 142 | ++nonValid; 143 | } 144 | else 145 | { 146 | 147 | float dist = o_edt.at(mp(1) ,mp(0)) * o_resolution; 148 | if (dist > o_maxDist) dist = o_maxDist; 149 | 150 | tot_dist += pow(dist / o_sigma, 2.0); 151 | ++valid; 152 | 153 | } 154 | } 155 | } 156 | 157 | geoW = 1.0 /(valid + nonValid); 158 | tot_dist += nonValid * pow(o_maxDist / o_sigma, 2.0); 159 | weight = o_coeff * exp(-0.5 * tot_dist * geoW); 160 | 161 | 162 | //Eigen::Vector2f pp = o_gmaps[floorID][0]->World2Map(Eigen::Vector2f(particle.pose(0), particle.pose(1))); 163 | //o_debugMaps[debug_id].at(pp(1), pp(0)) += 1.0; 164 | 165 | return weight; 166 | } 167 | 168 | 169 | float BeamEnd::getLikelihood(float distance) 170 | { 171 | float l = o_coeff * exp(-0.5 * pow(distance / o_sigma, 2)); 172 | return l; 173 | } 174 | 175 | 176 | /* 177 | void BeamEnd::plotParticles(std::vector& particles, std::string title, bool show) 178 | { 179 | cv::Mat img; 180 | cv::cvtColor(Gmap->Map(), img, cv::COLOR_GRAY2BGR); 181 | 182 | cv::namedWindow("Particles", cv::WINDOW_NORMAL); 183 | 184 | for(long unsigned int i = 0; i < particles.size(); ++i) 185 | { 186 | Eigen::Vector3f p = particles[i].pose; 187 | Eigen::Vector2f uv = Gmap->World2Map(Eigen::Vector2f(p(0), p(1))); 188 | 189 | cv::circle(img, cv::Point(uv(0), uv(1)), 5, cv::Scalar(0, 0, 255), -1); 190 | } 191 | if(show) 192 | { 193 | cv::imshow("Particles", img); 194 | cv::waitKey(0); 195 | } 196 | cv::imwrite("/home/nickybones/Code/YouBotMCL/nmcl/" + title + ".png", img); 197 | 198 | cv::destroyAllWindows(); 199 | }*/ 200 | 201 | 202 | void BeamEnd::plotScan(Eigen::Vector3f laser, std::vector& zMap) 203 | { 204 | cv::Mat img; 205 | cv::cvtColor(o_gmap->Map(), img, cv::COLOR_GRAY2BGR); 206 | 207 | //cv::namedWindow("Scan", cv::WINDOW_NORMAL); 208 | Eigen::Vector2f p = o_gmap->World2Map(Eigen::Vector2f(laser(0), laser(1))); 209 | 210 | cv::circle(img, cv::Point(p(0), p(1)), 5, cv::Scalar(255, 0, 0), -1); 211 | 212 | for(long unsigned int i = 0; i < zMap.size(); ++i) 213 | { 214 | Eigen::Vector2f p = zMap[i]; 215 | cv::circle(img, cv::Point(p(0), p(1)), 1, cv::Scalar(0, 0, 255), -1); 216 | } 217 | 218 | //cv::Rect myROI(475, 475, 400, 600); 219 | // Crop the full image to that image contained by the rectangle myROI 220 | // Note that this doesn't copy the data 221 | //cv::Mat img_(img, myROI); 222 | 223 | cv::imwrite("scan" + std::to_string(plot) + ".png", img); 224 | //cv::imshow("Scan", img_); 225 | //cv::waitKey(0); 226 | 227 | //cv::destroyAllWindows(); 228 | } 229 | 230 | //#include 231 | 232 | std::vector BeamEnd::scan2Map(const Particle& particle, const std::vector& scan) 233 | { 234 | Eigen::Vector3f pose = particle.pose; 235 | Eigen::Matrix3f trans = Vec2Trans(pose); 236 | std::vector mapPoints(scan.size()); 237 | 238 | // std::fstream fout; 239 | // fout.open("scan" + std::to_string(plot) + ".csv", std::ios::out | std::ios::app); 240 | // std::fstream fout2; 241 | // fout2.open("scanTr" + std::to_string(plot) + ".csv", std::ios::out | std::ios::app); 242 | // std::fstream fout3; 243 | // fout3.open("scanMp" + std::to_string(plot) + ".csv", std::ios::out | std::ios::app); 244 | 245 | for(long unsigned int i = 0; i < scan.size(); ++i) 246 | { 247 | 248 | Eigen::Vector3f ts = trans * scan[i]; 249 | Eigen::Vector2f mp = o_gmap->World2Map(Eigen::Vector2f(ts(0), ts(1))); 250 | mapPoints[i] = mp; 251 | 252 | //fout << scan[i](0) << ", " << scan[i](1) << ", " << scan[i](2) << std::endl; 253 | //fout2 << ts(0) << ", " << ts(1) << std::endl; 254 | //fout3 << mp(0) << ", " << mp(1) << std::endl; 255 | } 256 | //fout.close(); 257 | //fout2.close(); 258 | //fout3.close(); 259 | 260 | 261 | 262 | return mapPoints; 263 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | add_library(NCORE MCL.cpp DistributionExchange.cpp BeamEnd.cpp Resampling.cpp SetStatistics.cpp ParticleFilter.cpp Particle.cpp Lidar2D.cpp GMap.cpp FSR.cpp KMeansClustering.cpp Cluster.cpp ProrokClustering.cpp GoodPointsCompression.cpp StandardThinning.cpp DensityEstimationTree.cpp ClusterAbstraction.cpp Utils.cpp) 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/Cluster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Cluster.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "Cluster.h" 13 | #include "Utils.h" 14 | 15 | #include 16 | 17 | void Cluster::ComputeMean() 18 | { 19 | int pointNum = points.size(); 20 | mean = Eigen::Vector3f(0,0,0); 21 | 22 | double dx = 0; 23 | double dy = 0; 24 | double w = 0; 25 | 26 | for (int i = 0; i < pointNum; ++i) 27 | { 28 | mean(0) += points[i](0) * weights[i]; 29 | mean(1) += points[i](1) * weights[i]; 30 | dx += cos(points[i](2)) * weights[i]; // theta 31 | dy += sin(points[i](2)) * weights[i]; // theta 32 | w += weights[i]; 33 | } 34 | 35 | mean /= w; 36 | mean(2) = Wrap2Pi(atan2(dy, dx)); 37 | 38 | //std::cout << "dx dy:" << dx << ", " << dy << ", " << mean(2) << std::endl; 39 | //std::cout << "mean:" << mean(0) << ", " << mean(1) << ", " << mean(2) << ", " << w << std::endl; 40 | } 41 | 42 | void Cluster::ComputeError() 43 | { 44 | int n = points.size(); 45 | float error = 0; 46 | 47 | for (int i = 0; i < n; ++i) 48 | { 49 | Eigen::Vector2f diff = points[i].head(2) - mean.head(2); 50 | float dist = diff.norm(); 51 | error += dist; 52 | } 53 | 54 | error /= n; 55 | } 56 | 57 | void Cluster::ComputeWeight() 58 | { 59 | int n = points.size(); 60 | totalWeight = 0; 61 | for (int i = 0; i < n; ++i) 62 | { 63 | totalWeight += weights[i]; 64 | } 65 | } 66 | 67 | void Cluster::ComputeVariance() 68 | { 69 | int pointNum = points.size(); 70 | variance = Eigen::Vector3f(0, 0, 0); 71 | ComputeMean(); 72 | 73 | double w = 0; 74 | 75 | for (int i = 0; i < pointNum; ++i) 76 | { 77 | Eigen::Vector3f p = points[i]; 78 | Eigen::Vector3f diff = p - mean; 79 | w += weights[i]; 80 | 81 | variance(0) += diff(0) * diff(0) * weights[i]; 82 | variance(1) += diff(1) * diff(1) * weights[i]; 83 | // ignoring angular variance here 84 | } 85 | 86 | variance /= w; 87 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/ClusterAbstraction.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | /** 4 | # ############################################################################## 5 | # Copyright (c) 2023- University of Lugano # 6 | # All rights reserved. # 7 | # # 8 | # Author: Nicky Zimmerman # 9 | # # 10 | # File: ClusterAbstraction.cpp # 11 | # ############################################################################## 12 | **/ 13 | 14 | #include "ClusterAbstraction.h" 15 | #include 16 | #include 17 | #include 18 | 19 | std::vector ClusterAbstraction::shiftPoints(std::vector& points, const Eigen::Vector3f& trans) 20 | { 21 | size_t numPoints = points.size(); 22 | 23 | float r = trans.head(2).norm(); 24 | float theta = atan2(trans(1), trans(0)); 25 | 26 | std::vector shiftedPoints(numPoints); 27 | 28 | #pragma omp parallel for 29 | for(long unsigned int i = 0; i < numPoints; ++i) 30 | { 31 | Eigen::Vector3f p; 32 | p(0) = points[i](0) + r * cos(points[i](2) + theta); 33 | p(1) = points[i](1) + r * sin(points[i](2) + theta); 34 | shiftedPoints[i] = p; 35 | } 36 | 37 | return shiftedPoints; 38 | } 39 | 40 | Eigen::Vector3f ClusterAbstraction::shiftPoint(Eigen::Vector3f& point, const Eigen::Vector2f& trans) 41 | { 42 | float r = trans(0); 43 | float theta = trans(1); 44 | 45 | Eigen::Vector3f p; 46 | p(0) = point(0) + r * cos(point(2) + theta); 47 | p(1) = point(1) + r * sin(point(2) + theta); 48 | p(2) = point(2); 49 | 50 | 51 | return p; 52 | } 53 | 54 | 55 | Eigen::Vector2f ClusterAbstraction::computePolarDifference(Eigen::Vector3f pm, Eigen::Vector3f pn) 56 | { 57 | Eigen::Vector2f diff(0, 0); 58 | 59 | diff(0) = (pm.head(2) - pn.head(2)).norm(); 60 | diff(1) = Wrap2Pi(atan2(pn(1) - pm(1), pn(0) - pm(0)) - (pm(2))); //Do I need to add M_PI here? 61 | 62 | return diff; 63 | } 64 | 65 | 66 | void ClusterAbstraction::computePolarVariance(std::vector& polarPoints, std::vector& weights) 67 | { 68 | int pointNum = polarPoints.size(); 69 | double dx = 0; 70 | double dy = 0; 71 | double w = 0; 72 | double r = 0; 73 | 74 | mu = Eigen::Vector2f(0, 0); 75 | covariance = Eigen::Matrix2f::Zero(); 76 | 77 | if (pointNum == 0) 78 | { 79 | mu = Eigen::Vector2f(NAN, NAN); 80 | return; 81 | } 82 | 83 | for (int i = 0; i < pointNum; ++i) 84 | { 85 | r += polarPoints[i](0) * weights[i]; 86 | dx += cos(polarPoints[i](1)) * weights[i]; // theta 87 | dy += sin(polarPoints[i](1)) * weights[i]; // theta 88 | w += weights[i]; 89 | } 90 | 91 | mu(0) = r / w; 92 | mu(1) = Wrap2Pi(atan2(dy, dx)); 93 | 94 | if (pointNum > 1) 95 | { 96 | for (int i = 0; i < pointNum; ++i) 97 | { 98 | Eigen::Vector2f diff = polarPoints[i] - mu; 99 | //covariance += diff * diff.transpose(); 100 | covariance(0,0) += diff(0) * diff(0) * weights[i]; 101 | covariance(1,1) += diff(1) * diff(1) * weights[i]; 102 | } 103 | 104 | covariance /= w; 105 | } 106 | 107 | } 108 | 109 | 110 | ClusterAbstraction::ClusterAbstraction(Cluster& cluster, Eigen::Vector3f trans) 111 | { 112 | int numPoints = cluster.points.size(); 113 | centroid = cluster.mean; 114 | weight = std::accumulate(cluster.weights.begin(), cluster.weights.end(), 0.0); 115 | 116 | std::vector shiftedPoints = ClusterAbstraction::shiftPoints(cluster.points, trans); 117 | std::vector polarDiff(numPoints); 118 | 119 | #pragma omp parallel for 120 | for(long unsigned int i = 0; i < numPoints; ++i) 121 | { 122 | polarDiff[i] = ClusterAbstraction::computePolarDifference(centroid, shiftedPoints[i]); 123 | } 124 | 125 | computePolarVariance(polarDiff, cluster.weights); 126 | } 127 | 128 | ClusterAbstraction::ClusterAbstraction() 129 | { 130 | 131 | centroid = Eigen::Vector3f(); 132 | weight = 0; 133 | mu = Eigen::Vector2f(); 134 | covariance = Eigen::Matrix2f::Zero(); 135 | } 136 | 137 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/DensityEstimationTree.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: DensityEstimationTree.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "DensityEstimationTree.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | using namespace mlpack; 19 | using namespace std; 20 | 21 | 22 | 23 | DensityEstimationTree::DensityEstimationTree(int minLeafSize, int maxLeafSize) 24 | { 25 | o_minLeafSize = minLeafSize; 26 | o_maxLeafSize = maxLeafSize; 27 | o_resampler = std::make_shared(Resampling()); 28 | } 29 | 30 | std::shared_ptr> DensityEstimationTree::TrainDET(const std::vector& particles) 31 | { 32 | size_t numParticles = particles.size(); 33 | arma::mat testData(2, numParticles); 34 | arma::Col oTest(numParticles); 35 | 36 | #pragma omp parallel for 37 | for (long unsigned int i = 0; i < numParticles; ++i) 38 | { 39 | testData(0, i) = particles[i].pose(0); 40 | testData(1, i) = particles[i].pose(1); 41 | oTest(i) = i; 42 | } 43 | 44 | std::shared_ptr> testDTree = std::make_shared>(DTree(testData)); 45 | double alpha = testDTree->Grow(testData, oTest, false, o_maxLeafSize, o_minLeafSize); 46 | 47 | return testDTree; 48 | } 49 | 50 | double DensityEstimationTree::QueryDET(std::shared_ptr> tree, Particle p) 51 | { 52 | arma::vec q(2); 53 | q = { p.pose(0), p.pose(1) }; 54 | double w = (tree->ComputeValue(q)) ; 55 | 56 | return w; 57 | } 58 | 59 | 60 | std::shared_ptr DensityEstimationTree::Compress(const std::vector& particles_, Eigen::Vector3f trans) 61 | { 62 | std::vector particles = computeDetectedParticles(particles_, trans); 63 | o_resampler->SetTH(10000000000); //always resample 64 | o_resampler->Resample(particles); 65 | 66 | size_t numParticles = particles.size(); 67 | arma::mat testData(2, numParticles); 68 | arma::Col oTest(numParticles); 69 | 70 | #pragma omp parallel for 71 | for (long unsigned int i = 0; i < numParticles; ++i) 72 | { 73 | testData(0, i) = particles[i].pose(0); 74 | testData(1, i) = particles[i].pose(1); 75 | oTest(i) = i; 76 | } 77 | 78 | DTree testDTree(testData); 79 | double alpha = testDTree.Grow(testData, oTest, false, o_maxLeafSize, o_minLeafSize); 80 | 81 | // no prunning, can add https://github.com/mlpack/mlpack/blob/master/src/mlpack/methods/det/dt_utils_impl.hpp 82 | 83 | std::ostringstream os(std::stringstream::binary); 84 | cereal::BinaryOutputArchive arout(os); 85 | arout(cereal::make_nvp("det", testDTree)); 86 | std::string osstr = os.str(); 87 | std::vector bytes(osstr.begin(), osstr.end()); 88 | std::shared_ptr distData = std::make_shared(DistributionData(DISTRIBUTION::DET, bytes)); 89 | 90 | return distData; 91 | } 92 | 93 | void DensityEstimationTree::Fuse(std::vector& particles, std::shared_ptr& distData) 94 | { 95 | std::vector distributionBytes = distData->DET(); 96 | DTree testDTree; 97 | 98 | std::string distribution(distributionBytes.begin(),distributionBytes.end()); 99 | 100 | std::istringstream is(distribution, std::stringstream::binary); 101 | cereal::BinaryInputArchive arin(is); 102 | try { 103 | arin(cereal::make_nvp("det", testDTree)); 104 | } 105 | catch (std::runtime_error e) { 106 | 107 | #include 108 | using namespace std; 109 | ofstream myfile; 110 | myfile.open ("exception.txt"); 111 | myfile << e.what() << std::endl; 112 | myfile.close(); 113 | e.what(); 114 | } 115 | 116 | #pragma omp parallel for 117 | for(long unsigned int i = 0; i < particles.size(); ++i) 118 | { 119 | Eigen::Vector3f p = particles[i].pose; 120 | arma::vec q(2); 121 | q = { p(0), p(1) }; 122 | double w = (testDTree.ComputeValue(q)) ; 123 | particles[i].weight *= (w + o_penalty); 124 | //std::cout << w << std::endl; 125 | } 126 | 127 | o_resampler->SetTH(0.5); 128 | o_resampler->Resample(particles); 129 | } 130 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/FSR.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: FSR.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "FSR.h" 13 | #include 14 | #include 15 | #include "Utils.h" 16 | #include 17 | 18 | 19 | 20 | Eigen::Vector3f FSR::SampleMotion(const Eigen::Vector3f& p1, const std::vector& command, const std::vector& weights, const Eigen::Vector3f& noise) 21 | { 22 | Eigen::Vector3f u(0, 0, 0); 23 | float choose = drand48(); 24 | float w = 0.0; 25 | 26 | for(long unsigned int i = 0; i < command.size(); ++i) 27 | { 28 | w += weights[i]; 29 | if(choose <= w) 30 | { 31 | u = command[i]; 32 | break; 33 | } 34 | } 35 | 36 | float f = u(0); 37 | float s = u(1); 38 | float r = u(2); 39 | 40 | float f_h = f - SampleGuassian(noise(0) * fabs(f)); 41 | float s_h = s - SampleGuassian(noise(1) * fabs(s)); 42 | float r_h = r - SampleGuassian(noise(2) * fabs(r)); 43 | 44 | 45 | Eigen::Vector3f new_p = Forward(p1, Eigen::Vector3f(f_h, s_h, r_h)); 46 | 47 | return new_p; 48 | 49 | } 50 | 51 | 52 | Eigen::Vector3f FSR::Backward(Eigen::Vector3f p1, Eigen::Vector3f p2) 53 | { 54 | Eigen::Vector3f dp = p2 - p1; 55 | 56 | float a = cos(p1(2)); 57 | float b = sin(p1(2)); 58 | 59 | float f = (dp.x() * a + dp.y() * b) / (pow(a, 2.0) + pow(b, 2.0)); 60 | float s = (dp.y() - f * b) / a; 61 | float r = dp(2); 62 | 63 | 64 | return Eigen::Vector3f(f, s, r); 65 | 66 | } 67 | 68 | Eigen::Vector3f FSR::Forward(Eigen::Vector3f p1, Eigen::Vector3f u) 69 | { 70 | float f = u(0); 71 | float s = u(1); 72 | float r = u(2); 73 | 74 | float x = p1(0) + f * cos(p1(2)) - s * sin(p1(2)); 75 | float y = p1(1) + f * sin(p1(2)) + s * cos(p1(2)); 76 | float theta = Wrap2Pi(r + p1(2)); 77 | 78 | return Eigen::Vector3f(x, y, theta); 79 | 80 | } 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/GMap.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: GMap.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "GMap.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "Utils.h" 18 | 19 | GMap::GMap(cv::Mat& gridMap, Eigen::Vector3f origin, float resolution) 20 | { 21 | o_resolution = resolution; 22 | o_origin = origin; 23 | if (gridMap.channels() == 3) 24 | { 25 | cv::cvtColor(gridMap, o_gridmap, cv::COLOR_BGR2GRAY); 26 | } 27 | else 28 | { 29 | gridMap.copyTo(o_gridmap); 30 | } 31 | 32 | o_gridmap = 255 - o_gridmap; 33 | o_maxy = o_gridmap.rows; 34 | 35 | //compute the borders 36 | getBorders(); 37 | } 38 | 39 | 40 | GMap::GMap(const std::string& mapFolder, const std::string& yamlName) 41 | { 42 | 43 | std::vector fields = File2Lines(mapFolder + yamlName); 44 | 45 | // "image: " - 7 46 | fields[0].erase(0,7); 47 | // "iresolution: " - 12 48 | fields[1].erase(0,12); 49 | // "origin: " - 8 50 | fields[2].erase(0,8); 51 | // "occupied_thresh: " - 17 52 | fields[4].erase(0,17); 53 | // "free_thresh: " - 13 54 | fields[5].erase(0,13); 55 | 56 | std::string imgPath = mapFolder + fields[0]; 57 | o_resolution = std::stof(fields[1]); 58 | 59 | std::vector vec = StringToVec(fields[2]); 60 | 61 | o_origin = Eigen::Vector3f(vec[0], vec[1], vec[2]); 62 | o_gridmap = cv::imread(imgPath, cv::IMREAD_GRAYSCALE); 63 | //cv::cvtColor( gridMap_, map, cv::COLOR_BGR2GRAY); 64 | o_gridmap = 255 - o_gridmap; 65 | o_maxy = o_gridmap.rows; 66 | 67 | //compute the borders 68 | getBorders(); 69 | } 70 | 71 | 72 | void GMap::getBorders() 73 | { 74 | cv::Mat binMap; 75 | cv::threshold(o_gridmap, binMap, 127, 255, 0); 76 | std::vector locations; 77 | cv::findNonZero(binMap, locations); 78 | cv::Rect rect = cv::boundingRect(locations); 79 | 80 | // remember: cv::Point = (col, row) - might consider inverting this 81 | // or using Eigen 82 | cv::Point tl = rect.tl(); 83 | cv::Point br = rect.br(); 84 | o_topLeft = Eigen::Vector2f(tl.x, tl.y); 85 | o_bottomRight = Eigen::Vector2f((br.x < o_gridmap.cols) ? br.x : o_gridmap.cols - 1, (br.y < o_maxy) ? br.y : (o_maxy - 1)); 86 | } 87 | 88 | Eigen::Vector2f GMap::World2Map(Eigen::Vector2f xy) const 89 | { 90 | int u = round((xy(0) - o_origin(0)) / o_resolution); 91 | int v = o_maxy - round((xy(1) - o_origin(1)) / o_resolution); 92 | return Eigen::Vector2f(u, v); 93 | } 94 | 95 | 96 | 97 | Eigen::Vector2f GMap::Map2World(Eigen::Vector2f uv) const 98 | { 99 | float x = uv(0) * o_resolution + o_origin(0); 100 | float y = (o_maxy - uv(1)) * o_resolution + o_origin(1); 101 | return Eigen::Vector2f(x, y); 102 | } 103 | 104 | 105 | bool GMap::IsValid(Eigen::Vector3f pose) const 106 | { 107 | Eigen::Vector2f xy = Eigen::Vector2f(pose(0), pose(1)); 108 | Eigen::Vector2f mp = World2Map(xy); 109 | 110 | return IsValid2D(mp); 111 | } 112 | 113 | bool GMap::IsInMap(Eigen::Vector2f mp) const 114 | { 115 | Eigen::Vector2f br = o_bottomRight; 116 | if ((mp(0) < 0) || (mp(1) < 0) || (mp(0) > br(0)) || (mp(1) > br(1))) return false; 117 | 118 | return true; 119 | } 120 | 121 | 122 | bool GMap::IsValid2D(Eigen::Vector2f mp) const 123 | { 124 | Eigen::Vector2f br = o_bottomRight; 125 | if ((mp(0) < 0) || (mp(1) < 0) || (mp(0) > br(0)) || (mp(1) > br(1))) return false; 126 | 127 | int val = o_gridmap.at(mp(1) ,mp(0)); 128 | 129 | if (val > 1) return false; 130 | 131 | return true; 132 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/GoodPointsCompression.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: GoodPointsCompression.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "GoodPointsCompression.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace py = pybind11; 19 | using namespace py::literals; 20 | 21 | GoodPointsCompression::GoodPointsCompression(const std::string& modulePath, Eigen::Vector2f detectionNoise) 22 | { 23 | try { 24 | py::initialize_interpreter(); 25 | } 26 | catch (...) { 27 | std::cout << "Interpreter already initialized!" << std::endl; 28 | } 29 | py::module_ sys = py::module_::import("sys"); 30 | py::list path = sys.attr("path"); 31 | path.attr("append")(modulePath); 32 | 33 | //py::object gp = py::module_::import("goodpoints"); 34 | dc = py::module_::import("dist_compress"); 35 | comp_kt = dc.attr("compresspp_kt_test"); 36 | 37 | o_detectionNoise = detectionNoise; 38 | o_resampler = std::make_shared(Resampling()); 39 | } 40 | 41 | 42 | Eigen::MatrixXi GoodPointsCompression::Compress(Eigen::MatrixXd& data) 43 | { 44 | py::object result = comp_kt(data); 45 | Eigen::MatrixXi coreset = result.cast(); 46 | 47 | return coreset; 48 | } 49 | 50 | 51 | 52 | 53 | std::shared_ptr GoodPointsCompression::Compress(const std::vector& particles_, Eigen::Vector3f trans) 54 | { 55 | std::vector particles = computeDetectedParticles(particles_, trans); 56 | o_resampler->SetTH(10000000000); //always resample 57 | o_resampler->Resample(particles); 58 | 59 | int n = particles.size(); 60 | Eigen::MatrixXd data(n ,2); 61 | 62 | #pragma omp parallel for 63 | for(int i = 0; i < n; ++i) 64 | { 65 | data(i, 0) = particles[i].pose(0); 66 | data(i, 1) = particles[i].pose(1); 67 | } 68 | 69 | 70 | py::object result = comp_kt(data); 71 | Eigen::MatrixXi c = result.cast(); 72 | 73 | int m = c.rows(); 74 | std::vector coreset(m); 75 | #pragma omp parallel for 76 | for(int i = 0; i < m; ++i) 77 | { 78 | int index = c(i, 0); 79 | coreset[i] = particles[index]; 80 | } 81 | 82 | float r = trans.head(2).norm(); 83 | Eigen::Vector2f variance; 84 | variance(0) = r * o_detectionNoise(0); 85 | variance(1) = o_detectionNoise(1); 86 | 87 | //maybe compute the variance here somehow? 88 | 89 | std::shared_ptr distData = std::make_shared(DistributionData(DISTRIBUTION::GOODPOINTS, coreset, variance)); 90 | 91 | return distData; 92 | } 93 | 94 | void GoodPointsCompression::Fuse(std::vector& particles, std::shared_ptr& distData) 95 | { 96 | std::vector observerParticles = distData->Particles(); 97 | Eigen::Vector2f variance = distData->Variance(); 98 | 99 | int k = observerParticles.size(); 100 | 101 | #pragma omp parallel for 102 | for(long unsigned int i = 0; i < particles.size(); ++i) 103 | { 104 | double w = 0; 105 | Particle p = particles[i]; 106 | Eigen::Vector3f pDet = p.pose; 107 | 108 | for(long unsigned int j = 0; j < k; ++j) 109 | { 110 | Eigen::Vector3f pObs = observerParticles[j].pose; 111 | float dx = pObs(0) - pDet(0); 112 | float dy = pObs(1) - pDet(1); 113 | float dr = dx * dx + dy * dy; 114 | float dt = Wrap2Pi(atan2(dy, dx)); 115 | 116 | w += exp(-0.5 * (dr / variance(0)));// + (d_t * d_t) / variance(1)); //* observerParticles[j].weight; 117 | 118 | } 119 | 120 | // We need some weighing for wrong observation, like p(right detection)*p(r, t dist) + p(wrong detection) 121 | particles[i].weight *= (w + o_penalty); 122 | } 123 | 124 | //o_resampler->SetTH(0.5); 125 | //o_resampler->Resample(particles); 126 | ReciprocalSampling(particles, distData); 127 | } 128 | 129 | void GoodPointsCompression::ReciprocalSampling(std::vector& particles, std::shared_ptr& distData) 130 | { 131 | std::vector observerParticles = distData->Particles(); 132 | Eigen::Vector2f variance = distData->Variance(); 133 | 134 | int k = observerParticles.size(); 135 | 136 | o_resampler->SetTH(10000000000); //always resample 137 | o_resampler->Resample(particles); 138 | 139 | float dx = abs(variance(0) * cos(variance(1))); 140 | float dy = abs(variance(0) * sin(variance(1))); 141 | 142 | float alpha = 0.06; 143 | #pragma omp parallel for 144 | for(long unsigned int i = 0; i < particles.size(); ++i) 145 | { 146 | if(drand48() < alpha) 147 | { 148 | int c = drand48() * k; 149 | particles[i] = observerParticles[c]; 150 | particles[i].weight = 1.0 / particles.size(); 151 | particles[i].pose(0) += SampleGuassian(sqrt(dx)); 152 | particles[i].pose(1) += SampleGuassian(sqrt(dy)); 153 | particles[i].pose(2) += Wrap2Pi(2 * M_PI * drand48() - M_PI); 154 | } 155 | } 156 | } 157 | 158 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/KMeansClustering.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: KMeansClustering.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "KMeansClustering.h" 13 | #include "Utils.h" 14 | #include 15 | 16 | KMeansClustering::KMeansClustering(int k, int epochs, Eigen::Vector2f detectionNoise) 17 | { 18 | o_k = k; 19 | o_epochs = epochs; 20 | o_detectionNoise = detectionNoise; 21 | o_resampler = std::make_shared(Resampling()); 22 | } 23 | 24 | 25 | std::shared_ptr KMeansClustering::Compress(const std::vector& particles_, Eigen::Vector3f trans) 26 | { 27 | std::vector particles = computeDetectedParticles(particles_, trans); 28 | 29 | size_t n = particles.size(); 30 | std::vector points(n); 31 | std::vector weights(n); 32 | std::vector clusters(o_k); 33 | 34 | #pragma omp parallel for 35 | for(long unsigned int i = 0; i < n; ++i) 36 | { 37 | points[i] = particles[i].pose; 38 | weights[i] = particles[i].weight; 39 | } 40 | 41 | // initialize the clusters 42 | for (int c = 0; c < o_k; ++c) 43 | { 44 | clusters[c].mean = points[rand() % n]; 45 | } 46 | 47 | for (int e = 0; e < o_epochs; ++e) 48 | { 49 | AssignClusters(points, weights, clusters); 50 | RecomputeCentroids(clusters); 51 | } 52 | 53 | // incorporate detection error variance 54 | float r = trans.head(2).norm(); 55 | float varX = abs(r * o_detectionNoise(0) * cos(o_detectionNoise(1))); 56 | float varY = abs(r * o_detectionNoise(0) * sin(o_detectionNoise(1))); 57 | 58 | #pragma omp parallel for 59 | for (int c = 0; c < o_k; ++c) 60 | { 61 | clusters[c].ComputeVariance(); 62 | clusters[c].variance(0) += varX; 63 | clusters[c].variance(1) += varY; 64 | clusters[c].ComputeError(); 65 | clusters[c].ComputeWeight(); 66 | } 67 | 68 | 69 | 70 | 71 | std::shared_ptr distData = std::make_shared(DistributionData(DISTRIBUTION::KMEANS, clusters)); 72 | 73 | return distData; 74 | } 75 | 76 | void KMeansClustering::Fuse(std::vector& particles, std::shared_ptr& distData) 77 | { 78 | 79 | std::vector clusters = distData->KMeans(); 80 | int k = clusters.size(); 81 | size_t n = particles.size(); 82 | 83 | #pragma omp parallel for 84 | for(long unsigned int i = 0; i < n; ++i) 85 | { 86 | Eigen::Vector3f p = particles[i].pose; 87 | double w = 0.0; 88 | 89 | for (int c = 0; c < k; ++c) 90 | { 91 | Eigen::Vector3f delta = p - clusters[c].mean; 92 | 93 | w += exp(- 0.5 * ((delta(0) * delta(0))/ clusters[c].variance(0) + ((delta(1) * delta(1))/ clusters[c].variance(1)))) * clusters[c].totalWeight; 94 | } 95 | 96 | particles[i].weight *= w ; 97 | } 98 | 99 | ReciprocalSampling(particles, distData); 100 | } 101 | 102 | void KMeansClustering::ReciprocalSampling(std::vector& particles, std::shared_ptr& distData) 103 | { 104 | std::vector clusters = distData->KMeans(); 105 | int k = clusters.size(); 106 | 107 | std::vector accW(k, 0.0); 108 | double w = 0; 109 | for(int j = 0; j < k; ++j) 110 | { 111 | w += clusters[j].totalWeight; 112 | accW[j] = w; 113 | } 114 | for(int j = 0; j < k; ++j) 115 | { 116 | accW[j] /= w; 117 | } 118 | 119 | float alpha = 0.06; 120 | o_resampler->SetTH(10000000000); //always resample 121 | o_resampler->Resample(particles); 122 | 123 | #pragma omp parallel for 124 | for(long unsigned int i = 0; i < particles.size(); ++i) 125 | { 126 | if(drand48() < alpha) 127 | { 128 | int c = 0; 129 | float u = drand48(); 130 | while(u > accW[c]) 131 | { 132 | ++c; 133 | } 134 | 135 | float x = clusters[c].mean(0) + SampleGuassian(sqrt(clusters[c].variance(0))); 136 | float y = clusters[c].mean(1) + SampleGuassian(sqrt(clusters[c].variance(1))); 137 | 138 | /*while (!o_gmap->IsValid(Eigen::Vector3f(x,y, 0))) 139 | { 140 | c = 0; 141 | float u = drand48(); 142 | while(u > accW[c]) 143 | { 144 | ++c; 145 | } 146 | x = clusters[c].mean(0) + SampleGuassian(sqrt(clusters[c].variance(0))); 147 | y = clusters[c].mean(1) + SampleGuassian(sqrt(clusters[c].variance(1))); 148 | }*/ 149 | 150 | particles[i].pose(0) = x; 151 | particles[i].pose(1) = y; 152 | particles[i].pose(2) = 2 * M_PI * drand48() - M_PI; 153 | particles[i].weight = 1 / particles.size(); 154 | } 155 | } 156 | } 157 | 158 | 159 | 160 | 161 | 162 | void KMeansClustering::AssignClusters(std::vector& points, std::vector& weights, std::vector& clusters) 163 | { 164 | int pointNum = points.size(); 165 | int k = clusters.size(); 166 | 167 | std::vector minDistance(pointNum, __FLT_MAX__); 168 | std::vector clusterID(pointNum, 0); 169 | 170 | #pragma omp parallel for 171 | for (int c = 0; c < k; ++c) 172 | { 173 | clusters[c].points.clear(); 174 | } 175 | 176 | #pragma omp parallel for 177 | for (int i = 0; i < pointNum; ++i) 178 | { 179 | //uint cID = 0; 180 | for (int c = 0; c < k; ++c) 181 | { 182 | Eigen::Vector2f diff = points[i].head(2) - clusters[c].mean.head(2); 183 | float dist = diff.squaredNorm(); 184 | 185 | if (dist < minDistance[i]) 186 | { 187 | minDistance[i] = dist; 188 | clusterID[i] = c; 189 | } 190 | } 191 | } 192 | 193 | for (int i = 0; i < pointNum; ++i) 194 | { 195 | clusters[clusterID[i]].points.push_back(points[i]); 196 | clusters[clusterID[i]].weights.push_back(weights[i]); 197 | } 198 | } 199 | 200 | void KMeansClustering::RecomputeCentroids(std::vector& clusters) 201 | { 202 | int k = clusters.size(); 203 | 204 | #pragma omp parallel for 205 | for (int c = 0; c < k; ++c) 206 | { 207 | clusters[c].ComputeMean(); 208 | } 209 | } 210 | 211 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/Lidar2D.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Lidar2D.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "Lidar2D.h" 14 | #include "Utils.h" 15 | #include 16 | #include 17 | #include 18 | 19 | Lidar2D::Lidar2D(std::string name, Eigen::Vector3f origin, int numBeams, float maxAngle, float minAngle) 20 | { 21 | o_name = name; 22 | o_trans = Vec2Trans(origin); 23 | 24 | 25 | float reso = (maxAngle - minAngle) / (numBeams - 1); 26 | for(int i = 0; i < numBeams; ++i) 27 | { 28 | float angle = minAngle + i * reso; 29 | o_heading.push_back(angle); 30 | } 31 | } 32 | 33 | Lidar2D::Lidar2D(std::string jsonPath) 34 | { 35 | using json = nlohmann::json; 36 | 37 | std::ifstream file(jsonPath); 38 | json config; 39 | file >> config; 40 | o_name = config["name"]; 41 | float maxAngle = config["angle_max"]; 42 | float minAngle = config["angle_min"]; 43 | int numBeams = config["num_beams"]; 44 | std::vector origin = config["origin"]; 45 | o_trans = Vec2Trans(Eigen::Vector3f(origin[0], origin[1], origin[2])); 46 | float reso = (maxAngle - minAngle) / (numBeams - 1); 47 | for(int i = 0; i < numBeams; ++i) 48 | { 49 | float angle = minAngle + i * reso; 50 | o_heading.push_back(angle); 51 | } 52 | 53 | } 54 | 55 | 56 | Lidar2D::Lidar2D(std::string name, std::string yamlFolder) 57 | { 58 | o_name = name; 59 | std::vector fields = File2Lines(yamlFolder + name + ".yaml"); 60 | 61 | // "angle_max: " - 11 62 | fields[1].erase(0,11); 63 | // "angle_min: " - 11 64 | fields[2].erase(0,11); 65 | // "num_beams: " - 11 66 | fields[3].erase(0,11); 67 | // "origin: " - 8 68 | fields[4].erase(0,8); 69 | 70 | float maxAngle = std::stof(fields[1]); 71 | float minAngle = std::stof(fields[2]); 72 | int numBeams = std::stoi(fields[3]); 73 | std::vector vec = StringToVec(fields[4]); 74 | Eigen::Vector3f origin = Eigen::Vector3f(vec[0], vec[1], vec[2]); 75 | o_trans = Vec2Trans(origin); 76 | 77 | float reso = (maxAngle - minAngle) / (numBeams - 1); 78 | for(int i = 0; i < numBeams; ++i) 79 | { 80 | float angle = minAngle + i * reso; 81 | o_heading.push_back(angle); 82 | } 83 | } 84 | 85 | 86 | 87 | std::vector Lidar2D::Center(const std::vector& ranges) 88 | { 89 | std::vector points_3d = Ranges2Points(ranges, o_heading); 90 | 91 | int n = points_3d.size(); 92 | std::vector transPoints(n); 93 | 94 | for(int i = 0; i < n; i++) 95 | { 96 | Eigen::Vector3f p = points_3d[i]; 97 | Eigen::Vector3f p_trans = o_trans * p; 98 | transPoints[i] = p_trans; 99 | } 100 | 101 | return transPoints; 102 | } 103 | 104 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/MCL.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: MCL.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "MCL.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "Utils.h" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | MCL::MCL(const std::string& jsonConfigPath) 27 | { 28 | using json = nlohmann::json; 29 | 30 | std::ifstream file(jsonConfigPath); 31 | json config; 32 | try 33 | { 34 | file >> config; 35 | } 36 | catch(std::exception& e) 37 | { 38 | std::cerr << "Failed to load config fail" << std::endl; 39 | std::cout << e.what() << '\n'; 40 | } 41 | 42 | o_motionModel = std::make_shared(FSR()); 43 | o_resampler = std::make_shared(Resampling()); 44 | 45 | std::string folderPath = boost::filesystem::path(jsonConfigPath).parent_path().string() + "/"; 46 | std::string mapPath = std::string(config["mapPath"]); 47 | o_gmap = std::make_shared(GMap(folderPath, mapPath)); 48 | 49 | float likelihoodSigma = config["sensorModel"]["likelihoodSigma"]; 50 | float maxDist = config["sensorModel"]["maxDist"]; 51 | int wScheme = config["sensorModel"]["weightingScheme"]; 52 | o_beamEndModel = std::make_shared(o_gmap, likelihoodSigma, maxDist, BeamEnd::Weighting(1)); 53 | 54 | int clusterNum = config["detectionModel"]["clusters"]; 55 | DISTRIBUTION distType = static_cast(config["detectionModel"]["type"]); 56 | //o_distExcange = std::make_shared(DistributionExchange(clusterNum, o_detectionNoise)); 57 | o_distExcange = std::make_shared(DistributionExchange(jsonConfigPath)); 58 | 59 | o_particleFilter = std::make_shared(ParticleFilter(o_gmap)); 60 | o_numParticles = config["numParticles"]; 61 | bool tracking = config["tracking"]["mode"]; 62 | if (tracking) 63 | { 64 | std::vector covariances; 65 | std::vector initGuesses; 66 | 67 | std::vector x = config["tracking"]["x"]; 68 | std::vector y = config["tracking"]["y"]; 69 | std::vector yaw = config["tracking"]["yaw"]; 70 | std::vector covx = config["tracking"]["cov_x"]; 71 | std::vector covy = config["tracking"]["cov_y"]; 72 | std::vector covyaw = config["tracking"]["cov_yaw"]; 73 | 74 | for (int i = 0; i < x.size(); ++i) 75 | { 76 | Eigen::Matrix3d cov; 77 | cov << covx[i], 0, 0, 0, covy[i], 0, 0, 0, covyaw[i]; 78 | covariances.push_back(cov); 79 | initGuesses.push_back(Eigen::Vector3f(x[i], y[i], yaw[i])); 80 | } 81 | o_particleFilter->InitGaussian(o_particles, o_numParticles, initGuesses, covariances); 82 | } 83 | else o_particleFilter->InitUniform(o_particles, o_numParticles); 84 | 85 | o_stats = SetStatistics::ComputeParticleSetStatistics(o_particles); 86 | } 87 | 88 | MCL::MCL(std::shared_ptr gmap, std::shared_ptr mm, std::shared_ptr sm, 89 | std::shared_ptr rs, int n_particles) 90 | { 91 | o_motionModel = mm; 92 | o_beamEndModel = sm; 93 | o_resampler = rs; 94 | o_numParticles = n_particles; 95 | o_gmap = gmap; 96 | o_detectionNoise = Eigen::Vector2f(0.1, 0.1); 97 | 98 | o_distExcange = std::make_shared(DistributionExchange(8, o_detectionNoise)); 99 | o_particleFilter = std::make_shared(ParticleFilter(o_gmap)); 100 | o_particleFilter->InitUniform(o_particles, o_numParticles); 101 | o_stats = SetStatistics::ComputeParticleSetStatistics(o_particles); 102 | 103 | //dumpParticles(); 104 | 105 | } 106 | 107 | MCL::MCL(std::shared_ptr gmap, std::shared_ptr mm, std::shared_ptr sm, 108 | std::shared_ptr rs, int n_particles, std::vector initGuess, 109 | std::vector covariances) 110 | { 111 | o_motionModel = mm; 112 | o_beamEndModel = sm; 113 | o_resampler = rs; 114 | o_numParticles = n_particles; 115 | o_gmap = gmap; 116 | 117 | o_particleFilter = std::make_shared(ParticleFilter(o_gmap)); 118 | o_particleFilter->InitGaussian(o_particles, o_numParticles, initGuess, covariances); 119 | o_stats = o_particleFilter->ComputeStatistics(o_particles); 120 | 121 | } 122 | 123 | 124 | void MCL::dumpParticles() 125 | { 126 | std::string path = std::to_string(o_frame) + "_particles.csv"; 127 | std::ofstream particleFile; 128 | particleFile.open(path, std::ofstream::out); 129 | particleFile << "x" << "," << "y" << "," << "yaw" << "," << "w" << std::endl; 130 | for(long unsigned int p = 0; p < o_particles.size(); ++p) 131 | { 132 | Eigen::Vector3f pose = o_particles[p].pose; 133 | float w = o_particles[p].weight; 134 | particleFile << pose(0) << "," << pose(1) << "," << pose(2) << "," << w << std::endl; 135 | } 136 | particleFile.close(); 137 | ++o_frame; 138 | } 139 | 140 | 141 | 142 | void MCL::Predict(const std::vector& u, const std::vector& odomWeights, const Eigen::Vector3f& noise) 143 | { 144 | #pragma omp parallel for 145 | for(int i = 0; i < o_numParticles; ++i) 146 | { 147 | Eigen::Vector3f pose = o_motionModel->SampleMotion(o_particles[i].pose, u, odomWeights, noise); 148 | o_particles[i].pose = pose; 149 | 150 | // if (drand48() < 0.01) 151 | // { 152 | // std::vector new_particle; 153 | // o_particleFilter->InitUniform(new_particle, 1); 154 | // new_particle[0].weight = 1.0 / o_numParticles; 155 | // o_particles[i] = new_particle[0]; 156 | // } 157 | 158 | //particle pruning - if particle is outside the map, we replace it 159 | while (!o_gmap->IsValid(o_particles[i].pose)) 160 | { 161 | std::vector new_particle; 162 | o_particleFilter->InitUniform(new_particle, 1); 163 | new_particle[0].weight = 1.0 / o_numParticles; 164 | o_particles[i] = new_particle[0]; 165 | } 166 | } 167 | } 168 | 169 | 170 | 171 | 172 | 173 | void MCL::Correct(std::shared_ptr data) 174 | { 175 | o_beamEndModel->ComputeWeights(o_particles, data); 176 | //dumpParticles(); 177 | 178 | //o_particleFilter->NormalizeWeights(o_particles); 179 | o_resampler->Resample(o_particles); 180 | //o_stats = SetStatistics::ComputeParticleSetStatistics(o_particles); 181 | o_stats = o_particleFilter->ComputeStatistics(o_particles); 182 | //std::cout << o_stats.Mean() << std::endl; 183 | 184 | } 185 | 186 | 187 | void MCL::Recover() 188 | { 189 | o_particleFilter->InitUniform(o_particles, o_numParticles); 190 | } 191 | 192 | 193 | int MCL::traceInMap(Eigen::Vector3f p1, Eigen::Vector3f p2) 194 | { 195 | 196 | Eigen::Vector2f uv1 = o_gmap->World2Map(p1.head(2)); 197 | Eigen::Vector2f uv2 = o_gmap->World2Map(p2.head(2)); 198 | 199 | if (false == o_gmap->IsValid2D(uv2)) return -1; 200 | 201 | float dist = (uv2 - uv1).norm(); 202 | Eigen::Vector2f dir = (uv2 - uv1).normalized(); 203 | 204 | Eigen::Vector2f currPose = uv1; 205 | float step = 1; 206 | int count = 0; 207 | 208 | for(int i = 0; i < int(dist / step); ++i) 209 | { 210 | currPose += step * dir; 211 | if (!o_gmap->IsValid2D(currPose)) 212 | { 213 | ++count; 214 | } 215 | } 216 | 217 | return count; 218 | } 219 | 220 | 221 | void MCL::detectionReweight(Eigen::Vector3f trans) 222 | { 223 | std::vector shiftedParticles = computeDetectedParticles(o_particles, trans); 224 | float res = o_gmap->Resolution(); 225 | float r = trans.head(2).norm(); 226 | float sigma = r * o_detectionNoise(0); 227 | float maxDist = 20; 228 | int numParticles = o_particles.size(); 229 | 230 | #pragma omp parallel for 231 | for (long unsigned int i = 0; i < numParticles; ++i) 232 | { 233 | int trace = traceInMap(o_particles[i].pose, shiftedParticles[i].pose); 234 | if (trace < 0) o_particles[i].weight *= exp(-0.5 *(maxDist * maxDist)/ sigma); 235 | else 236 | { 237 | o_particles[i].weight *= exp(-0.5 * (trace * res) * (trace * res) / sigma); 238 | } 239 | 240 | } 241 | 242 | o_particleFilter->NormalizeWeights(o_particles); 243 | } 244 | 245 | 246 | #include 247 | std::shared_ptr MCL::CreateDistribution(DISTRIBUTION distType, const Eigen::Vector3f& relTrans) 248 | { 249 | 250 | /*if ((distType != DISTRIBUTION::DET) and (distType != DISTRIBUTION::PROROK)) 251 | { 252 | detectionReweight(relTrans); 253 | }*/ 254 | std::vector detectedParticles; 255 | std::copy(o_particles.begin(), o_particles.end(), std::back_inserter(detectedParticles)); 256 | 257 | 258 | std::shared_ptr distData = o_distExcange->CreateDistribution(detectedParticles, distType, relTrans); 259 | o_stats = o_particleFilter->ComputeStatistics(o_particles); 260 | 261 | 262 | return distData; 263 | } 264 | 265 | void MCL::FuseDistribution(std::shared_ptr& distData) 266 | { 267 | DISTRIBUTION distType = distData->Type(); 268 | Eigen::Vector2f variance = distData->Variance(); 269 | /*switch(distType) 270 | { 271 | case DISTRIBUTION::PARTICLES: 272 | { 273 | if ((variance(0) > 2.0) || (variance(1) > 2.0)) return; 274 | break; 275 | } 276 | case DISTRIBUTION::DET: 277 | { 278 | if ((variance(0) > 2.0) || (variance(1) > 2.0)) return; 279 | break; 280 | } 281 | case DISTRIBUTION::KMEANS: 282 | { 283 | bool ignore = true; 284 | std::vector variances = distData->KMeans()->variances; 285 | for(long unsigned int i = 0; i < variances.size(); ++i) 286 | { 287 | Eigen::Vector2f variance = variances[i]; 288 | if ((variance(0) < 2.0) && (variance(1) < 2.0)) ignore = false; 289 | } 290 | if (ignore) return; 291 | break; 292 | } 293 | }*/ 294 | /*static int countB = 0; 295 | using namespace std; 296 | ofstream myfile; 297 | myfile.open ("particlesB_" + std::to_string(countB) + ".csv", std::ios_base::app); 298 | ++countB; 299 | 300 | for(long unsigned int i = 0; i < o_particles.size(); ++i) 301 | { 302 | Eigen::Vector3f p = o_particles[i].pose; 303 | myfile << p(0) << ", " << p(1) << ", " << p(2) << ", " << o_particles[i]. weight << std::endl; 304 | } 305 | myfile.close();*/ 306 | 307 | o_distExcange->FuseDistribution(o_particles, distData); 308 | 309 | //o_resampler->Resample(o_particles); 310 | //o_stats = o_particleFilter->ComputeStatistics(o_particles); 311 | } 312 | 313 | 314 | 315 | 316 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/Particle.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Particle.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "Particle.h" 14 | 15 | Particle::Particle(Eigen::Vector3f p, double w) 16 | { 17 | pose = p; 18 | weight = w; 19 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/ParticleFilter.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: ParticleFilter.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "ParticleFilter.h" 13 | #include "Utils.h" 14 | #include 15 | #include 16 | 17 | ParticleFilter::ParticleFilter(std::shared_ptr GMap) 18 | { 19 | o_gmap = GMap; 20 | } 21 | 22 | 23 | 24 | 25 | void ParticleFilter::InitUniform(std::vector& particles, int n_particles) 26 | { 27 | particles = std::vector(n_particles); 28 | 29 | Eigen::Vector2f tl = o_gmap->Map2World(o_gmap->TopLeft()); 30 | Eigen::Vector2f br = o_gmap->Map2World(o_gmap->BottomRight()); 31 | 32 | int i = 0; 33 | while(i < n_particles) 34 | { 35 | float x = drand48() * (br(0) - tl(0)) + tl(0); 36 | float y = drand48() * (br(1) - tl(1)) + tl(1); 37 | if(!o_gmap->IsValid(Eigen::Vector3f(x, y, 0))) continue; 38 | 39 | float theta = drand48() * 2 * M_PI - M_PI; 40 | Particle p(Eigen::Vector3f(x, y, theta), 1.0 / n_particles); 41 | particles[i] = p; 42 | ++i; 43 | } 44 | 45 | //particles[0] = Particle(Eigen::Vector3f(0.0, 0.0, 0.0), 1.0 / n_particles); 46 | 47 | 48 | o_particles = particles; 49 | } 50 | 51 | 52 | void ParticleFilter::InitGaussian(std::vector& particles, int n_particles, const std::vector& initGuess, const std::vector& covariances) 53 | { 54 | particles = std::vector(n_particles); 55 | 56 | for(long unsigned int i = 0; i < initGuess.size(); ++i) 57 | { 58 | Eigen::Vector3f initG = initGuess[i].head(3); 59 | Eigen::Matrix3d cov = covariances[i]; 60 | 61 | float dx = fabs(SampleGuassian(cov(0, 0))); 62 | float dy = fabs(SampleGuassian(cov(1, 1))); 63 | float dt = fabs(SampleGuassian(cov(2, 2))); 64 | 65 | if (cov(2, 2) < 0.0) dt = M_PI; 66 | 67 | Eigen::Vector3f delta(dx, dy, dt); 68 | 69 | Eigen::Vector3f tl = initG + delta; 70 | Eigen::Vector3f br = initG - delta; 71 | 72 | int n = 0; 73 | while(n < n_particles) 74 | { 75 | float x = drand48() * (br(0) - tl(0)) + tl(0); 76 | float y = drand48() * (br(1) - tl(1)) + tl(1); 77 | if(!o_gmap->IsValid(Eigen::Vector3f(x, y, 0))) continue; 78 | 79 | float theta = drand48() * (br(2) - tl(2)) + tl(2); 80 | Particle p(Eigen::Vector3f(x, y, theta), 1.0 / n_particles); 81 | particles[n + n_particles * i] = p; 82 | ++n; 83 | } 84 | } 85 | 86 | o_particles = particles; 87 | } 88 | 89 | 90 | 91 | 92 | SetStatistics ParticleFilter::ComputeStatistics(const std::vector& particles) 93 | { 94 | o_stats = SetStatistics::ComputeParticleSetStatistics(o_particles); 95 | 96 | return o_stats; 97 | } 98 | 99 | 100 | 101 | 102 | void ParticleFilter::NormalizeWeights(std::vector& particles) 103 | { 104 | 105 | double w = 0; 106 | for(long unsigned int i = 0; i < particles.size(); ++i) 107 | { 108 | w += particles[i].weight; 109 | } 110 | 111 | for(long unsigned int i = 0; i < particles.size(); ++i) 112 | { 113 | particles[i].weight = particles[i].weight / w; 114 | } 115 | } 116 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/ProrokClustering.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: ProrokClustering.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "ProrokClustering.h" 13 | #include 14 | 15 | 16 | ProrokClustering::ProrokClustering(int k, Eigen::Vector2f detectionNoise) 17 | { 18 | o_k = k; 19 | o_detectionNoise = detectionNoise; 20 | o_resampler = std::make_shared(Resampling()); 21 | 22 | } 23 | 24 | void ProrokClustering::findHighestVarianceCluster(std::vector& clusters, int& index, int& dim) 25 | { 26 | int k = clusters.size(); 27 | 28 | float max_variance = 0; 29 | 30 | for(int i = 0; i < k; ++i) 31 | { 32 | clusters[i].ComputeVariance(); 33 | 34 | if (clusters[i].variance(0) > clusters[i].variance(1) && clusters[i].variance(0) > max_variance) 35 | { 36 | index = i; 37 | max_variance = clusters[i].variance(0); 38 | dim = 0; 39 | } 40 | else if (clusters[i].variance(1) >= clusters[i].variance(0) && clusters[i].variance(1) > max_variance) 41 | { 42 | index = i; 43 | max_variance = clusters[i].variance(1); 44 | dim = 1; 45 | } 46 | } 47 | } 48 | 49 | void ProrokClustering::splitClusters(std::vector& clusters, int& index, int& dim) 50 | { 51 | std::vector new_clusters; 52 | for(int i = 0; i < clusters.size(); ++i) 53 | { 54 | if (i != index) new_clusters.push_back(clusters[i]); 55 | } 56 | 57 | //split along the mean on the dim 58 | Cluster c1; 59 | Cluster c2; 60 | int numPoints = clusters[index].points.size(); 61 | 62 | for(int i = 0; i < numPoints; ++i) 63 | { 64 | if (clusters[index].mean(dim) >= clusters[index].points[i](dim)) 65 | { 66 | c1.points.push_back(clusters[index].points[i]); 67 | c1.weights.push_back(clusters[index].weights[i]); 68 | } 69 | else 70 | { 71 | c2.points.push_back(clusters[index].points[i]); 72 | c2.weights.push_back(clusters[index].weights[i]); 73 | } 74 | } 75 | 76 | 77 | // add new clusters to the list 78 | new_clusters.push_back(c1); 79 | new_clusters.push_back(c2); 80 | 81 | 82 | //clusters.erase(clusters.begin() + index); 83 | clusters = new_clusters; 84 | 85 | } 86 | 87 | std::vector ProrokClustering::ComputeClusters(const std::vector& particles) 88 | { 89 | int numPoints = particles.size(); 90 | std::vector points(numPoints); 91 | std::vector weights(numPoints); 92 | 93 | double w = 0.0; 94 | for(long unsigned int i = 0; i < numPoints; ++i) 95 | { 96 | w += particles[i].weight; 97 | } 98 | 99 | #pragma omp parallel for 100 | for(long unsigned int i = 0; i < numPoints; ++i) 101 | { 102 | points[i] = particles[i].pose; 103 | weights[i] = particles[i].weight / w; 104 | } 105 | 106 | Cluster c0; 107 | c0.points = points; 108 | c0.weights = weights; 109 | std::vector clusters; 110 | clusters.push_back(c0); 111 | 112 | if (o_k == 1) 113 | { 114 | clusters[0].ComputeVariance(); 115 | } 116 | 117 | for(long unsigned int i = 0; i < o_k - 1; ++i) 118 | { 119 | int dim = -1; 120 | int index = -1; 121 | findHighestVarianceCluster(clusters, index, dim); 122 | splitClusters(clusters, index, dim); 123 | } 124 | int dim = -1; 125 | int index = -1; 126 | findHighestVarianceCluster(clusters, index, dim); 127 | 128 | return clusters; 129 | } 130 | 131 | std::shared_ptr ProrokClustering::Compress(const std::vector& particles, Eigen::Vector3f trans) 132 | { 133 | std::vector clusters = ComputeClusters(particles); 134 | 135 | std::vector clusterAbstractions(o_k); 136 | 137 | float r = trans.head(2).norm(); 138 | #pragma omp parallel for 139 | for(int i = 0; i < o_k; ++i) 140 | { 141 | clusterAbstractions[i] = ClusterAbstraction(clusters[i], trans); 142 | clusterAbstractions[i].covariance(0,0) += o_detectionNoise(0) * r; 143 | clusterAbstractions[i].covariance(1,1) += o_detectionNoise(1); 144 | } 145 | 146 | std::shared_ptr distData = std::make_shared(DistributionData(DISTRIBUTION::PROROK, clusterAbstractions)); 147 | 148 | return distData; 149 | } 150 | 151 | void ProrokClustering::Fuse(std::vector& particles, std::shared_ptr& distData) 152 | { 153 | std::vector clusterAbstractions = distData->Prorok(); 154 | int k = clusterAbstractions.size(); 155 | 156 | #pragma omp parallel for 157 | for(long unsigned int i = 0; i < particles.size(); ++i) 158 | { 159 | Eigen::Vector3f p = particles[i].pose; 160 | double w = 0; 161 | 162 | for(int j = 0; j < k; ++j) 163 | { 164 | if (clusterAbstractions[j].weight < 0.01) continue; 165 | 166 | Eigen::Vector2f delta = ClusterAbstraction::computePolarDifference(clusterAbstractions[j].centroid, p); 167 | double dx = (delta(0) - clusterAbstractions[j].mu(0)); 168 | double dy = (delta(1) - clusterAbstractions[j].mu(1)); 169 | double sx = (clusterAbstractions[j].covariance(0, 0)); 170 | double sy = (clusterAbstractions[j].covariance(1, 1)); 171 | 172 | w += clusterAbstractions[j].weight * exp(-0.5 * ((dx * dx) / sx + (dy * dy) / sy)); 173 | 174 | } 175 | particles[i].weight *= w ; 176 | } 177 | 178 | ReciprocalSampling(particles, distData); 179 | } 180 | 181 | void ProrokClustering::ReciprocalSampling(std::vector& particles, std::shared_ptr& distData) 182 | { 183 | std::vector clusterAbstractions = distData->Prorok(); 184 | int k = clusterAbstractions.size(); 185 | 186 | std::vector accW(k, 0.0); 187 | 188 | double w = 0; 189 | for(int j = 0; j < k; ++j) 190 | { 191 | w += clusterAbstractions[j].weight; 192 | accW[j] = w; 193 | } 194 | for(int j = 0; j < k; ++j) 195 | { 196 | clusterAbstractions[j].weight /= w; 197 | accW[j] /= w; 198 | } 199 | 200 | o_resampler->SetTH(10000000000); //always resample 201 | o_resampler->Resample(particles); 202 | 203 | 204 | #pragma omp parallel for 205 | for(long unsigned int i = 0; i < particles.size(); ++i) 206 | { 207 | if(drand48() < 0.06) 208 | { 209 | int c = 0; 210 | float u = drand48(); 211 | while(u > accW[c]) 212 | { 213 | ++c; 214 | } 215 | 216 | Eigen::Vector3f centroid = clusterAbstractions[c].centroid; 217 | float r = clusterAbstractions[c].mu(0); 218 | float theta = clusterAbstractions[c].mu(1); 219 | float dr = SampleGuassian(sqrt(clusterAbstractions[c].covariance(0, 0))); 220 | float dt = SampleGuassian(sqrt(clusterAbstractions[c].covariance(1, 1))); 221 | 222 | Eigen::Vector3f p; 223 | p(0) = centroid(0) + (r + dr) * cos(centroid(2) + theta + dt); 224 | p(1) = centroid(1) + (r + dr) * sin(centroid(2) + theta + dt); 225 | p(2) = 2 * M_PI * drand48() - M_PI; 226 | 227 | particles[i].pose = p; 228 | particles[i].weight = 1.0 / particles.size(); 229 | } 230 | } 231 | 232 | 233 | } 234 | 235 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/Resampling.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: LowVarianceResampling.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "Resampling.h" 14 | 15 | #include 16 | #include 17 | 18 | void Resampling::Resample(std::vector& particles) 19 | { 20 | int n_particles = particles.size(); 21 | std::vector transWeights(n_particles); 22 | std::vector new_particles(n_particles); 23 | 24 | double w = 0; 25 | for(long unsigned int i = 0; i < n_particles; ++i) 26 | { 27 | w += particles[i].weight; 28 | } 29 | 30 | for(long unsigned int i = 0; i < n_particles; ++i) 31 | { 32 | particles[i].weight = particles[i].weight / w; 33 | } 34 | 35 | 36 | std::transform(particles.begin(), particles.end(), transWeights.begin(), [](Particle &p){ return p.weight * p.weight; }); 37 | double sumWeights = std::accumulate(transWeights.begin(), transWeights.end(), 0.0); 38 | 39 | double effN = 1.0 / sumWeights; 40 | 41 | if (effN < o_th * n_particles) 42 | { 43 | double unitW = 1.0 / n_particles; 44 | //std::cout << "resample" << std::endl; 45 | double r = drand48() * 1.0 / n_particles; 46 | double acc = particles[0].weight; 47 | int i = 0; 48 | 49 | for(int j = 0; j < n_particles; ++j) 50 | { 51 | double U = r + j * 1.0 / n_particles; 52 | while((U > acc) && (i < n_particles)) 53 | { 54 | ++i; 55 | acc += particles[i].weight; 56 | } 57 | particles[i].weight = unitW; 58 | new_particles[j] = particles[i]; 59 | } 60 | particles = new_particles; 61 | } 62 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/SetStatistics.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: SetStatistics.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "SetStatistics.h" 13 | #include 14 | 15 | SetStatistics SetStatistics::ComputeParticleSetStatistics(const std::vector& particles) 16 | { 17 | 18 | Eigen::Vector4d m = Eigen::Vector4d(0, 0, 0, 0); 19 | Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(3, 3); 20 | int n = particles.size(); 21 | double tot_w = 0.0; 22 | 23 | for(int i = 0; i < n; ++i) 24 | { 25 | Eigen::Vector3f p = particles[i].pose; 26 | double w = particles[i].weight; 27 | 28 | m(0) += p(0) * w; // mean x 29 | m(1) += p(1) * w; // mean y 30 | m(2) += cos(p(2)) * w; // theta 31 | m(3) += sin(p(2)) * w; // theta 32 | 33 | tot_w += w; 34 | 35 | // linear components cov 36 | for(int j = 0; j < 2; ++j) 37 | { 38 | for(int k = 0; k < 2; ++k) 39 | { 40 | cov(j,k) += w * p(j) * p(k); 41 | } 42 | } 43 | } 44 | 45 | Eigen::Vector3d mean; 46 | mean(0) = m(0) / tot_w; 47 | mean(1) = m(1) / tot_w; 48 | mean(2) = atan2(m(3), m(2)); 49 | 50 | 51 | // normalize linear components cov 52 | for(int j = 0; j < 2; ++j) 53 | { 54 | for(int k = 0; k < 2; ++k) 55 | { 56 | cov(j, k) = cov(j, k) /tot_w - mean(j) * mean(k); 57 | } 58 | } 59 | 60 | // angular covariance 61 | double R = sqrt(m(2) * m(2) + m(3) * m(3)); 62 | 63 | // https://github.com/ros-planning/navigation/blob/2b807bd312fac1b476851800c84cb962559cbc53/amcl/src/amcl/pf/pf.c#L690 64 | //cov(2, 2) = -2 * log(R); 65 | 66 | // https://www.ebi.ac.uk/thornton-srv/software/PROCHECK/nmr_manual/man_cv.html 67 | cov(2, 2) = 1 - R / tot_w; 68 | 69 | 70 | SetStatistics stats = SetStatistics(mean, cov); 71 | 72 | return stats; 73 | 74 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/StandardThinning.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: StandardThinning.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "StandardThinning.h" 13 | 14 | 15 | 16 | StandardThinning::StandardThinning(int subSize, Eigen::Vector2f detectionNoise) 17 | { 18 | o_n = subSize; 19 | o_detectionNoise = detectionNoise; 20 | o_resampler = std::make_shared(Resampling()); 21 | } 22 | 23 | std::shared_ptr StandardThinning::Compress(const std::vector& particles_, Eigen::Vector3f trans) 24 | { 25 | std::vector particles = computeDetectedParticles(particles_, trans); 26 | 27 | float r = trans.head(2).norm(); 28 | Eigen::Vector2f variance; 29 | variance(0) = r * o_detectionNoise(0); 30 | variance(1) = o_detectionNoise(1); 31 | std::shared_ptr distData; 32 | int m = particles.size(); 33 | 34 | if (m == o_n) 35 | { 36 | distData = std::make_shared(DistributionData(DISTRIBUTION::PARTICLES, particles, variance)); 37 | } 38 | else 39 | { 40 | std::vector subset(o_n); 41 | for(int j = 0; j < o_n; ++j) 42 | { 43 | subset[j] = particles[(int)(drand48() * m)]; 44 | } 45 | distData = std::make_shared(DistributionData(DISTRIBUTION::PARTICLES, subset, variance)); 46 | } 47 | 48 | return distData; 49 | } 50 | 51 | void StandardThinning::Fuse(std::vector& particles, std::shared_ptr& distData) 52 | { 53 | std::vector& observerParticles = distData->Particles(); 54 | Eigen::Vector2f variance = distData->Variance(); 55 | 56 | #pragma omp parallel for 57 | for(long unsigned int i = 0; i < particles.size(); ++i) 58 | { 59 | double w = 0; 60 | Particle p = particles[i]; 61 | Eigen::Vector3f pDet = p.pose; 62 | 63 | for(long unsigned int j = 0; j < observerParticles.size(); ++j) 64 | { 65 | Eigen::Vector3f pObs = observerParticles[j].pose; 66 | float dx = pObs(0) - pDet(0); 67 | float dy = pObs(1) - pDet(1); 68 | float dr = dx * dx + dy * dy; 69 | float dt = Wrap2Pi(atan2(dy, dx)); 70 | 71 | w += exp(-0.5 * (dr / variance(0))) * observerParticles[j].weight; 72 | } 73 | 74 | // We need some weighing for wrong observation, like p(right detection)*p(r, t dist) + p(wrong detection) 75 | particles[i].weight *= (w + o_penalty); 76 | } 77 | 78 | //o_resampler->SetTH(0.5); 79 | //o_resampler->Resample(particles); 80 | ReciprocalSampling(particles, distData); 81 | 82 | } 83 | 84 | void StandardThinning::ReciprocalSampling(std::vector& particles, std::shared_ptr& distData) 85 | { 86 | std::vector& observerParticles = distData->Particles(); 87 | Eigen::Vector2f variance = distData->Variance(); 88 | 89 | int m = observerParticles.size(); 90 | o_resampler->SetTH(10000000000); //always resample 91 | o_resampler->Resample(observerParticles); 92 | o_resampler->Resample(particles); 93 | float dx = abs(variance(0) * cos(variance(1))); 94 | float dy = abs(variance(0) * sin(variance(1))); 95 | 96 | #pragma omp parallel for 97 | for(long unsigned int i = 0; i < particles.size(); ++i) 98 | { 99 | if(drand48() < 0.06) 100 | { 101 | particles[i] = observerParticles[(int)(drand48() * m)]; 102 | particles[i].pose(0) += SampleGuassian(sqrt(dx)); 103 | particles[i].pose(1) += SampleGuassian(sqrt(dy)); 104 | particles[i].pose(2) += 2 * M_PI * drand48() - M_PI; 105 | particles[i].weight = 1.0 / particles.size(); 106 | } 107 | } 108 | 109 | } 110 | 111 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/Utils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Utils.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include "Utils.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | 25 | Eigen::Matrix3f Vec2Trans(Eigen::Vector3f v) 26 | { 27 | float c = cos(v(2)); 28 | float s = sin(v(2)); 29 | Eigen::Matrix3f trans; 30 | trans << c, -s, v(0), s, c, v(1), 0, 0, 1; 31 | 32 | return trans; 33 | } 34 | 35 | float CosineSimilarity(float yaw1, float yaw2) 36 | { 37 | Eigen::Vector2f unitVec1 = Eigen::Vector2f(cos(yaw1), sin(yaw1)); 38 | Eigen::Vector2f unitVec2 = Eigen::Vector2f(cos(yaw2), sin(yaw2)); 39 | 40 | float sim = unitVec1.dot(unitVec2); 41 | 42 | return sim; 43 | } 44 | 45 | float Wrap2Pi(float angle) 46 | { 47 | float wAngle = angle; 48 | while (wAngle < -M_PI) wAngle += 2 * M_PI; 49 | 50 | while (wAngle > M_PI) wAngle -= 2 * M_PI; 51 | 52 | return wAngle; 53 | } 54 | 55 | float GetYaw(float qz, float qw) 56 | { 57 | float yaw = 2 * atan2(qz, qw); 58 | yaw = Wrap2Pi(yaw); 59 | 60 | return yaw; 61 | } 62 | 63 | std::vector Ranges2Points(const std::vector& ranges, const std::vector& angles) 64 | { 65 | int n = ranges.size(); 66 | std::vector homoPoints(n); 67 | 68 | for(int i = 0; i < n; ++i) 69 | { 70 | float r = ranges[i]; 71 | float a = angles[i]; 72 | Eigen::Vector3f p = Eigen::Vector3f(r * cos(a), r * sin(a), 1); 73 | homoPoints[i] = p; 74 | } 75 | 76 | // consider using a matrix instead of a list for later stuff 77 | return homoPoints; 78 | } 79 | 80 | 81 | 82 | std::vector Downsample(const std::vector& ranges, int N) 83 | { 84 | int n = ranges.size() / N; 85 | std::vector downsampled(n); 86 | 87 | for(int i = 0; i < n; ++i) 88 | { 89 | downsampled[i] = ranges[i * N]; 90 | } 91 | 92 | return downsampled; 93 | } 94 | 95 | std::vector Downsample(const std::vector& ranges, int N) 96 | { 97 | int n = ranges.size() / N; 98 | std::vector downsampled(n); 99 | 100 | for(int i = 0; i < n; ++i) 101 | { 102 | downsampled[i] = ranges[i * N]; 103 | } 104 | 105 | return downsampled; 106 | } 107 | 108 | 109 | 110 | std::vector StringToVec(std::string seq) 111 | { 112 | std::vector vec; 113 | 114 | seq.erase(std::remove(seq.begin(), seq.end(), ','), seq.end()); 115 | seq.erase(std::remove(seq.begin(), seq.end(), '['), seq.end()); 116 | seq.erase(std::remove(seq.begin(), seq.end(), ']'), seq.end()); 117 | 118 | size_t pos = 0; 119 | std::string space_delimiter = " "; 120 | std::vector words; 121 | while ((pos = seq.find(space_delimiter)) != std::string::npos) 122 | { 123 | words.push_back(seq.substr(0, pos)); 124 | seq.erase(0, pos + space_delimiter.length()); 125 | } 126 | words.push_back(seq.substr(0, pos)); 127 | 128 | for(long unsigned int i = 0; i < words.size(); ++i) 129 | { 130 | //std::cout << words[i] << std::endl; 131 | vec.push_back(std::stof(words[i])); 132 | } 133 | 134 | return vec; 135 | } 136 | 137 | 138 | std::vector File2Lines(std::string filePath) 139 | { 140 | std::ifstream file(filePath); 141 | 142 | std::vector fields; 143 | 144 | if (file.is_open()) 145 | { 146 | std::string line; 147 | while (std::getline(file, line)) 148 | { 149 | fields.push_back(line); 150 | } 151 | file.close(); 152 | } 153 | 154 | return fields; 155 | } 156 | 157 | 158 | float SampleGuassian(float sigma) 159 | { 160 | float sample = 0; 161 | 162 | for(int i = 0; i < 12; ++i) 163 | { 164 | sample += drand48() * 2 * sigma - sigma; 165 | } 166 | sample *= 0.5; 167 | 168 | return sample; 169 | 170 | } 171 | 172 | std::vector computeDetectedParticles(const std::vector& particles, const Eigen::Vector3f& relTrans) 173 | { 174 | size_t numParticles = particles.size(); 175 | 176 | std::vector detectedParticles(numParticles); 177 | Eigen::Vector3f relTransXY(relTrans(0), relTrans(1), 1.0); 178 | 179 | #pragma omp parallel for 180 | for(long unsigned int i = 0; i < numParticles; ++i) 181 | { 182 | Eigen::Matrix3f particleFrame = Vec2Trans(particles[i].pose); 183 | Eigen::Vector3f top = particleFrame * relTransXY; 184 | //top(2) = relTrans(2) + particles[i].pose(2); 185 | top(2) = particles[i].pose(2); 186 | detectedParticles[i].pose = top; 187 | detectedParticles[i].weight = particles[i].weight; 188 | } 189 | 190 | return detectedParticles; 191 | } 192 | 193 | 194 | /* 195 | void ComputePolarVariance(std::vector& points, std::vector& weights) 196 | { 197 | int pointNum = points.size(); 198 | double dx = 0; 199 | double dy = 0; 200 | double w = 0; 201 | double r = 0; 202 | 203 | Eigen::Vector2f mu = Eigen::Vector2f(0, 0); 204 | Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero(); 205 | 206 | if (pointNum == 0) 207 | { 208 | mu = Eigen::Vector2f(NAN, NAN); 209 | return; 210 | } 211 | 212 | for (int i = 0; i < pointNum; ++i) 213 | { 214 | r += points[i].head(2).norm() * weights[i]; 215 | dx += points[i](0) * weights[i]; 216 | dy += points[i](1) * weights[i]; 217 | w += weights[i]; 218 | } 219 | 220 | mu(0) = r / w; 221 | mu(1) = Wrap2Pi(atan2(dy, dx)); 222 | 223 | if (pointNum > 1) 224 | { 225 | for (int i = 0; i < pointNum; ++i) 226 | { 227 | float diff += points[i](0) - mu(0); 228 | 229 | covariance(0,0) += diff * diff * weights[i]; 230 | } 231 | 232 | cov(0, 0) /= w; 233 | double R = sqrt(dx * dx + dy * dy); 234 | cov(1, 1) = 1.0 - R / w; 235 | // https://www.ebi.ac.uk/thornton-srv/software/PROCHECK/nmr_manual/man_cv.html 236 | } 237 | }*/ -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/src/dist_compress.py: -------------------------------------------------------------------------------- 1 | 2 | #import numpy as np 3 | #import pandas as pd 4 | from goodpoints import compress 5 | 6 | def compresspp_kt_test(X, g=3): 7 | 8 | kernel_id=0 9 | kernel_type = b"" 10 | if kernel_id == 0: 11 | kernel_type = b"gaussian" 12 | elif kernel_id == 1: 13 | kernel_type = b"sobolev" 14 | else: 15 | print("Invalid kernel_id") 16 | exit() 17 | 18 | #print(X.shape) 19 | 20 | #df = pd.DataFrame(X) 21 | #df.to_csv("pydump.csv", header=False, index=False) 22 | 23 | coreset = compress.compresspp_kt(X, b"gaussian", g=g) 24 | 25 | #X_sampled = X[coreset] 26 | #print(coreset) 27 | #print(X_sampled) 28 | return coreset 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/BenchmarkClustering.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: BenchmarkClustering.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "DistributionExchange.h" 21 | #include "TestUtils.h" 22 | 23 | 24 | void testGoodpoints(std::vector particlesA, std::vector particlesB) 25 | { 26 | GoodPointsCompression gp("/ros_ws/src/cmcl/cmcl_ros/ncore/src/", Eigen::Vector2f(0.15, 0.15)); 27 | 28 | int iter = 100; 29 | auto millis = 0; 30 | 31 | std::shared_ptr distData; 32 | 33 | for(int i = 0; i < iter; ++i) 34 | { 35 | auto start = std::chrono::system_clock::now(); 36 | distData = gp.Compress(particlesA, Eigen::Vector3f(0, 0, 0)); 37 | auto end = std::chrono::system_clock::now(); 38 | std::chrono::duration dur = end - start; 39 | millis += std::chrono::duration_cast(dur).count(); 40 | } 41 | std::cout << "Goodpoints Compress: " << (float)millis / (iter * 1000) << " ms" << std::endl; 42 | //std::cout << distData->Particles().size() << std::endl; 43 | 44 | millis = 0; 45 | 46 | for(int i = 0; i < iter; ++i) 47 | { 48 | auto start = std::chrono::system_clock::now(); 49 | gp.Fuse(particlesB, distData); 50 | auto end = std::chrono::system_clock::now(); 51 | std::chrono::duration dur = end - start; 52 | millis += std::chrono::duration_cast(dur).count(); 53 | } 54 | std::cout << "Goodpoints Fuse: " << (float)millis / (iter * 1000) << " ms" << std::endl; 55 | } 56 | 57 | 58 | 59 | void testKmeans(std::vector particlesA, std::vector particlesB) 60 | { 61 | KMeansClustering km(64, 5, Eigen::Vector2f(0, 0)); 62 | int iter = 100; 63 | auto millis = 0; 64 | std::shared_ptr distData; 65 | 66 | for(int i = 0; i < iter; ++i) 67 | { 68 | auto start = std::chrono::system_clock::now(); 69 | distData = km.Compress(particlesA, Eigen::Vector3f(0, 0, 0)); 70 | auto end = std::chrono::system_clock::now(); 71 | std::chrono::duration dur = end - start; 72 | millis += std::chrono::duration_cast(dur).count(); 73 | } 74 | std::cout << "Kmeans Compress: " << (float)millis / (iter * 1000) << " ms" << std::endl; 75 | 76 | millis = 0; 77 | for(int i = 0; i < iter; ++i) 78 | { 79 | auto start = std::chrono::system_clock::now(); 80 | km.Fuse(particlesB, distData); 81 | auto end = std::chrono::system_clock::now(); 82 | std::chrono::duration dur = end - start; 83 | millis += std::chrono::duration_cast(dur).count(); 84 | } 85 | std::cout << "Kmeans Fuse: " << (float)millis / (iter * 1000) << " ms" << std::endl; 86 | } 87 | 88 | void testProrok(std::vector particlesA, std::vector particlesB) 89 | { 90 | ProrokClustering pc(64, Eigen::Vector2f(0.15, 0.15)); 91 | int iter = 100; 92 | auto millis = 0; 93 | std::shared_ptr distData; 94 | Eigen::Vector3f trans(3.0, -2.0, 0.98); 95 | 96 | 97 | for(int i = 0; i < iter; ++i) 98 | { 99 | auto start = std::chrono::system_clock::now(); 100 | distData = pc.Compress(particlesA, trans); 101 | auto end = std::chrono::system_clock::now(); 102 | std::chrono::duration dur = end - start; 103 | millis += std::chrono::duration_cast(dur).count(); 104 | } 105 | std::cout << "Prorok Compress: " << (float)millis / (iter * 1000) << " ms" << std::endl; 106 | 107 | millis = 0; 108 | for(int i = 0; i < iter; ++i) 109 | { 110 | auto start = std::chrono::system_clock::now(); 111 | pc.Fuse(particlesB, distData); 112 | auto end = std::chrono::system_clock::now(); 113 | std::chrono::duration dur = end - start; 114 | millis += std::chrono::duration_cast(dur).count(); 115 | } 116 | std::cout << "Prorok Fuse: " << (float)millis / (iter * 1000) << " ms" << std::endl; 117 | 118 | } 119 | 120 | void testDET(std::vector particlesA, std::vector particlesB) 121 | { 122 | DensityEstimationTree det(150, 200); 123 | int iter = 100; 124 | auto millis = 0; 125 | std::shared_ptr distData; 126 | 127 | for(int i = 0; i < iter; ++i) 128 | { 129 | auto start = std::chrono::system_clock::now(); 130 | distData = det.Compress(particlesA, Eigen::Vector3f(0, 0, 0)); 131 | auto end = std::chrono::system_clock::now(); 132 | std::chrono::duration dur = end - start; 133 | millis += std::chrono::duration_cast(dur).count(); 134 | } 135 | std::cout << "DET Compress: " << (float)millis / (iter * 1000) << " ms" << std::endl; 136 | 137 | millis = 0; 138 | for(int i = 0; i < iter; ++i) 139 | { 140 | auto start = std::chrono::system_clock::now(); 141 | det.Fuse(particlesB, distData); 142 | auto end = std::chrono::system_clock::now(); 143 | std::chrono::duration dur = end - start; 144 | millis += std::chrono::duration_cast(dur).count(); 145 | } 146 | std::cout << "DET Fuse: " << (float)millis / (iter * 1000) << " ms" << std::endl; 147 | 148 | } 149 | 150 | void testNaive(std::vector particlesA, std::vector particlesB) 151 | { 152 | StandardThinning st(10000, Eigen::Vector2f(0.15, 0.15)); 153 | int iter = 100; 154 | auto millis = 0; 155 | std::shared_ptr distData; 156 | 157 | for(int i = 0; i < iter; ++i) 158 | { 159 | auto start = std::chrono::system_clock::now(); 160 | distData = st.Compress(particlesA, Eigen::Vector3f(0, 0, 0)); 161 | auto end = std::chrono::system_clock::now(); 162 | std::chrono::duration dur = end - start; 163 | millis += std::chrono::duration_cast(dur).count(); 164 | } 165 | std::cout << "Naive Compress: " << (float)millis / (iter * 1000) << " ms" << std::endl; 166 | 167 | millis = 0; 168 | for(int i = 0; i < iter; ++i) 169 | { 170 | auto start = std::chrono::system_clock::now(); 171 | st.Fuse(particlesB, distData); 172 | auto end = std::chrono::system_clock::now(); 173 | std::chrono::duration dur = end - start; 174 | millis += std::chrono::duration_cast(dur).count(); 175 | } 176 | std::cout << "Naive Fuse: " << (float)millis / (iter * 1000) << " ms" << std::endl; 177 | 178 | } 179 | 180 | void testThin(std::vector particlesA, std::vector particlesB) 181 | { 182 | StandardThinning st(64, Eigen::Vector2f(0.15, 0.15)); 183 | int iter = 100; 184 | auto millis = 0; 185 | std::shared_ptr distData; 186 | 187 | for(int i = 0; i < iter; ++i) 188 | { 189 | auto start = std::chrono::system_clock::now(); 190 | distData = st.Compress(particlesA, Eigen::Vector3f(0, 0, 0)); 191 | auto end = std::chrono::system_clock::now(); 192 | std::chrono::duration dur = end - start; 193 | millis += std::chrono::duration_cast(dur).count(); 194 | } 195 | std::cout << "Thin Compress: " << (float)millis / (iter * 1000) << " ms" << std::endl; 196 | 197 | millis = 0; 198 | for(int i = 0; i < iter; ++i) 199 | { 200 | auto start = std::chrono::system_clock::now(); 201 | st.Fuse(particlesB, distData); 202 | auto end = std::chrono::system_clock::now(); 203 | std::chrono::duration dur = end - start; 204 | millis += std::chrono::duration_cast(dur).count(); 205 | } 206 | std::cout << "Thin Fuse: " << (float)millis / (iter * 1000) << " ms" << std::endl; 207 | } 208 | 209 | 210 | int main() 211 | { 212 | int n = 10000; 213 | int k = 4; 214 | int s = 1; 215 | int d = 0; 216 | std::string csv_pathA = "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/" + std::to_string(n) + "/" + std::to_string(s) + "/particlesA_" + std::to_string(d) + ".csv"; 217 | std::vector particlesA = load_csv(csv_pathA); 218 | 219 | std::string csv_pathB = "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/" + std::to_string(n) + "/" + std::to_string(s) + "/particlesB_" + std::to_string(d) + ".csv"; 220 | std::vector particlesB = load_csv(csv_pathB); 221 | 222 | std::cout << particlesA.size() << ", " << particlesB.size() << std::endl; 223 | 224 | testNaive(particlesA, particlesB); 225 | testGoodpoints(particlesA, particlesB); 226 | testKmeans(particlesA, particlesB); 227 | testDET(particlesA, particlesB); 228 | testThin(particlesA, particlesB); 229 | testProrok(particlesA, particlesB); 230 | 231 | 232 | 233 | return 0; 234 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | add_executable(UnitTests UnitTests.cpp) 5 | target_link_libraries(UnitTests ${OpenCV_LIBS} ${catkin_LIBRARIES} NCORE nlohmann_json::nlohmann_json GTest::GTest gtest_main ${Boost_LIBRARIES} ${ARMADILLO_LIBRARIES} ${LAPACK_LIBRARIES} pybind11::embed) 6 | target_compile_definitions(UnitTests PRIVATE PROJECT_TEST_DATA_DIR="${PROJECT_SOURCE_DIR}/data") 7 | add_test(AllTestsInTests UnitTests) 8 | 9 | 10 | add_executable(BenchmarkClustering BenchmarkClustering.cpp TestUtils.cpp) 11 | target_link_libraries(BenchmarkClustering ${OpenCV_LIBS} ${catkin_LIBRARIES} NCORE nlohmann_json::nlohmann_json ${Boost_LIBRARIES} ${ARMADILLO_LIBRARIES} ${LAPACK_LIBRARIES} pybind11::embed) 12 | target_compile_definitions(BenchmarkClustering PRIVATE PROJECT_TEST_DATA_DIR="${PROJECT_SOURCE_DIR}/data") 13 | #add_test(AllTestsInTests UnitTests) 14 | 15 | 16 | add_executable(DumpClustering DumpClustering.cpp TestUtils.cpp) 17 | target_link_libraries(DumpClustering ${OpenCV_LIBS} ${catkin_LIBRARIES} NCORE nlohmann_json::nlohmann_json ${Boost_LIBRARIES} ${ARMADILLO_LIBRARIES} ${LAPACK_LIBRARIES} pybind11::embed) 18 | 19 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clustering.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clustering.pdf -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clustering4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clustering4.pdf -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/ClusteringNotSym.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/src/cmcl/cmcl_ros/ncore/tst/ClusteringNotSym.pdf -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clusteringfail.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clusteringfail.pdf -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clusteringsuccess.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/idsia-robotics/Collaborative-Monte-Carlo-Localization/918d41678b1eaca9ebbaf0ab47427f127ac0f1c3/ros_ws/src/cmcl/cmcl_ros/ncore/tst/Clusteringsuccess.pdf -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/DumpClustering.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: DumpClustering.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "DistributionExchange.h" 21 | #include "TestUtils.h" 22 | 23 | void dumpProrok(int k, std::vector& particles, const std::string& dumpPath) 24 | { 25 | Eigen::Vector3f trans(0.0, 0.0, 0.0); 26 | ProrokClustering pc(k, Eigen::Vector2f(0.15, 0.15)); 27 | 28 | std::shared_ptr distData = pc.Compress(particles, trans); 29 | 30 | std::vector points(k); 31 | 32 | for(int c = 0; c < k; ++c) 33 | { 34 | points[c] = distData->Prorok()[c].centroid; 35 | //std::cout << distData->Prorok()[c].centroid << std::endl; 36 | } 37 | 38 | dump_csv(points, dumpPath); 39 | } 40 | 41 | void dumpKMeans(int k, std::vector& particles, const std::string& dumpPath) 42 | { 43 | KMeansClustering km(k, 5, Eigen::Vector2f(0, 0)); 44 | 45 | std::shared_ptr distData = km.Compress(particles, Eigen::Vector3f(0, 0, 0)); 46 | 47 | std::vector points(k); 48 | 49 | for(int c = 0; c < k; ++c) 50 | { 51 | points[c] = distData->KMeans()[c].mean; 52 | //std::cout << distData->Prorok()[c].centroid << std::endl; 53 | } 54 | 55 | dump_csv(points, dumpPath); 56 | } 57 | 58 | void dumpGoodPoints(std::vector& particles, const std::string& dumpPath) 59 | { 60 | Eigen::Vector3f trans(0.0, 0.0, 0.0); 61 | GoodPointsCompression gp("/ros_ws/src/cmcl/cmcl_ros/ncore/src/", Eigen::Vector2f(0.15, 0.15)); 62 | 63 | std::shared_ptr distData = gp.Compress(particles, trans); 64 | 65 | dump_csv(distData->Particles(), dumpPath); 66 | } 67 | 68 | void dumpThin(int n, std::vector& particles, const std::string& dumpPath) 69 | { 70 | int m = particles.size(); 71 | std::vector subset(n); 72 | for(int j = 0; j < n; ++j) 73 | { 74 | subset[j] = particles[(int)(drand48() * m)]; 75 | } 76 | 77 | dump_csv(subset, dumpPath); 78 | } 79 | 80 | 81 | int main() 82 | { 83 | int n = 20; 84 | int k = 4; 85 | int s = 1; 86 | int d = 0; 87 | 88 | srand48(13); 89 | 90 | std::vector particles = createSymmetricXDist(n, k); 91 | std::string csv_path = "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/fail/particlesA_0.csv"; 92 | dump_csv(particles, csv_path); 93 | 94 | dumpProrok(8, particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/fail/prorok_dump.csv"); 95 | dumpKMeans(8, particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/fail/kmeans_dump.csv"); 96 | dumpGoodPoints(particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/fail/goodpoints_dump.csv"); 97 | dumpThin(8, particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/fail/thin_dump.csv"); 98 | 99 | particles = createSymmetricXDistNoCenter(n, k); 100 | csv_path = "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/success/particlesA_0.csv"; 101 | dump_csv(particles, csv_path); 102 | 103 | dumpProrok(8, particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/success/prorok_dump.csv"); 104 | dumpKMeans(8, particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/success/kmeans_dump.csv"); 105 | dumpGoodPoints(particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/success/goodpoints_dump.csv"); 106 | dumpThin(8, particles, "/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/success/thin_dump.csv"); 107 | 108 | return 0; 109 | } 110 | 111 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/TestUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: TestUtils.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "TestUtils.h" 13 | 14 | std::vector load_csv(std::string csv_path) 15 | { 16 | std::vector particles; 17 | 18 | std::ifstream file; 19 | file.open(csv_path); 20 | std::string line; 21 | while (std::getline(file, line)) 22 | { 23 | std::vector data; 24 | 25 | std::stringstream ss(line); 26 | while (ss.good()) 27 | { 28 | std::string substr; 29 | std::getline(ss, substr, ','); 30 | data.push_back(std::stod(substr)); 31 | } 32 | 33 | Particle p; 34 | p.pose = Eigen::Vector3f(data[0], data[1], data[2]); 35 | p.weight = data[3]; 36 | 37 | particles.push_back(p); 38 | } 39 | 40 | file.close(); 41 | 42 | return particles; 43 | } 44 | 45 | void dump_csv(std::vector& particles, std::string csv_path) 46 | { 47 | std::ofstream file; 48 | file.open(csv_path); 49 | 50 | for(int i = 0; i < particles.size(); ++i) 51 | { 52 | Eigen::Vector3f p = particles[i].pose; 53 | file << p(0) << "," << p(1) << "," << p(2) << "," << particles[i].weight << std::endl; 54 | } 55 | 56 | file.close(); 57 | } 58 | 59 | void dump_csv(std::vector& points, std::string csv_path) 60 | { 61 | std::ofstream file; 62 | file.open(csv_path); 63 | 64 | for(int i = 0; i < points.size(); ++i) 65 | { 66 | Eigen::Vector3f p = points[i]; 67 | file << p(0) << "," << p(1) << "," << p(2) << ",1.0" << std::endl; 68 | } 69 | 70 | file.close(); 71 | } 72 | 73 | void dump_csv(Eigen::MatrixXd& data, std::string csv_path) 74 | { 75 | std::ofstream file; 76 | file.open(csv_path); 77 | 78 | for(int i = 0; i < data.rows(); ++i) 79 | { 80 | file << data(i, 0) << "," << data(i, 1) << std::endl; 81 | } 82 | 83 | file.close(); 84 | 85 | } 86 | 87 | std::vector createSymmetricXDist(int n, int k) 88 | { 89 | std::vector particles(n * (k + 1)); 90 | std::vector centers(k + 1); 91 | centers[0] = Eigen::Vector3f(0, 0, 0); 92 | 93 | float ang = 2 * M_PI / k; 94 | if (k == 2) 95 | { 96 | ang = M_PI; 97 | } 98 | float r = 3.0; 99 | 100 | for(int c = 1; c < k + 1; ++c) 101 | { 102 | float t = ang * c; 103 | centers[c] = Eigen::Vector3f( r * cos(t), r * sin(t), 0.0); 104 | } 105 | 106 | for(int c = 0; c < k + 1; ++c) 107 | { 108 | for (int i = 0; i < n; ++i) 109 | { 110 | Eigen::Vector3f rand = Eigen::Vector3f::Ones() - 2.0 * Eigen::Vector3f(drand48(), drand48(), drand48()); 111 | particles[i + n * c].pose = centers[c] + 0.5 * rand; 112 | particles[i + n * c].weight = 1.0; 113 | } 114 | } 115 | 116 | return particles; 117 | } 118 | 119 | std::vector createSymmetricXDistNoCenter(int n, int k) 120 | { 121 | std::vector particles(n * k); 122 | std::vector centers(k); 123 | 124 | float ang = 2 * M_PI / k; 125 | if (k == 2) 126 | { 127 | ang = M_PI; 128 | } 129 | float r = 3.0; 130 | 131 | for(int c = 0; c < k; ++c) 132 | { 133 | float t = ang * c; 134 | centers[c] = Eigen::Vector3f( r * cos(t), r * sin(t), 0.0); 135 | } 136 | 137 | for(int c = 0; c < k; ++c) 138 | { 139 | for (int i = 0; i < n; ++i) 140 | { 141 | Eigen::Vector3f rand = Eigen::Vector3f::Ones() - 2.0 * Eigen::Vector3f(drand48(), drand48(), drand48()); 142 | particles[i + n * c].pose = centers[c] + 0.5 * rand; 143 | particles[i + n * c].weight = 1.0; 144 | } 145 | } 146 | 147 | return particles; 148 | } 149 | 150 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/TestUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: TestUtils.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef TESTUTILS_H 13 | #define TESTUTILS_H 14 | 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "DistributionExchange.h" 24 | 25 | 26 | std::vector load_csv(std::string csv_path); 27 | 28 | void dump_csv(std::vector& particles, std::string csv_path); 29 | 30 | void dump_csv(std::vector& points, std::string csv_path); 31 | 32 | void dump_csv(Eigen::MatrixXd& data, std::string csv_path); 33 | 34 | std::vector createSymmetricXDist(int n, int k); 35 | 36 | std::vector createSymmetricXDistNoCenter(int n, int k); 37 | 38 | 39 | #endif -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/ncore/tst/particle_test.py: -------------------------------------------------------------------------------- 1 | from goodpoints import kt 2 | #from util_k_mmd import kernel_eval 3 | import numpy as np 4 | # for partial functions, to use kernel_eval for kernel 5 | #from functools import partial 6 | from goodpoints.tictoc import tic, toc 7 | import pandas as pd 8 | import matplotlib.pyplot as plt 9 | 10 | 11 | from goodpoints import compress 12 | 13 | def compresspp_kt_test(X, kernel_id=0, g=0): 14 | 15 | kernel_type = b"" 16 | if kernel_id == 0: 17 | kernel_type = b"gaussian" 18 | elif kernel_id == 1: 19 | kernel_type = b"sobolev" 20 | else: 21 | print("Invalid kernel_id") 22 | exit() 23 | 24 | coreset = compress.compresspp_kt(X, kernel_type, g=g) 25 | 26 | X_sampled = X[coreset] 27 | print(coreset) 28 | print(coreset.shape) 29 | return X_sampled 30 | 31 | 32 | # def kernel_thinning_test(X, m=8): 33 | 34 | # d = int(2) 35 | # var = 1. 36 | 37 | # params_k_swap = {"name": "gauss", "var": var, "d": int(d)} 38 | # params_k_split = {"name": "gauss_rt", "var": var/2., "d": int(d)} 39 | 40 | # split_kernel = partial(kernel_eval, params_k=params_k_split) 41 | # swap_kernel = partial(kernel_eval, params_k=params_k_swap) 42 | # coreset = kt.thin(X, m, split_kernel, swap_kernel, delta=0.5, seed=None, store_K=False, 43 | # meanK=None, unique=False, verbose=False) 44 | 45 | # X_sampled = X[coreset] 46 | # print(coreset) 47 | # print(coreset.shape) 48 | # return X_sampled 49 | 50 | 51 | def plot(X, X_sampled): 52 | 53 | plt.scatter(X[:, 0], X[:, 1], c='g', marker='o', alpha=0.2) 54 | plt.scatter(X_sampled[:, 0], X_sampled[:, 1], c='r', marker='o', alpha=1.0) 55 | 56 | plt.show() 57 | 58 | 59 | 60 | def main(): 61 | 62 | df = pd.read_csv("/home/nickybones/Code/docker_ros2/ros_ws/src/cmcl/cmcl_ros/ncore/tst/data/10000/sym4/particlesA_0.csv", index_col=False, header=None) 63 | data = df.to_numpy() 64 | X = data[:,[0, 1]] 65 | verbose = True 66 | 67 | #tic() 68 | #X_sampled = kernel_thinning_test(X) 69 | #toc(print_elapsed=verbose) 70 | 71 | tic() 72 | X_sampled =compresspp_kt_test(X, kernel_id=0) 73 | #X_sampled =compresspp_kt_test(X_sampled) 74 | toc(print_elapsed=verbose) 75 | print(X_sampled) 76 | 77 | plot(X, X_sampled) 78 | 79 | if __name__ == '__main__': 80 | main() 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cmcl_ros 5 | 0.0.0 6 | TODO: Package description 7 | nickybones 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | std_msgs 13 | sensor_msgs 14 | nav_msgs 15 | geometry_msgs 16 | message_filters 17 | tf2 18 | pcl_conversions 19 | cmcl_msgs 20 | robomaster_msgs 21 | 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/src/DetectionNoiseNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: DetectionNoiseNode.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "tf2/LinearMath/Quaternion.h" 20 | #include "cmcl_msgs/msg/robot_detection.hpp" 21 | #include 22 | #include "Utils.h" 23 | 24 | 25 | class DetectionNoiseNode : public rclcpp::Node 26 | { 27 | public: 28 | DetectionNoiseNode() : Node("DetectionNoiseNode") 29 | { 30 | std::string detectionTopic; 31 | this->declare_parameter("detectionTopic", ""); 32 | this->get_parameter("detectionTopic", detectionTopic); 33 | RCLCPP_INFO(this->get_logger(), "detectionTopic %s", detectionTopic.c_str()); 34 | 35 | std::string noisyDetectionTopic; 36 | this->declare_parameter("noisyDetectionTopic", ""); 37 | this->get_parameter("noisyDetectionTopic", noisyDetectionTopic); 38 | RCLCPP_INFO(this->get_logger(), "noisyDetectionTopic %s", noisyDetectionTopic.c_str()); 39 | 40 | this->declare_parameter("detectionNoise", std::vector(2)); 41 | rclcpp::Parameter dblArrParam =this->get_parameter("detectionNoise"); 42 | std::vector detectionNoise = dblArrParam.as_double_array(); 43 | RCLCPP_INFO(this->get_logger(), "detectionNoise %f %f", detectionNoise[0], detectionNoise[1]); 44 | o_detectionNoise = Eigen::Vector2f(detectionNoise[0], detectionNoise[1]); 45 | 46 | rclcpp::QoS qos(10); 47 | qos.best_effort(); 48 | 49 | 50 | o_detectionSub = create_subscription(detectionTopic, qos, std::bind(&DetectionNoiseNode::detectionCallback, this, std::placeholders::_1)); 51 | o_detectPub = this->create_publisher(noisyDetectionTopic, 10); 52 | 53 | } 54 | 55 | void detectionCallback(const cmcl_msgs::msg::RobotDetection::SharedPtr det_msg) 56 | { 57 | cmcl_msgs::msg::RobotDetection noisy_msg; 58 | noisy_msg.header = det_msg->header; 59 | noisy_msg.detected_id = det_msg->detected_id; 60 | noisy_msg.observer_id = det_msg->observer_id; 61 | noisy_msg.pose = det_msg->pose; 62 | noisy_msg.type = det_msg->type; 63 | 64 | float x = det_msg->pose.position.x; 65 | float y = det_msg->pose.position.y; 66 | 67 | float r = sqrt(x * x + y * y); 68 | float theta = atan2(y, x); 69 | 70 | float dr = abs(r * SampleGuassian(o_detectionNoise(0))); 71 | float dt = Wrap2Pi(SampleGuassian(o_detectionNoise(1))); 72 | 73 | float x_n = (r + dr) * cos(theta + dt); 74 | float y_n = (r + dr) * sin(theta + dt); 75 | 76 | noisy_msg.pose.position.x = x_n; 77 | noisy_msg.pose.position.y = y_n; 78 | 79 | //RCLCPP_INFO(this->get_logger(), "orig %f %f, noisy %f %f", x, y, x_n, y_n); 80 | 81 | o_detectPub->publish(noisy_msg); 82 | } 83 | 84 | 85 | private: 86 | 87 | rclcpp::Subscription::SharedPtr o_detectionSub; 88 | rclcpp::Publisher::SharedPtr o_detectPub; 89 | Eigen::Vector2f o_detectionNoise = Eigen::Vector2f(0.15, 0.15); 90 | 91 | 92 | 93 | 94 | }; 95 | 96 | 97 | 98 | int main(int argc, char * argv[]) 99 | { 100 | rclcpp::init(argc, argv); 101 | rclcpp::spin(std::make_shared()); 102 | rclcpp::shutdown(); 103 | return 0; 104 | } 105 | 106 | -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/src/EvalNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: EvalNode.cpp.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "RosUtils.h" 23 | #include "Utils.h" 24 | #include "cmcl_msgs/msg/prediction.hpp" 25 | 26 | 27 | #include 28 | #include 29 | 30 | 31 | //typedef message_filters::sync_policies::ApproximateTime PoseSyncPolicy; 32 | 33 | typedef message_filters::sync_policies::ApproximateTime PoseSyncPolicy; 34 | 35 | 36 | class EvalNode : public rclcpp::Node 37 | { 38 | public: 39 | EvalNode() 40 | : Node("EvalNode") 41 | { 42 | std::string gtTopic; 43 | std::string poseTopic; 44 | std::string csvPath; 45 | std::string predTopic; 46 | 47 | 48 | this->declare_parameter("gtTopic", ""); 49 | this->declare_parameter("poseTopic", ""); 50 | this->declare_parameter("predTopic", ""); 51 | this->declare_parameter("csvPath", ""); 52 | 53 | this->get_parameter("gtTopic", gtTopic); 54 | RCLCPP_INFO(this->get_logger(), "gtTopic %s", gtTopic.c_str()); 55 | this->get_parameter("poseTopic", poseTopic); 56 | RCLCPP_INFO(this->get_logger(), "poseTopic %s", poseTopic.c_str()); 57 | this->get_parameter("predTopic", predTopic); 58 | RCLCPP_INFO(this->get_logger(), "predTopic %s", predTopic.c_str()); 59 | this->get_parameter("csvPath", csvPath); 60 | RCLCPP_INFO(this->get_logger(), "csvPath %s", csvPath.c_str()); 61 | 62 | 63 | o_csv.open(csvPath); 64 | 65 | //o_csv << "t,gt_x,gt_y,gt_yaw,pose_x,pose_y,pose_yaw" << std::endl; 66 | o_csv << "t,gt_x,gt_y,gt_yaw,pose_x,pose_y,pose_yaw,source" << std::endl; 67 | 68 | 69 | gtSub = std::make_shared>(this, gtTopic); 70 | //poseSub = std::make_shared>(this, poseTopic); 71 | predSub = std::make_shared>(this, predTopic); 72 | 73 | o_poseSync = std::make_shared>(PoseSyncPolicy(10), *predSub, *gtSub); 74 | o_poseSync->registerCallback(std::bind(&EvalNode::callback, this, std::placeholders::_1, std::placeholders::_2)); 75 | 76 | 77 | RCLCPP_INFO(this->get_logger(), "Ready!"); 78 | 79 | 80 | } 81 | 82 | /* void callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& poseMsg, const geometry_msgs::msg::PoseStamped::ConstSharedPtr& gtMsg) 83 | { 84 | 85 | float yaw = Wrap2Pi(2 * atan2(poseMsg->pose.pose.orientation.z, poseMsg->pose.pose.orientation.w)); 86 | Eigen::Vector3f pose = Eigen::Vector3f(poseMsg->pose.pose.position.x, poseMsg->pose.pose.position.y, yaw); 87 | 88 | yaw = Wrap2Pi(2 * atan2(gtMsg->pose.orientation.z, gtMsg->pose.orientation.w)); 89 | Eigen::Vector3f gt = Eigen::Vector3f(gtMsg->pose.position.x, gtMsg->pose.position.y, yaw); 90 | 91 | //double stamp = double(poseMsg->header.stamp.sec) + double(poseMsg->header.stamp.nanosec) * 1e-9; 92 | 93 | o_csv << std::to_string(long(poseMsg->header.stamp.sec)) <<"." << std::to_string(long(poseMsg->header.stamp.nanosec)) << "," << gt(0) << "," 94 | << gt(1) << "," << gt(2) << "," << pose(0) << "," << pose(1) << "," << pose(2) << std::endl; 95 | 96 | //RCLCPP_INFO(this->get_logger(), "Logging!"); 97 | 98 | }*/ 99 | 100 | void callback(const cmcl_msgs::msg::Prediction::ConstSharedPtr& predMsg, const geometry_msgs::msg::PoseStamped::ConstSharedPtr& gtMsg) 101 | { 102 | geometry_msgs::msg::PoseWithCovarianceStamped poseMsg = predMsg->pose; 103 | float yaw = Wrap2Pi(2 * atan2(poseMsg.pose.pose.orientation.z, poseMsg.pose.pose.orientation.w)); 104 | Eigen::Vector3f pose = Eigen::Vector3f(poseMsg.pose.pose.position.x, poseMsg.pose.pose.position.y, yaw); 105 | 106 | yaw = Wrap2Pi(2 * atan2(gtMsg->pose.orientation.z, gtMsg->pose.orientation.w)); 107 | Eigen::Vector3f gt = Eigen::Vector3f(gtMsg->pose.position.x, gtMsg->pose.position.y, yaw); 108 | 109 | //double stamp = double(poseMsg->header.stamp.sec) + double(poseMsg->header.stamp.nanosec) * 1e-9; 110 | 111 | o_csv << std::to_string(long(poseMsg.header.stamp.sec)) <<"." << std::to_string(long(poseMsg.header.stamp.nanosec)) << "," << gt(0) << "," 112 | << gt(1) << "," << gt(2) << "," << pose(0) << "," << pose(1) << "," << pose(2) << "," << std::to_string(predMsg->type) << std::endl; 113 | 114 | //RCLCPP_INFO(this->get_logger(), "Logging %d", predMsg->type); 115 | 116 | } 117 | 118 | private: 119 | 120 | std::shared_ptr> gtSub; 121 | //std::shared_ptr> poseSub; 122 | std::shared_ptr> predSub; 123 | 124 | std::shared_ptr> o_poseSync; 125 | std::ofstream o_csv; 126 | 127 | }; 128 | 129 | int main(int argc, char * argv[]) 130 | { 131 | rclcpp::init(argc, argv); 132 | rclcpp::spin(std::make_shared()); 133 | rclcpp::shutdown(); 134 | return 0; 135 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/src/LidarScan2PointCloudNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: LidarScan2PointCloudNode.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "std_msgs/msg/string.hpp" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "tf2_ros/buffer.h" 24 | 25 | 26 | #include "Utils.h" 27 | #include "RosUtils.h" 28 | 29 | #include 30 | #include 31 | 32 | 33 | 34 | class LidarScan2PointCloudNode : public rclcpp::Node 35 | { 36 | public: 37 | LidarScan2PointCloudNode() 38 | : Node("LidarScan2PointCloudNode") 39 | { 40 | std::string lidarScanTopic; 41 | std::string lidarTF; 42 | 43 | this->declare_parameter("lidarScanTopic" ,""); 44 | this->declare_parameter("baseLinkTF", ""); 45 | this->declare_parameter("pointCloudTopic", ""); 46 | this->declare_parameter("lidarTF", ""); 47 | 48 | this->get_parameter("lidarScanTopic", lidarScanTopic); 49 | RCLCPP_INFO(this->get_logger(), "lidarScanTopic %s", lidarScanTopic.c_str()); 50 | this->get_parameter("pointCloudTopic", o_pointCloudTopic); 51 | RCLCPP_INFO(this->get_logger(), "pointCloudTopic %s", o_pointCloudTopic.c_str()); 52 | this->get_parameter("baseLinkTF", o_baseLinkTF); 53 | RCLCPP_INFO(this->get_logger(), "baseLinkTF %s", o_baseLinkTF.c_str()); 54 | this->get_parameter("lidarTF", lidarTF); 55 | RCLCPP_INFO(this->get_logger(), "lidarTF %s", lidarTF.c_str()); 56 | 57 | 58 | 59 | o_pointCloudPub = this->create_publisher(o_pointCloudTopic, 1); 60 | tf_scan = std::make_unique(*this); 61 | 62 | std::unique_ptr tf_buffer = std::make_unique(this->get_clock()); 63 | std::shared_ptr tf_listener = std::make_shared(*tf_buffer); 64 | geometry_msgs::msg::TransformStamped t; 65 | 66 | bool getTF = true; 67 | Eigen::Vector3f origin; 68 | while(getTF) 69 | { 70 | try 71 | { 72 | //t = tf_buffer->lookupTransform(lidarTF.c_str(), o_baseLinkTF.c_str(), tf2::TimePointZero, tf2::Duration(1000000000)); 73 | t = tf_buffer->lookupTransform(o_baseLinkTF.c_str(), lidarTF.c_str(), tf2::TimePointZero, tf2::Duration(1000000000)); 74 | RCLCPP_INFO( 75 | this->get_logger(), "Transform %s to %s: %f %f %f", o_baseLinkTF.c_str(), lidarTF.c_str(), 76 | t.transform.translation.x, t.transform.translation.y, GetYaw(t.transform.rotation.z, t.transform.rotation.w)); 77 | 78 | origin = Eigen::Vector3f(t.transform.translation.x, t.transform.translation.y, GetYaw(t.transform.rotation.z, t.transform.rotation.w)); 79 | getTF = false; 80 | } 81 | catch (const tf2::TransformException & ex) { 82 | RCLCPP_INFO(this->get_logger(), "Could not transform %s to %s: %s", 83 | o_baseLinkTF.c_str(), lidarTF.c_str(), ex.what()); 84 | } 85 | } 86 | 87 | //origin = Eigen::Vector3f(0, 0,0); 88 | 89 | o_trans = Vec2Trans(origin); 90 | //o_trans = o_trans.inverse(); 91 | 92 | 93 | rclcpp::QoS qos(10); 94 | qos.best_effort(); 95 | //qos.durability_volatile(); 96 | 97 | o_lidrScanSub = create_subscription(lidarScanTopic, qos, std::bind(&LidarScan2PointCloudNode::callback, this, std::placeholders::_1)); 98 | 99 | RCLCPP_INFO(this->get_logger(), "LidarScan2PointCloudNode running!"); 100 | 101 | 102 | } 103 | 104 | void callback(const sensor_msgs::msg::LaserScan::ConstSharedPtr& laserMsg) 105 | { 106 | std::vector ranges = laserMsg->ranges; 107 | float angleMin = laserMsg->angle_min; 108 | float angleMax = laserMsg->angle_max; 109 | float angIncr = laserMsg->angle_increment; 110 | 111 | int numBeams = fabs(angleMax - angleMin) / angIncr; 112 | std::vector heading(numBeams); 113 | 114 | for(int i = 0; i < numBeams; i++) 115 | { 116 | heading[i] = angleMin + i * angIncr; 117 | } 118 | 119 | // here add masking code 120 | std::vector points_3d = Ranges2Points(ranges, heading); 121 | 122 | int n = points_3d.size(); 123 | 124 | for(int i = 0; i < n; i++) 125 | { 126 | Eigen::Vector3f p = points_3d[i]; 127 | Eigen::Vector3f p_trans = o_trans * p; 128 | points_3d[i] = p_trans; 129 | } 130 | 131 | 132 | //std::vector points_3d = l2d->Center(ranges); 133 | 134 | 135 | // geometry_msgs::msg::TransformStamped t; 136 | // t.header.stamp = rclcpp::Time(); 137 | // t.header.frame_id = o_baseLinkTF.c_str(); 138 | // t.child_frame_id = "scan_merged"; 139 | // t.transform.translation.x = 0.0; 140 | // t.transform.translation.y = 0.0; 141 | // t.transform.translation.z = 0.0; 142 | // tf2::Quaternion q; 143 | // q.setRPY(0, 0, 0); 144 | // t.transform.rotation.x = q.x(); 145 | // t.transform.rotation.y = q.y(); 146 | // t.transform.rotation.z = q.z(); 147 | // t.transform.rotation.w = q.w(); 148 | // tf_scan->sendTransform(t); 149 | 150 | pcl::PointCloud pcl = Vec2PointCloud(points_3d); 151 | sensor_msgs::msg::PointCloud2 pcl_msg; 152 | pcl::toROSMsg(pcl, pcl_msg); 153 | pcl_msg.header.stamp = laserMsg->header.stamp; 154 | pcl_msg.header.frame_id = o_pointCloudTopic; 155 | o_pointCloudPub->publish(pcl_msg); 156 | 157 | 158 | tf2::Transform tf_orig; 159 | tf_orig.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); 160 | tf2::Quaternion q; 161 | q.setRPY(0.0, 0.0, 0.0); 162 | tf_orig.setRotation(q); 163 | tf2::Transform tf_inv = tf_orig.inverse(); 164 | geometry_msgs::msg::TransformStamped t; 165 | t.header.stamp = laserMsg->header.stamp; 166 | t.header.frame_id = o_baseLinkTF.c_str(); 167 | t.child_frame_id = o_pointCloudTopic; 168 | t.transform.translation.x = tf_inv.getOrigin().x(); 169 | t.transform.translation.y = tf_inv.getOrigin().y(); 170 | t.transform.translation.z = tf_inv.getOrigin().z(); 171 | t.transform.rotation.x = tf_inv.getRotation().x(); 172 | t.transform.rotation.y = tf_inv.getRotation().y(); 173 | t.transform.rotation.z = tf_inv.getRotation().z(); 174 | t.transform.rotation.w = tf_inv.getRotation().w(); 175 | tf_scan->sendTransform(t); 176 | 177 | } 178 | 179 | private: 180 | 181 | std::string o_baseLinkTF; 182 | rclcpp::Publisher::SharedPtr o_pointCloudPub; 183 | rclcpp::Subscription::SharedPtr o_lidrScanSub; 184 | std::unique_ptr tf_scan; 185 | Eigen::Matrix3f o_trans; 186 | std::string o_pointCloudTopic; 187 | 188 | 189 | 190 | 191 | 192 | }; 193 | 194 | int main(int argc, char * argv[]) 195 | { 196 | rclcpp::init(argc, argv); 197 | rclcpp::spin(std::make_shared()); 198 | rclcpp::shutdown(); 199 | return 0; 200 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/src/MapLoaderNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: MapLoaderNode.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "rclcpp/wait_for_message.hpp" 19 | #include 20 | #include "nav2_map_server/map_server.hpp" 21 | #include "nav2_map_server/map_io.hpp" 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | using namespace nav2_map_server; 28 | class MapLoaderNode : public rclcpp::Node 29 | { 30 | public: 31 | MapLoaderNode() : Node("MapLoaderNode") 32 | { 33 | 34 | std::string cmclConfigPath; 35 | std::string mapTopic; 36 | this->declare_parameter("mapTopic", ""); 37 | this->declare_parameter("cmclConfigPath", ""); 38 | this->get_parameter("mapTopic", mapTopic); 39 | RCLCPP_INFO(this->get_logger(), "mapTopic %s", mapTopic.c_str()); 40 | this->get_parameter("cmclConfigPath", cmclConfigPath); 41 | RCLCPP_INFO(this->get_logger(), "cmclConfigPath %s", cmclConfigPath.c_str()); 42 | 43 | //std::string cmclConfigPath = "/ros_ws/data/cmcl.config"; 44 | using json = nlohmann::json; 45 | std::ifstream file(cmclConfigPath); 46 | json config; 47 | try 48 | { 49 | file >> config; 50 | } 51 | catch(std::exception& e) 52 | { 53 | std::cerr << "Failed to load config fail" << std::endl; 54 | std::cout << e.what() << '\n'; 55 | } 56 | 57 | std::string folderPath = boost::filesystem::path(cmclConfigPath).parent_path().string() + "/"; 58 | std::string mapConfigPath = folderPath + std::string(config["mapPath"]); 59 | 60 | nav_msgs::msg::OccupancyGrid mapMsg; 61 | LoadParameters params = loadMapYaml(mapConfigPath); 62 | //params.image_file_name = params.image_file_name; 63 | loadMapFromFile(params, mapMsg); 64 | o_mapPub = this->create_publisher(mapTopic, 1); 65 | 66 | 67 | int cnt = 0; 68 | while(cnt < 100000000) 69 | { 70 | o_mapPub->publish(mapMsg); 71 | ++cnt; 72 | } 73 | 74 | RCLCPP_INFO(this->get_logger(), "MapLoaderNode running!"); 75 | 76 | 77 | } 78 | 79 | private: 80 | 81 | rclcpp::Publisher::SharedPtr o_mapPub; 82 | }; 83 | 84 | 85 | 86 | int main(int argc, char * argv[]) 87 | { 88 | rclcpp::init(argc, argv); 89 | rclcpp::spin(std::make_shared()); 90 | rclcpp::shutdown(); 91 | return 0; 92 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/src/OprtitrackPublisherNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: OprtitrackPublisherNode.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "std_msgs/msg/string.hpp" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "tf2_ros/buffer.h" 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include "rclcpp/wait_for_message.hpp" 30 | #include 31 | 32 | #include "Utils.h" 33 | #include "RosUtils.h" 34 | 35 | 36 | class OprtitrackPublisherNode : public rclcpp::Node 37 | { 38 | public: 39 | OprtitrackPublisherNode() 40 | : Node("OprtitrackPublisherNode") 41 | { 42 | std::string robotName; 43 | 44 | 45 | 46 | this->declare_parameter("robotName" ,""); 47 | this->get_parameter("robotName", robotName); 48 | RCLCPP_INFO(this->get_logger(), "robotName %s", robotName.c_str()); 49 | rclcpp::QoS qos(10); 50 | qos.best_effort(); 51 | o_gtPub = this->create_publisher(robotName + "/GT", 10); 52 | o_optitrackSub = create_subscription("/optitrack" + robotName, qos, std::bind(&OprtitrackPublisherNode::callback, this, std::placeholders::_1)); 53 | 54 | 55 | RCLCPP_INFO(this->get_logger(), "OprtitrackPublisherNode running!"); 56 | } 57 | 58 | void callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& optitrack_msg) 59 | { 60 | geometry_msgs::msg::PoseStamped gt_msg; 61 | 62 | gt_msg.pose = optitrack_msg->pose; 63 | gt_msg.header = optitrack_msg->header; 64 | gt_msg.header.frame_id = "map"; 65 | 66 | o_gtPub->publish(gt_msg); 67 | } 68 | private: 69 | 70 | rclcpp::Subscription::SharedPtr o_optitrackSub; 71 | rclcpp::Publisher::SharedPtr o_gtPub; 72 | 73 | 74 | }; 75 | 76 | int main(int argc, char * argv[]) 77 | { 78 | rclcpp::init(argc, argv); 79 | rclcpp::spin(std::make_shared()); 80 | rclcpp::shutdown(); 81 | return 0; 82 | } -------------------------------------------------------------------------------- /ros_ws/src/cmcl/cmcl_ros/src/RelativePosedNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2023- University of Lugano # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: RelativePosedNode.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "std_msgs/msg/string.hpp" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "tf2_ros/buffer.h" 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include "rclcpp/wait_for_message.hpp" 30 | #include 31 | 32 | #include "Utils.h" 33 | #include "RosUtils.h" 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include "cmcl_msgs/msg/robot_detection.hpp" 41 | 42 | 43 | typedef message_filters::sync_policies::ApproximateTime PoseSyncPolicy; 44 | 45 | 46 | class RelativePosedNode : public rclcpp::Node 47 | { 48 | public: 49 | RelativePosedNode() 50 | : Node("RelativePosedNode") 51 | { 52 | std::string visionTopic; 53 | std::string robotName1; 54 | std::string robotName2; 55 | std::string detectedRobot; 56 | 57 | this->declare_parameter("visionTopic" ,""); 58 | this->declare_parameter("robotName1" ,""); 59 | this->declare_parameter("robotName2", ""); 60 | this->declare_parameter("detectedRobot", ""); 61 | this->declare_parameter("robotID", 0); 62 | 63 | 64 | this->get_parameter("visionTopic", visionTopic); 65 | RCLCPP_INFO(this->get_logger(), "visionTopic %s", visionTopic.c_str()); 66 | this->get_parameter("robotName1", robotName1); 67 | RCLCPP_INFO(this->get_logger(), "robotName1 %s", robotName1.c_str()); 68 | this->get_parameter("robotName2", robotName2); 69 | RCLCPP_INFO(this->get_logger(), "robotName2 %s", robotName2.c_str()); 70 | this->get_parameter("detectedRobot", detectedRobot); 71 | RCLCPP_INFO(this->get_logger(), "detectedRobot %s", detectedRobot.c_str()); 72 | this->get_parameter("robotID", o_robotID); 73 | RCLCPP_INFO(this->get_logger(), "robotID %d", o_robotID); 74 | 75 | o_detectionPub = this->create_publisher(detectedRobot, 10); 76 | r1Sub = std::make_shared>(this, "/optitrack" + robotName1); 77 | r2Sub = std::make_shared>(this, "/optitrack" + robotName2); 78 | detectionSub = std::make_shared>(this, visionTopic); 79 | o_depthSync = std::make_shared>(PoseSyncPolicy(1), *r1Sub, *r2Sub, *detectionSub); 80 | o_depthSync->registerCallback(std::bind(&RelativePosedNode::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); 81 | 82 | RCLCPP_INFO(this->get_logger(), "RelativePosedNode running!"); 83 | } 84 | 85 | void callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& r1_msg, 86 | const geometry_msgs::msg::PoseStamped::ConstSharedPtr& r2_msg, 87 | const robomaster_msgs::msg::Detection::ConstSharedPtr& detection_msg) 88 | { 89 | 90 | if (detection_msg->robots.size()) 91 | { 92 | Eigen::Vector3f pose1 = Eigen::Vector3f(r1_msg->pose.position.x, r1_msg->pose.position.y, GetYaw(r1_msg->pose.orientation.z, r1_msg->pose.orientation.w)); 93 | Eigen::Vector3f pose2 = Eigen::Vector3f(r2_msg->pose.position.x, r2_msg->pose.position.y, GetYaw(r2_msg->pose.orientation.z, r2_msg->pose.orientation.w)); 94 | 95 | Eigen::Matrix3f trans = Vec2Trans(pose1); 96 | Eigen::Matrix3f invTrans = trans.inverse(); 97 | 98 | Eigen::Vector3f relPose = invTrans * Eigen::Vector3f(pose2(0), pose2(1), 1.0); 99 | relPose(2) = Wrap2Pi(pose2(2) - pose1(2)); 100 | 101 | if(abs(atan2(relPose(1), relPose(0))) > 0.9) return; 102 | 103 | cmcl_msgs::msg::RobotDetection msg; 104 | msg.header.stamp = detection_msg->header.stamp; 105 | msg.observer_id = o_robotID; 106 | msg.detected_id = 1 - o_robotID; 107 | msg.type = 1; 108 | msg.pose.position.x = relPose(0); 109 | msg.pose.position.y = relPose(1); 110 | msg.pose.position.z = 0.0; 111 | o_detectionPub->publish(msg); 112 | 113 | //RCLCPP_INFO(this->get_logger(), "Detection!"); 114 | } 115 | } 116 | 117 | 118 | private: 119 | 120 | int o_robotID = 0; 121 | rclcpp::Publisher::SharedPtr o_detectionPub; 122 | std::shared_ptr> r1Sub; 123 | std::shared_ptr> r2Sub; 124 | std::shared_ptr> detectionSub; 125 | std::shared_ptr> o_depthSync; 126 | 127 | 128 | }; 129 | 130 | 131 | int main(int argc, char * argv[]) 132 | { 133 | rclcpp::init(argc, argv); 134 | rclcpp::spin(std::make_shared()); 135 | rclcpp::shutdown(); 136 | return 0; 137 | } --------------------------------------------------------------------------------