├── src ├── CMakeLists.txt └── als_ros │ ├── README.md │ ├── launch │ ├── map_server.launch │ ├── scan2pc.launch │ ├── robot_tf.launch │ ├── mae_classifier_learning.launch │ ├── classifier_dataset_generator.launch │ ├── slamer.launch │ ├── mrf_failure_detector.launch │ ├── gl_pose_sampler.launch │ └── mcl.launch │ ├── classifiers │ └── MAE │ │ ├── plot_mae_decision_likelihoods_negative.gpt │ │ ├── plot_mae_decision_likelihoods_positive.gpt │ │ ├── classifier.yaml │ │ ├── plot_histograms.gpt │ │ ├── false_negative_maes.txt │ │ └── false_positive_maes.txt │ ├── src │ ├── gl_pose_sampler.cpp │ ├── classifier_dataset_generator.cpp │ ├── mrf_failure_detector.cpp │ ├── mcl.cpp │ ├── sm.cpp │ ├── slamer.cpp │ ├── scan2pc.cpp │ ├── mae_classifier_learning.cpp │ └── evaluator.cpp │ ├── package.xml │ ├── include │ └── als_ros │ │ ├── Point.h │ │ ├── Particle.h │ │ ├── Pose.h │ │ ├── Histogram.h │ │ ├── ISM.h │ │ ├── MAEClassifier.h │ │ ├── ClassifierDatasetGenerator.h │ │ └── MRFFailureDetector.h │ └── CMakeLists.txt ├── .catkin_workspace ├── .gitignore ├── README.md ├── LICENSE └── rviz ├── als_ros.rviz └── slamer.rviz /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /src/als_ros/README.md: -------------------------------------------------------------------------------- 1 | This directory is the als_ros package. 2 | 3 | If you don't want to make a new ROS workspace, please copy this directory to your workspace. -------------------------------------------------------------------------------- /src/als_ros/launch/map_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/als_ros/launch/scan2pc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Build space 2 | build 3 | build/* 4 | devel 5 | devel/* 6 | make.sh 7 | clean.sh 8 | 9 | # Prerequisites 10 | *.d 11 | 12 | # Compiled Object files 13 | *.slo 14 | *.lo 15 | *.o 16 | *.obj 17 | 18 | # Precompiled Headers 19 | *.gch 20 | *.pch 21 | 22 | # Compiled Dynamic libraries 23 | *.so 24 | *.dylib 25 | *.dll 26 | 27 | # Fortran module files 28 | *.mod 29 | *.smod 30 | 31 | # Compiled Static libraries 32 | *.lai 33 | *.la 34 | *.a 35 | *.lib 36 | 37 | # Executables 38 | *.exe 39 | *.out 40 | *.app 41 | -------------------------------------------------------------------------------- /src/als_ros/classifiers/MAE/plot_mae_decision_likelihoods_negative.gpt: -------------------------------------------------------------------------------- 1 | #! /bin/gnuplot 2 | 3 | set colors classic 4 | set size ratio 0.6 1 5 | set xlabel "Reliability" 6 | set ylabel "Mean absolute error [m]" 7 | set cblabel "Likelihood" 8 | set title font "Arial, 12" 9 | set key font "Arial, 12" 10 | set tics font "Arial, 12" 11 | set xlabel font "Arial, 12" 12 | set ylabel font "Arial, 12" 13 | set cblabel font "Arial, 12" 14 | set pm3d map interpolate 1, 1 15 | 16 | set xrange [ 0.0 : 1.0 ] 17 | set yrange [ 0.0 : 0.6 ] 18 | 19 | unset key 20 | unset grid 21 | 22 | splot "mae_decision_likelihoods.txt" u 1:2:4 with pm3d t "" 23 | pause -1 24 | -------------------------------------------------------------------------------- /src/als_ros/classifiers/MAE/plot_mae_decision_likelihoods_positive.gpt: -------------------------------------------------------------------------------- 1 | #! /bin/gnuplot 2 | 3 | set colors classic 4 | set size ratio 0.6 1 5 | set xlabel "Reliability" 6 | set ylabel "Mean absolute error [m]" 7 | set cblabel "Likelihood" 8 | set title font "Arial, 12" 9 | set key font "Arial, 12" 10 | set tics font "Arial, 12" 11 | set xlabel font "Arial, 12" 12 | set ylabel font "Arial, 12" 13 | set cblabel font "Arial, 12" 14 | set pm3d map interpolate 1, 1 15 | 16 | set xrange [ 0.0 : 1.0 ] 17 | set yrange [ 0.0 : 0.6 ] 18 | 19 | unset key 20 | unset grid 21 | 22 | splot "mae_decision_likelihoods.txt" u 1:2:5 with pm3d t "" 23 | pause -1 24 | -------------------------------------------------------------------------------- /src/als_ros/classifiers/MAE/classifier.yaml: -------------------------------------------------------------------------------- 1 | maxResidualError: 1.000000 2 | maeHistogramBinWidth: 0.025000 3 | failureThreshold: 0.225176 4 | successMAEMean: 0.165176 5 | successMAEStd: 0.059275 6 | failureMAEMean: 0.298687 7 | failureMAEStd: 0.090175 8 | truePositiveNum: 2584 9 | falseNegativeNum: 416 10 | trueNegativeNum: 2387 11 | falsePositiveNum: 613 12 | positiveMAEsFileName: positive_maes.txt 13 | negativeMAEsFileName: negative_maes.txt 14 | truePositiveMAEsFileName: true_positive_maes.txt 15 | trueNegativeMAEsFileName: true_negative_maes.txt 16 | falsePositiveMAEsFileName: false_positive_maes.txt 17 | falseNegativeMAEsFileName: false_negative_maes.txt 18 | -------------------------------------------------------------------------------- /src/als_ros/launch/robot_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 15 | 16 | -------------------------------------------------------------------------------- /src/als_ros/classifiers/MAE/plot_histograms.gpt: -------------------------------------------------------------------------------- 1 | #! /bin/gnuplot 2 | 3 | set colors classic 4 | set key top Right 5 | set title font "Arial, 14" 6 | set tics font "Arial, 14" 7 | set xlabel font "Arial, 14" 8 | set ylabel font "Arial, 14" 9 | set key font "Arial, 14" 10 | set grid 11 | set size ratio 0.5 1.0 12 | set xlabel "MAE [m]" 13 | set ylabel "Frequency" 14 | set style data histograms 15 | set style histogram cluster 16 | set style fill solid 0.7 border lt -1 17 | 18 | binwidth = 0.01 # histogram bin width 19 | set xrange [0.0 : 0.5] # [0.0 : max residual error] 20 | 21 | bin(x, width) = width * floor(x / width) 22 | 23 | plot "true_positive_maes.txt" u (bin($1, binwidth)):(1.0) smooth freq with boxes t "True positive", \ 24 | "false_negative_maes.txt" u (bin($1, binwidth)):(1.0) smooth freq with boxes t "False negative" 25 | 26 | # plot "false_positive_maes.txt" u (bin($1, binwidth)):(1.0) smooth freq with boxes t "Flase positive", \ 27 | # "true_negative_maes.txt" u (bin($1, binwidth)):(1.0) smooth freq with boxes t "True negative" 28 | 29 | pause -1 30 | 31 | -------------------------------------------------------------------------------- /src/als_ros/src/gl_pose_sampler.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | ros::init(argc, argv, "gl_pose_sampler"); 25 | als_ros::GLPoseSampler sampler; 26 | sampler.spin(); 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /src/als_ros/src/classifier_dataset_generator.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | ros::init(argc, argv, "classifier_dataset_generator"); 25 | als_ros::ClassifierDatasetGenerator generator; 26 | generator.datasetGenerationInit(); 27 | generator.generateDataset(); 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /src/als_ros/launch/mae_classifier_learning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 20 | 21 | 22 | ["/tmp/classifier_dataset_train/"] 23 | 24 | 25 | 26 | ["/tmp/classifier_dataset_test/"] 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /src/als_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | als_ros 4 | 0.0.0 5 | The als_ros package 6 | 7 | akai 8 | 9 | Apache License 2.0 10 | 11 | Naoki Akai --> 12 | 13 | catkin 14 | cv_bridge 15 | geometry_msgs 16 | nav_msgs 17 | roscpp 18 | rospy 19 | sensor_msgs 20 | std_msgs 21 | tf 22 | cv_bridge 23 | geometry_msgs 24 | nav_msgs 25 | roscpp 26 | rospy 27 | sensor_msgs 28 | std_msgs 29 | tf 30 | cv_bridge 31 | geometry_msgs 32 | nav_msgs 33 | roscpp 34 | rospy 35 | sensor_msgs 36 | std_msgs 37 | tf 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/als_ros/src/mrf_failure_detector.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | ros::init(argc, argv, "mrf_failure_detector"); 25 | 26 | als_ros::MRFFD detector; 27 | double failureDetectionHz = detector.getFailureDetectionHz(); 28 | ros::Rate loopRate(failureDetectionHz); 29 | while (ros::ok()) { 30 | ros::spinOnce(); 31 | detector.setCanUpdateResidualErrors(false); 32 | detector.predictFailureProbability(); 33 | detector.publishROSMessages(); 34 | detector.setCanUpdateResidualErrors(true); 35 | detector.printFailureProbability(); 36 | loopRate.sleep(); 37 | } 38 | 39 | return 0; 40 | } 41 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/Point.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __POINT_H__ 21 | #define __POINT_H__ 22 | 23 | namespace als_ros { 24 | 25 | class Point { 26 | private: 27 | double x_, y_; 28 | 29 | public: 30 | Point(): 31 | x_(0.0), y_(0.0) {}; 32 | 33 | Point(double x, double y): 34 | x_(x), y_(y) {}; 35 | 36 | ~Point() {}; 37 | 38 | inline void setX(double x) { x_ = x; } 39 | inline void setY(double y) { y_ = y; } 40 | inline void setPoint(double x, double y) { x_ = x, y_ = y; } 41 | inline void setPoint(Point p) { x_ = p.x_, y_ = p.y_; } 42 | 43 | inline double getX(void) { return x_; } 44 | inline double getY(void) { return y_; } 45 | inline Point getPoint(void) { return Point(x_, y_); } 46 | 47 | }; // class Point 48 | 49 | } // namespace als_ros 50 | 51 | #endif // __POINT_H__ 52 | -------------------------------------------------------------------------------- /src/als_ros/src/mcl.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | ros::init(argc, argv, "mcl"); 25 | 26 | als_ros::MCL mcl; 27 | double localizationHz = mcl.getLocalizationHz(); 28 | ros::Rate loopRate(localizationHz); 29 | 30 | while (ros::ok()) { 31 | ros::spinOnce(); 32 | mcl.updateParticlesByMotionModel(); 33 | mcl.setCanUpdateScan(false); 34 | mcl.calculateLikelihoodsByMeasurementModel(); 35 | mcl.calculateLikelihoodsByDecisionModel(); 36 | mcl.calculateGLSampledPosesLikelihood(); 37 | mcl.calculateAMCLRandomParticlesRate(); 38 | mcl.calculateEffectiveSampleSize(); 39 | mcl.estimatePose(); 40 | mcl.resampleParticles(); 41 | // mcl.plotScan(); 42 | // mcl.plotWorld(50.0); 43 | mcl.publishROSMessages(); 44 | mcl.broadcastTF(); 45 | // mcl.plotLikelihoodMap(); 46 | mcl.setCanUpdateScan(true); 47 | mcl.printResult(); 48 | loopRate.sleep(); 49 | } 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /src/als_ros/src/sm.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | ros::init(argc, argv, "mcl"); 25 | 26 | als_ros::MCL mcl; 27 | double localizationHz = mcl.getLocalizationHz(); 28 | ros::Rate loopRate(localizationHz); 29 | 30 | while (ros::ok()) { 31 | ros::spinOnce(); 32 | // mcl.updateParticlesByMotionModel(); 33 | mcl.setCanUpdateScan(false); 34 | // mcl.scanMatching(); 35 | mcl.scanMatching2(); 36 | // mcl.calculateLikelihoodsByMeasurementModel(); 37 | // mcl.calculateLikelihoodsByDecisionModel(); 38 | // mcl.calculateGLSampledPosesLikelihood(); 39 | // mcl.calculateAMCLRandomParticlesRate(); 40 | // mcl.calculateEffectiveSampleSize(); 41 | // mcl.estimatePose(); 42 | // mcl.resampleParticles(); 43 | // mcl.plotScan(); 44 | mcl.plotWorld(10.0); 45 | // mcl.publishROSMessages(); 46 | mcl.broadcastTF(); 47 | // mcl.plotLikelihoodMap(); 48 | mcl.setCanUpdateScan(true); 49 | // mcl.printResult(); 50 | loopRate.sleep(); 51 | } 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/Particle.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __PARTICLE_H__ 21 | #define __PARTICLE_H__ 22 | 23 | #include 24 | 25 | namespace als_ros { 26 | 27 | class Particle { 28 | private: 29 | Pose pose_; 30 | double w_; 31 | 32 | public: 33 | Particle(): pose_(0.0, 0.0, 0.0), w_(0.0) {}; 34 | 35 | Particle(double x, double y, double yaw, double w): pose_(x, y, yaw), w_(w) {}; 36 | 37 | Particle(Pose p, double w): pose_(p), w_(w) {}; 38 | 39 | ~Particle() {}; 40 | 41 | inline double getX(void) { return pose_.getX(); } 42 | inline double getY(void) { return pose_.getY(); } 43 | inline double getYaw(void) { return pose_.getYaw(); } 44 | inline Pose getPose(void) { return pose_; } 45 | inline double getW(void) { return w_; } 46 | 47 | inline void setX(double x) { pose_.setX(x); } 48 | inline void setY(double y) { pose_.setY(y); } 49 | inline void setYaw(double yaw) { pose_.setYaw(yaw); } 50 | inline void setPose(double x, double y, double yaw) { pose_.setPose(x, y, yaw); } 51 | inline void setPose(Pose p) { pose_.setPose(p); } 52 | inline void setW(double w) { w_ = w; } 53 | }; // class Particle 54 | 55 | } // namespace als_ros 56 | 57 | #endif // __PARTICLE_H__ 58 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/Pose.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __POSE_H__ 21 | #define __POSE_H__ 22 | 23 | #include 24 | 25 | namespace als_ros { 26 | 27 | class Pose { 28 | private: 29 | double x_, y_, yaw_; 30 | 31 | void modifyYaw(void) { 32 | while (yaw_ < -M_PI) 33 | yaw_ += 2.0 * M_PI; 34 | while (yaw_ > M_PI) 35 | yaw_ -= 2.0 * M_PI; 36 | } 37 | 38 | public: 39 | Pose(): 40 | x_(0.0), y_(0.0), yaw_(0.0) {}; 41 | 42 | Pose(double x, double y, double yaw): 43 | x_(x), y_(y), yaw_(yaw) {}; 44 | 45 | ~Pose() {}; 46 | 47 | inline void setX(double x) { x_ = x; } 48 | inline void setY(double y) { y_ = y; } 49 | inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); } 50 | inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); } 51 | inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); } 52 | 53 | inline double getX(void) { return x_; } 54 | inline double getY(void) { return y_; } 55 | inline double getYaw(void) { return yaw_; } 56 | inline Pose getPose(void) { return Pose(x_, y_, yaw_); } 57 | 58 | }; // class Pose 59 | 60 | } // namespace als_ros 61 | 62 | #endif // __POSE_H__ 63 | -------------------------------------------------------------------------------- /src/als_ros/src/slamer.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | if (argv[1] == NULL) { 25 | ROS_ERROR("argv[1] must be a yaml file for a indoor semantic map (ism)."); 26 | exit(1); 27 | } 28 | 29 | ros::init(argc, argv, "slamer"); 30 | als_ros::SLAMER slamer(argv[1]); 31 | double localizationHz = slamer.getLocalizationHz(); 32 | ros::Rate loopRate(localizationHz); 33 | 34 | while (ros::ok()) { 35 | ros::spinOnce(); 36 | slamer.updateParticlesByMotionModel(); 37 | slamer.setCanUpdateScan(false); 38 | slamer.calculateLikelihoodsByMeasurementModel(); 39 | slamer.calculateLikelihoodsBySLAMER(); 40 | slamer.recognizeObjectsWithMapAssist(); 41 | slamer.calculateLikelihoodsByDecisionModel(); 42 | slamer.calculateGLSampledPosesLikelihood(); 43 | slamer.calculateAMCLRandomParticlesRate(); 44 | slamer.calculateEffectiveSampleSize(); 45 | slamer.estimatePose(); 46 | slamer.resampleParticles(); 47 | slamer.publishROSMessages(); 48 | slamer.publishSLAMERROSMessages(); 49 | slamer.broadcastTF(); 50 | // slamer.plotLikelihoodMap(); 51 | slamer.setCanUpdateScan(true); 52 | slamer.printResult(); 53 | loopRate.sleep(); 54 | } 55 | 56 | return 0; 57 | } -------------------------------------------------------------------------------- /src/als_ros/src/scan2pc.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | class Scan2PC { 25 | private: 26 | ros::NodeHandle nh_; 27 | std::string scanName_, pcName_; 28 | ros::Subscriber scanSub_; 29 | ros::Publisher pcPub_; 30 | 31 | public: 32 | Scan2PC(void): 33 | nh_("~"), 34 | scanName_("/scan"), 35 | pcName_("/scan_point_cloud") 36 | { 37 | nh_.param("scan_name", scanName_, scanName_); 38 | nh_.param("pc_name", pcName_, pcName_); 39 | 40 | scanSub_ = nh_.subscribe(scanName_, 1, &Scan2PC::scanCB, this); 41 | pcPub_ = nh_.advertise(pcName_, 1); 42 | } 43 | 44 | void spin(void) { 45 | ros::spin(); 46 | } 47 | 48 | void scanCB(const sensor_msgs::LaserScan::ConstPtr &msg) { 49 | sensor_msgs::PointCloud pc; 50 | pc.header = msg->header; 51 | for (int i = 0; i < (int)msg->ranges.size(); ++i) { 52 | double r = msg->ranges[i]; 53 | if (r <= msg->range_min || msg->range_max <= r) 54 | continue; 55 | double t = msg->angle_min + (double)i * msg->angle_increment; 56 | double x = r * cos(t); 57 | double y = r * sin(t); 58 | geometry_msgs::Point32 p; 59 | p.x = x; 60 | p.y = y; 61 | p.z = 0.0; 62 | pc.points.push_back(p); 63 | } 64 | pcPub_.publish(pc); 65 | } 66 | }; // class Scan2PC 67 | 68 | int main(int argc, char **argv) { 69 | ros::init(argc, argv, "scan2pc"); 70 | Scan2PC node; 71 | node.spin(); 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /src/als_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(als_ros) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cv_bridge 6 | pcl_ros 7 | pcl_conversions 8 | geometry_msgs 9 | nav_msgs 10 | roscpp 11 | rospy 12 | sensor_msgs 13 | std_msgs 14 | tf 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | 20 | find_package(PkgConfig REQUIRED) 21 | pkg_check_modules(YAMLCPP yaml-cpp QUIET) 22 | if(NOT YAMLCPP_FOUND) 23 | find_package(yaml-cpp 0.6 REQUIRED) 24 | set(YAMLCPP_INCLUDE_DIRS ${YAML_CPP_INCLUDE_DIR}) 25 | set(YAMLCPP_LIBRARIES ${YAML_CPP_LIBRARIES}) 26 | add_definitions(-DHAVE_YAMLCPP_GT_0_5_0) 27 | else() 28 | if(YAMLCPP_VERSION VERSION_GREATER "0.5.0") 29 | add_definitions(-DHAVE_YAMLCPP_GT_0_5_0) 30 | endif() 31 | link_directories(${YAMLCPP_LIBRARY_DIRS}) 32 | endif() 33 | 34 | catkin_package( 35 | # INCLUDE_DIRS include 36 | # LIBRARIES als 37 | # CATKIN_DEPENDS cv_bridge geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf tf_conversions 38 | # DEPENDS system_lib 39 | ) 40 | 41 | include_directories( 42 | ${catkin_INCLUDE_DIRS} 43 | ${OpenCV_INCLUDE_DIRS} 44 | ${PCL_INCLUDE_DIRS} 45 | ${SDL_INCLUDE_DIR} 46 | ${SDL_IMAGE_INCLUDE_DIRS} 47 | ${YAMLCPP_INCLUDE_DIRS} 48 | "${PROJECT_SOURCE_DIR}/include" 49 | ) 50 | 51 | add_executable(mcl src/mcl.cpp) 52 | target_link_libraries(mcl ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${YAMLCPP_LIBRARIES}) 53 | 54 | add_executable(sm src/sm.cpp) 55 | target_link_libraries(sm ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${YAMLCPP_LIBRARIES}) 56 | 57 | add_executable(mrf_failure_detector src/mrf_failure_detector.cpp) 58 | target_link_libraries(mrf_failure_detector ${catkin_LIBRARIES}) 59 | 60 | add_executable(gl_pose_sampler src/gl_pose_sampler.cpp) 61 | target_link_libraries(gl_pose_sampler ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 62 | 63 | add_executable(classifier_dataset_generator src/classifier_dataset_generator.cpp) 64 | target_link_libraries(classifier_dataset_generator ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) 65 | 66 | add_executable(mae_classifier_learning src/mae_classifier_learning.cpp) 67 | target_link_libraries(mae_classifier_learning ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${YAMLCPP_LIBRARIES}) 68 | 69 | add_executable(evaluator src/evaluator.cpp) 70 | target_link_libraries(evaluator ${catkin_LIBRARIES}) 71 | 72 | add_executable(scan2pc src/scan2pc.cpp) 73 | target_link_libraries(scan2pc ${catkin_LIBRARIES}) 74 | 75 | add_executable(slamer src/slamer.cpp) 76 | target_link_libraries(slamer ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${YAMLCPP_LIBRARIES} ${PCL_LIBRARY_DIRS}) 77 | -------------------------------------------------------------------------------- /src/als_ros/src/mae_classifier_learning.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | int main(int argc, char **argv) { 27 | ros::init(argc, argv, "mae_classifier_learning"); 28 | ros::NodeHandle nh("~"); 29 | 30 | std::vector trainDirs, testDirs; 31 | std::string classifierDir; 32 | double maxResidualError, maeHistogramBinWidth; 33 | 34 | nh.param("train_dirs", trainDirs, trainDirs); 35 | nh.param("test_dirs", testDirs, testDirs); 36 | nh.param("classifier_dir", classifierDir, classifierDir); 37 | nh.param("max_residual_error", maxResidualError, maxResidualError); 38 | nh.param("histogram_bin_width", maeHistogramBinWidth, maeHistogramBinWidth); 39 | 40 | std::vector gtPosesTrain, successPosesTrain, failurePosesTrain; 41 | std::vector scansTrain; 42 | std::vector> successResidualErrorsTrain, failureResidualErrorsTrain; 43 | 44 | std::vector gtPosesTest, successPosesTest, failurePosesTest; 45 | std::vector scansTest; 46 | std::vector> successResidualErrorsTest, failureResidualErrorsTest; 47 | 48 | als_ros::ClassifierDatasetGenerator generator; 49 | generator.setTrainDirs(trainDirs); 50 | generator.setTestDirs(testDirs); 51 | generator.readTrainDataset(gtPosesTrain, successPosesTrain, failurePosesTrain, scansTrain, successResidualErrorsTrain, failureResidualErrorsTrain); 52 | generator.readTestDataset(gtPosesTest, successPosesTest, failurePosesTest, scansTest, successResidualErrorsTest, failureResidualErrorsTest); 53 | 54 | als_ros::MAEClassifier classifier; 55 | classifier.setClassifierDir(classifierDir); 56 | classifier.setMaxResidualError(maxResidualError); 57 | classifier.setMAEHistogramBinWidth(maeHistogramBinWidth); 58 | classifier.learnThreshold(successResidualErrorsTrain, failureResidualErrorsTrain); 59 | classifier.writeClassifierParams(successResidualErrorsTest, failureResidualErrorsTest); 60 | classifier.writeDecisionLikelihoods(); 61 | 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /src/als_ros/launch/classifier_dataset_generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 33 | 34 | 35 | 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 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/Histogram.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __HISTOGRAM_H__ 21 | #define __HISTOGRAM_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace als_ros { 28 | 29 | class Histogram { 30 | private: 31 | double binWidth_; 32 | double minValue_, maxValue_; 33 | int histogramSize_, valueNum_; 34 | std::vector histogram_; 35 | std::vector probability_; 36 | 37 | inline int val2bin(double val) { 38 | int bin = (int)((val - minValue_) / binWidth_); 39 | if (bin < 0 || histogramSize_ <= bin) 40 | bin = -1; 41 | return bin; 42 | } 43 | 44 | inline double bin2val(int bin) { 45 | return (double)bin * binWidth_ + minValue_; 46 | } 47 | 48 | void buildHistogram(std::vector values) { 49 | histogramSize_ = (int)((maxValue_ - minValue_) / binWidth_) + 1; 50 | 51 | histogram_.resize(histogramSize_, 0); 52 | valueNum_ = 0; 53 | for (int i = 0; i < (int)values.size(); ++i) { 54 | int bin = val2bin(values[i]); 55 | if (bin >= 0) { 56 | histogram_[bin]++; 57 | valueNum_++; 58 | } 59 | } 60 | 61 | probability_.resize(histogramSize_, 0.0); 62 | for (int i = 0; i < (int)histogram_.size(); ++i) 63 | probability_[i] = (double)histogram_[i] / (double)valueNum_; 64 | } 65 | 66 | public: 67 | Histogram(void) {} 68 | 69 | Histogram(std::vector values, double binWidth): 70 | binWidth_(binWidth) 71 | { 72 | minValue_ = *std::min_element(values.begin(), values.end()); 73 | maxValue_ = *std::max_element(values.begin(), values.end()); 74 | buildHistogram(values); 75 | } 76 | 77 | Histogram(std::vector values, double binWidth, double minValue, double maxValue): 78 | binWidth_(binWidth), 79 | minValue_(minValue), 80 | maxValue_(maxValue) 81 | { 82 | buildHistogram(values); 83 | } 84 | 85 | inline double getProbability(double value) { 86 | if (value >= maxValue_) 87 | value -= binWidth_; 88 | int bin = val2bin(value); 89 | if (bin >= 0) 90 | return probability_[bin]; 91 | else 92 | return -1.0; 93 | } 94 | 95 | void smoothHistogram(void) { 96 | std::vector vals((int)histogram_.size()); 97 | double sum = 0.0; 98 | for (int i = 0; i < (int)histogram_.size(); ++i) { 99 | int i0 = i - 1; 100 | if (i0 < 0) 101 | i0 = 0; 102 | int i1 = i + 1; 103 | if (i1 >= (int)histogram_.size()) 104 | i1 = (int)histogram_.size() - 1; 105 | double val = (histogram_[i0] + histogram_[i] + histogram_[i1]) / 3.0; 106 | vals[i] = val; 107 | sum += val; 108 | } 109 | for (int i = 0; i < (int)histogram_.size(); ++i) 110 | histogram_[i] = vals[i] / sum; 111 | } 112 | 113 | void printHistogram(void) { 114 | for (int i = 0; i < (int)histogram_.size(); ++i) 115 | printf("bin = %d: val = %lf, histogram[%d] = %d, probability[%d] = %lf\n", 116 | i, bin2val(i), i, histogram_[i], i, probability_[i]); 117 | } 118 | }; // class Histogram 119 | 120 | } // namespace als_ros 121 | 122 | #endif // __HISTOGRAM_H__ 123 | -------------------------------------------------------------------------------- /src/als_ros/launch/slamer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 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 | -------------------------------------------------------------------------------- /src/als_ros/launch/mrf_failure_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 47 | [0.8, 0.0, 0.2, 0.0, 0.8, 0.2, 0.333333, 0.333333, 0.333333] 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 | -------------------------------------------------------------------------------- /src/als_ros/launch/gl_pose_sampler.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 40 | 41 | 42 | 44 | 45 | 46 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 60 | 61 | 62 | 63 | 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # als_ros 2 | 3 | An Advanced Localization System [1] for Robot Operating System use (als_ros) is a localization package with 2D LiDAR. als_ros contains following functions; 4 | 5 | - Robust localization based on sensor measurement class estimation [2], 6 | - Reliability estimation based on Bayesian filtering with a simple classifier of localization correctness [3], 7 | - Misalignment recognition using Markov random fields with fully connected latent variables [4], 8 | - Quick re-localization based on fusion of pose tracking and global localization via the importance sampling [5]. 9 | 10 | These details can be seen at [Reliable Monte Carlo Localization for Mobile Robots (arXiv preprint)](https://arxiv.org/abs/2205.04769). 11 | 12 | [Demonstration video](https://www.youtube.com/watch?v=wsoXvUgJvWk) showing comparison of als_ros with ROS amcl. 13 | 14 | 15 | 16 | # How to install and use 17 | 18 | ## How to install 19 | 20 | ROS environment is needed to be installed first. I confirmed that als_ros works on **Ubuntu 18.04** with melodic and **Ubuntu 20.04** with noetic. 21 | 22 | als_ros can be installed with following commands. 23 | 24 | ``` 25 | $ git clone https://github.com/NaokiAkai/als_ros.git 26 | $ cd als_ros 27 | $ catkin_make 28 | $ source devel/setup.bash 29 | ``` 30 | 31 | If you do not want to make a new workspace for als_ros, please copy the als_ros package to your workspace. The cloned directory has a ROS workspace. 32 | 33 | 34 | 35 | ## How to use 36 | 37 | Following messages (topics) are needed to be published; 38 | 39 | - sensor_msgs::LaserScan (/scan) 40 | - nav_msgs::Odometry (/odom) 41 | - nav_msgs::OccupancyGrid (/map) 42 | 43 | Names inside of the brackets are default topic names. 44 | 45 | Also, static transformation between following two frames is needed to be set. 46 | 47 | - origin of a robot (base_link) 48 | - 2D LiDAR (laser) 49 | 50 | Names inside of the brackets are default frame names. 51 | 52 | There are launch files in the als_ros package. These names can be changed in **mcl.launch**. 53 | 54 | 55 | 56 | After setting the topics and transformation, the localization software can be used with mcl.launch. 57 | 58 | ``` 59 | $ roslaunch als_ros mcl.launch 60 | ``` 61 | 62 | In default, localization for pose tracking with the robust localization and reliability estimation techniques presented in [2, 3] is executed. 63 | 64 | 65 | 66 | If you want to use fusion of pose tracking and global localization, please set use_gl_pose_sampler flag to true. 67 | 68 | ``` 69 | $ roslaunch als_ros mcl.launch use_gl_pose_sampler:=true 70 | ``` 71 | 72 | In als_ros, global localization is implemented using the free-space feature presented in [6]. 73 | 74 | 75 | 76 | If you want to use estimation of localization failure probability with misalignment recognition, please set use_mrf_failure_detector flag to true. 77 | 78 | ``` 79 | $ roslaunch als_ros mcl.launch use_mrf_failure_detector:=true 80 | ``` 81 | 82 | 83 | 84 | ## Parameter descriptions 85 | 86 | Descriptions for all the parameters are written in the launch files. I am planning to make a more precise document. 87 | 88 | 89 | 90 | # Citation 91 | 92 | [arXiv preprint](https://arxiv.org/abs/2205.04769) and [journal paper](https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.22149) are available. If you used als_ros in your research, please cite the journal paper. 93 | 94 | ``` 95 | Naoki Akai. "Reliable Monte Carlo Localization for Mobile Robots," Journal of Field Robotics, vol. 40, no.3, pp. 595-613, 2023. 96 | ``` 97 | 98 | ``` 99 | @article{Akai2023JFR:ReliableMC, 100 | title = {Reliable Monte Carlo Localization for Mobile Robots}, 101 | author = {Akai, Naoki}, 102 | journal = {Journal of Field Robotics}, 103 | volume = {40}, 104 | number = {3}, 105 | pages = {595--613}, 106 | year = {2023} 107 | } 108 | ``` 109 | 110 | 111 | 112 | # References 113 | 114 | [1] Naoki Akai．"An advanced localization system using LiDAR: Performance improvement of localization for mobile robots and its implementation," CORONA PUBLISHING CO., LTD (to be appeared, in Japanese). 115 | 116 | [2] Naoki Akai, Luis Yoichi Morales, and Hiroshi Murase. "Mobile robot localization considering class of sensor observations," In *Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pp. 3159-3166, 2018. 117 | 118 | [3] Naoki Akai, Luis Yoichi Morales, Hiroshi Murase. "Simultaneous pose and reliability estimation using convolutional neural network and Rao-Blackwellized particle filter," *Advanced Robotics*, vol. 32, no. 17, pp. 930-944, 2018. 119 | 120 | [4] Naoki Akai, Luis Yoichi Morales, Takatsugu Hirayama, and Hiroshi Murase. "Misalignment recognition using Markov random fields with fully connected latent variables for detecting localization failures," *IEEE Robotics and Automation Letters*, vol. 4, no. 4, pp. 3955-3962, 2019. 121 | 122 | [5] Naoki Akai, Takatsugu Hirayama, and Hiroshi Murase. "Hybrid localization using model- and learning-based methods: Fusion of Monte Carlo and E2E localizations via importance sampling," In *Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)*, pp. 6469-6475, 2020. 123 | 124 | [6] Alexander Millane, Helen, Oleynikova, Juan Nieto, Roland Siegwart, and César Cadena. "Free-space features: Global localization in 2D laser SLAM using distance function maps," In *Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pp. 1271-1277, 2019. 125 | 126 | 127 | 128 | # SLAMER 129 | 130 | A node for Simultaneous Localization And Map-assisted Environment Recognition (SLAMER) is also implemented in this repository. However, I do not consider to support it officially. Please refer [the SLAMER paper](https://arxiv.org/abs/2207.09909) if you want to know details. This work was presented at ICRA 2023. 131 | 132 | 133 | 134 | # License 135 | 136 | Apache License 2.0 137 | 138 | -------------------------------------------------------------------------------- /src/als_ros/classifiers/MAE/false_negative_maes.txt: -------------------------------------------------------------------------------- 1 | 0.234409 2 | 0.228541 3 | 0.254901 4 | 0.239693 5 | 0.238372 6 | 0.246345 7 | 0.372380 8 | 0.324212 9 | 0.227140 10 | 0.235902 11 | 0.365915 12 | 0.231051 13 | 0.392905 14 | 0.572949 15 | 0.364072 16 | 0.254624 17 | 0.295502 18 | 0.230154 19 | 0.261905 20 | 0.227747 21 | 0.241679 22 | 0.251290 23 | 0.233649 24 | 0.232595 25 | 0.240913 26 | 0.229751 27 | 0.305187 28 | 0.244396 29 | 0.255445 30 | 0.233327 31 | 0.244587 32 | 0.356345 33 | 0.240493 34 | 0.234564 35 | 0.252025 36 | 0.225684 37 | 0.328553 38 | 0.279209 39 | 0.229882 40 | 0.277577 41 | 0.250168 42 | 0.235852 43 | 0.231301 44 | 0.235228 45 | 0.255822 46 | 0.380405 47 | 0.321140 48 | 0.261960 49 | 0.255617 50 | 0.234547 51 | 0.252354 52 | 0.240684 53 | 0.313962 54 | 0.282758 55 | 0.253752 56 | 0.245339 57 | 0.261239 58 | 0.259041 59 | 0.248440 60 | 0.242718 61 | 0.239992 62 | 0.306622 63 | 0.239597 64 | 0.236613 65 | 0.226002 66 | 0.407351 67 | 0.254250 68 | 0.281746 69 | 0.231908 70 | 0.245400 71 | 0.256852 72 | 0.243332 73 | 0.248855 74 | 0.230676 75 | 0.250347 76 | 0.242376 77 | 0.234935 78 | 0.256580 79 | 0.257630 80 | 0.270709 81 | 0.234971 82 | 0.380027 83 | 0.256972 84 | 0.232037 85 | 0.255363 86 | 0.255324 87 | 0.237237 88 | 0.228159 89 | 0.230017 90 | 0.225619 91 | 0.229595 92 | 0.329947 93 | 0.263758 94 | 0.302184 95 | 0.237254 96 | 0.240718 97 | 0.228385 98 | 0.228252 99 | 0.258569 100 | 0.291293 101 | 0.228599 102 | 0.241433 103 | 0.307667 104 | 0.275639 105 | 0.268295 106 | 0.236679 107 | 0.228673 108 | 0.237277 109 | 0.257357 110 | 0.301027 111 | 0.230995 112 | 0.243122 113 | 0.360163 114 | 0.242818 115 | 0.226542 116 | 0.252820 117 | 0.261742 118 | 0.226185 119 | 0.269725 120 | 0.268981 121 | 0.261173 122 | 0.227485 123 | 0.242704 124 | 0.227899 125 | 0.235992 126 | 0.230602 127 | 0.266249 128 | 0.263258 129 | 0.240486 130 | 0.329193 131 | 0.235960 132 | 0.365356 133 | 0.235518 134 | 0.316465 135 | 0.255164 136 | 0.247720 137 | 0.244397 138 | 0.240381 139 | 0.234628 140 | 0.230541 141 | 0.304402 142 | 0.259335 143 | 0.245892 144 | 0.243224 145 | 0.227380 146 | 0.239481 147 | 0.276811 148 | 0.231010 149 | 0.238075 150 | 0.238244 151 | 0.228352 152 | 0.241828 153 | 0.244942 154 | 0.310487 155 | 0.257465 156 | 0.230972 157 | 0.234843 158 | 0.236265 159 | 0.314933 160 | 0.228310 161 | 0.233889 162 | 0.227478 163 | 0.268869 164 | 0.272909 165 | 0.242010 166 | 0.248746 167 | 0.234509 168 | 0.243172 169 | 0.228150 170 | 0.278350 171 | 0.393165 172 | 0.561741 173 | 0.291102 174 | 0.275784 175 | 0.259892 176 | 0.227375 177 | 0.255235 178 | 0.227138 179 | 0.255799 180 | 0.226618 181 | 0.299418 182 | 0.246836 183 | 0.253085 184 | 0.275498 185 | 0.265012 186 | 0.246795 187 | 0.265877 188 | 0.301943 189 | 0.232542 190 | 0.251500 191 | 0.286847 192 | 0.236917 193 | 0.254724 194 | 0.256691 195 | 0.252611 196 | 0.236817 197 | 0.252318 198 | 0.225239 199 | 0.290540 200 | 0.270778 201 | 0.290997 202 | 0.260088 203 | 0.232034 204 | 0.239244 205 | 0.226314 206 | 0.272551 207 | 0.231650 208 | 0.249447 209 | 0.240569 210 | 0.278422 211 | 0.231995 212 | 0.229065 213 | 0.258845 214 | 0.309399 215 | 0.271090 216 | 0.257403 217 | 0.234452 218 | 0.251598 219 | 0.240022 220 | 0.226432 221 | 0.229340 222 | 0.231308 223 | 0.331095 224 | 0.280672 225 | 0.285252 226 | 0.385790 227 | 0.238144 228 | 0.308957 229 | 0.276690 230 | 0.385814 231 | 0.294878 232 | 0.278466 233 | 0.282222 234 | 0.254168 235 | 0.245760 236 | 0.241915 237 | 0.228676 238 | 0.272863 239 | 0.231277 240 | 0.245793 241 | 0.246926 242 | 0.295478 243 | 0.265960 244 | 0.230795 245 | 0.228915 246 | 0.261036 247 | 0.300682 248 | 0.234135 249 | 0.252213 250 | 0.323578 251 | 0.247108 252 | 0.251533 253 | 0.284265 254 | 0.232229 255 | 0.254547 256 | 0.246481 257 | 0.229509 258 | 0.268266 259 | 0.269141 260 | 0.303857 261 | 0.238019 262 | 0.231044 263 | 0.262968 264 | 0.248598 265 | 0.357343 266 | 0.230658 267 | 0.293067 268 | 0.297272 269 | 0.259624 270 | 0.280128 271 | 0.231039 272 | 0.477619 273 | 0.275270 274 | 0.231401 275 | 0.272557 276 | 0.437345 277 | 0.368938 278 | 0.247958 279 | 0.231723 280 | 0.240987 281 | 0.247533 282 | 0.234786 283 | 0.380533 284 | 0.265636 285 | 0.234109 286 | 0.270540 287 | 0.311395 288 | 0.244213 289 | 0.265455 290 | 0.240970 291 | 0.244241 292 | 0.542635 293 | 0.244111 294 | 0.321817 295 | 0.299210 296 | 0.227445 297 | 0.232197 298 | 0.251242 299 | 0.261769 300 | 0.297549 301 | 0.245627 302 | 0.249368 303 | 0.227945 304 | 0.244580 305 | 0.229365 306 | 0.230073 307 | 0.270887 308 | 0.231662 309 | 0.248392 310 | 0.236100 311 | 0.228449 312 | 0.296349 313 | 0.253582 314 | 0.270471 315 | 0.259930 316 | 0.238130 317 | 0.315321 318 | 0.291686 319 | 0.227280 320 | 0.259807 321 | 0.293859 322 | 0.232931 323 | 0.274280 324 | 0.261734 325 | 0.258613 326 | 0.248088 327 | 0.225753 328 | 0.254767 329 | 0.232286 330 | 0.291223 331 | 0.287081 332 | 0.227283 333 | 0.250276 334 | 0.240980 335 | 0.227685 336 | 0.260623 337 | 0.243504 338 | 0.225297 339 | 0.282622 340 | 0.241557 341 | 0.269024 342 | 0.258079 343 | 0.233751 344 | 0.249705 345 | 0.226618 346 | 0.239655 347 | 0.293147 348 | 0.226462 349 | 0.227462 350 | 0.235189 351 | 0.243623 352 | 0.240984 353 | 0.251584 354 | 0.232354 355 | 0.245439 356 | 0.293509 357 | 0.264276 358 | 0.233139 359 | 0.269327 360 | 0.259919 361 | 0.304573 362 | 0.284687 363 | 0.326426 364 | 0.259534 365 | 0.296104 366 | 0.272065 367 | 0.243940 368 | 0.233037 369 | 0.230707 370 | 0.228521 371 | 0.234417 372 | 0.291025 373 | 0.254475 374 | 0.227735 375 | 0.274809 376 | 0.277465 377 | 0.245665 378 | 0.319983 379 | 0.262646 380 | 0.261127 381 | 0.275766 382 | 0.290681 383 | 0.230692 384 | 0.250082 385 | 0.251625 386 | 0.236840 387 | 0.226891 388 | 0.228104 389 | 0.235949 390 | 0.228503 391 | 0.241337 392 | 0.230615 393 | 0.228490 394 | 0.332638 395 | 0.249984 396 | 0.264646 397 | 0.289451 398 | 0.270922 399 | 0.276042 400 | 0.275723 401 | 0.479004 402 | 0.230491 403 | 0.238787 404 | 0.250412 405 | 0.307049 406 | 0.231029 407 | 0.264319 408 | 0.277274 409 | 0.339019 410 | 0.282690 411 | 0.263821 412 | 0.308741 413 | 0.421560 414 | 0.230056 415 | 0.267586 416 | 0.239238 417 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/ISM.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __ISM_H__ 21 | #define __ISM_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace als_ros { 34 | 35 | class ISM { 36 | private: 37 | std::vector objectNames_; 38 | std::vector> objectColors_; 39 | 40 | int mapWidth_, mapHeight_; 41 | double mapResolution_; 42 | Pose mapOrigin_; 43 | 44 | std::vector distMaps_; 45 | pcl::PointCloud::Ptr ismPoints_; 46 | 47 | public: 48 | ISM(void): 49 | ismPoints_(new pcl::PointCloud()) {} 50 | 51 | inline std::vector getObjectNames(void) { return objectNames_; } 52 | inline double getMapResolution(void) { return mapResolution_; } 53 | 54 | void readISM(std::string ismYamlFile) { 55 | std::string ismYamlPath = (std::string)realpath(ismYamlFile.c_str(), NULL); 56 | std::string dirPath = ismYamlPath.substr(0, ismYamlPath.find_last_of("/") + 1); 57 | // std::cout << "ISM YAML payh is " << ismYamlPath << std::endl; 58 | // std::cout << "Dir path is " << dirPath << std::endl; 59 | YAML::Node ismNode = YAML::LoadFile(ismYamlFile); 60 | 61 | std::string ogmInfoYamlFile = ismNode["ogm_info_yaml"].as(); 62 | std::string ogmFilePath = dirPath + ogmInfoYamlFile; 63 | YAML::Node ogmNode = YAML::LoadFile(ogmFilePath); 64 | mapResolution_ = ogmNode["resolution"].as(); 65 | std::vector mapOrigin = ogmNode["origin"].as>(); 66 | mapOrigin_.setPose(mapOrigin[0], mapOrigin[1], mapOrigin[2]); 67 | 68 | std::string ismImageFile = ismNode["image"].as(); 69 | std::string ismFilePath = dirPath + ismImageFile; 70 | // std::cout << "ISM image file path is " << ismFilePath << std::endl; 71 | cv::Mat ismImage = cv::imread(ismFilePath.c_str(), 1); 72 | if (ismImage.empty()) { 73 | ROS_ERROR("Could not read the ISM image -> %s", ismFilePath.c_str()); 74 | exit(1); 75 | } 76 | mapWidth_ = ismImage.cols; 77 | mapHeight_ = ismImage.rows; 78 | // std::cout << "mapWidth_ " << mapWidth_ << std::endl; 79 | // std::cout << "mapHeight_ " << mapHeight_ << std::endl; 80 | 81 | objectNames_ = ismNode["objects"].as>(); 82 | objectColors_.resize((int)objectNames_.size()); 83 | for (int i = 0; i < (int)objectNames_.size(); ++i) { 84 | std::string tagName = objectNames_[i] + "_color"; 85 | objectColors_[i] = ismNode[tagName.c_str()].as>(); 86 | // std::cout << i << " " << objectColors_[i][0] << " " << objectColors_[i][1] << " " << objectColors_[i][2] << std::endl; 87 | } 88 | 89 | std::vector objectMaps((int)objectNames_.size()); 90 | for (int i = 0; i < (int)objectMaps.size(); ++i) 91 | objectMaps[i] = cv::Mat::ones(mapHeight_, mapWidth_, CV_8UC1); 92 | 93 | for (int u = 0; u < ismImage.cols; ++u) { 94 | for (int v = 0; v < ismImage.rows; ++v) { 95 | cv::Vec3b val = ismImage.at(v, u); 96 | if (val[0] == 205 && val[1] == 205 && val[2] == 205) // unknown space 97 | continue; 98 | if (val[0] == 254 && val[1] == 254 && val[2] == 254) // free space 99 | continue; 100 | // if (val[0] == 0 && val[1] == 0 && val[2] == 0) // occupied space 101 | // continue; 102 | // NOTE: occupied space is treated as others in slamer 103 | for (int i = 0; i < (int)objectNames_.size(); ++i) { 104 | if (val[0] == objectColors_[i][2] && val[1] == objectColors_[i][1] && val[2] == objectColors_[i][0]) { 105 | pcl::PointXYZRGB p; 106 | uv2xy(u, v, &p.x, &p.y); 107 | p.z = 0.0; 108 | p.r = objectColors_[i][0]; 109 | p.g = objectColors_[i][1]; 110 | p.b = objectColors_[i][2]; 111 | ismPoints_->points.push_back(p); 112 | objectMaps[i].at(v, u) = 0; 113 | // printf("%s, at (u, v) = (%d, %d) with (r, g, b) = (%d, %d, %d)\n", 114 | // objectNames_[i].c_str(), u, v, val[2], val[1], val[0]); 115 | break; 116 | } 117 | } 118 | } 119 | } 120 | ismPoints_->width = ismPoints_->points.size(); 121 | ismPoints_->height = 1; 122 | 123 | distMaps_.resize((int)objectMaps.size()); 124 | for (int i = 0; i < (int)objectMaps.size(); ++i) { 125 | distMaps_[i] = cv::Mat(mapHeight_, mapWidth_, CV_32FC1); 126 | cv::distanceTransform(objectMaps[i], distMaps_[i], cv::DIST_L2, 5); 127 | for (int v = 0; v < mapHeight_; v++) { 128 | for (int u = 0; u < mapWidth_; u++) { 129 | float d = distMaps_[i].at(v, u) * (float)mapResolution_; 130 | distMaps_[i].at(v, u) = d; 131 | } 132 | } 133 | } 134 | } 135 | 136 | inline sensor_msgs::PointCloud2 getISMPointsAsPC2(void) { 137 | sensor_msgs::PointCloud2 ismPointsMsg; 138 | pcl::toROSMsg(*ismPoints_.get(), ismPointsMsg); 139 | return ismPointsMsg; 140 | } 141 | 142 | inline float getDistance(double x, double y, std::string object) { 143 | int u, v; 144 | xy2uv(x, y, &u, &v); 145 | if (u < 0 || mapWidth_ <= u || v < 0 || mapHeight_ <= v) 146 | return -1.0f; 147 | 148 | for (int i = 0; i < (int)objectNames_.size(); ++i) { 149 | if (objectNames_[i] == object) 150 | return distMaps_[i].at(v, u); 151 | } 152 | return -1.0f; 153 | } 154 | 155 | private: 156 | template 157 | inline void xy2uv(T x, T y, int *u, int *v) { 158 | *u = (int)((x - mapOrigin_.getX()) / mapResolution_); 159 | *v = mapHeight_ - 1 - (int)((y - mapOrigin_.getY()) / mapResolution_); 160 | } 161 | 162 | template 163 | inline void uv2xy(int u, int v, T *x, T *y) { 164 | *x = (T)u * mapResolution_ + mapOrigin_.getX(); 165 | *y = (T)(mapHeight_ - 1 - v) * mapResolution_ + mapOrigin_.getY(); 166 | } 167 | }; // class ISM 168 | 169 | } // namespace als_ros 170 | 171 | #endif // __ISM_H__ -------------------------------------------------------------------------------- /src/als_ros/classifiers/MAE/false_positive_maes.txt: -------------------------------------------------------------------------------- 1 | 0.189139 2 | 0.133544 3 | 0.217986 4 | 0.165542 5 | 0.166591 6 | 0.211395 7 | 0.120852 8 | 0.223556 9 | 0.191761 10 | 0.132508 11 | 0.112646 12 | 0.192059 13 | 0.224686 14 | 0.172721 15 | 0.220637 16 | 0.166787 17 | 0.182533 18 | 0.202209 19 | 0.207323 20 | 0.190363 21 | 0.221817 22 | 0.178220 23 | 0.133886 24 | 0.181222 25 | 0.168485 26 | 0.197259 27 | 0.216359 28 | 0.200811 29 | 0.193017 30 | 0.210958 31 | 0.103041 32 | 0.161311 33 | 0.174729 34 | 0.093019 35 | 0.179788 36 | 0.074454 37 | 0.212736 38 | 0.213214 39 | 0.221425 40 | 0.174185 41 | 0.153561 42 | 0.158525 43 | 0.164849 44 | 0.200984 45 | 0.195925 46 | 0.182586 47 | 0.106582 48 | 0.154539 49 | 0.201262 50 | 0.116951 51 | 0.212067 52 | 0.175719 53 | 0.223053 54 | 0.221528 55 | 0.143659 56 | 0.121780 57 | 0.104775 58 | 0.113673 59 | 0.215998 60 | 0.215887 61 | 0.158812 62 | 0.223421 63 | 0.144991 64 | 0.172275 65 | 0.056944 66 | 0.199648 67 | 0.176158 68 | 0.201669 69 | 0.219657 70 | 0.195702 71 | 0.184491 72 | 0.070772 73 | 0.204510 74 | 0.119407 75 | 0.118567 76 | 0.189710 77 | 0.122220 78 | 0.211963 79 | 0.162440 80 | 0.201016 81 | 0.199651 82 | 0.215028 83 | 0.196216 84 | 0.218181 85 | 0.173371 86 | 0.199407 87 | 0.158151 88 | 0.211726 89 | 0.173649 90 | 0.146073 91 | 0.167942 92 | 0.193788 93 | 0.189713 94 | 0.191050 95 | 0.172710 96 | 0.151421 97 | 0.193177 98 | 0.163473 99 | 0.200526 100 | 0.144145 101 | 0.215036 102 | 0.214776 103 | 0.163862 104 | 0.165133 105 | 0.122728 106 | 0.094463 107 | 0.188856 108 | 0.137531 109 | 0.188939 110 | 0.078819 111 | 0.160891 112 | 0.205115 113 | 0.203514 114 | 0.204122 115 | 0.215270 116 | 0.118028 117 | 0.162207 118 | 0.132610 119 | 0.134291 120 | 0.195425 121 | 0.192365 122 | 0.181959 123 | 0.140214 124 | 0.202190 125 | 0.152755 126 | 0.216250 127 | 0.201506 128 | 0.128566 129 | 0.207770 130 | 0.216169 131 | 0.203921 132 | 0.141735 133 | 0.138882 134 | 0.107591 135 | 0.119611 136 | 0.199021 137 | 0.223573 138 | 0.167904 139 | 0.173185 140 | 0.212868 141 | 0.153489 142 | 0.188081 143 | 0.183157 144 | 0.188567 145 | 0.209254 146 | 0.200147 147 | 0.224282 148 | 0.206325 149 | 0.114078 150 | 0.214611 151 | 0.173569 152 | 0.170946 153 | 0.134810 154 | 0.175041 155 | 0.124466 156 | 0.206891 157 | 0.210454 158 | 0.203573 159 | 0.141236 160 | 0.147409 161 | 0.120890 162 | 0.172494 163 | 0.111403 164 | 0.183881 165 | 0.191763 166 | 0.162393 167 | 0.215866 168 | 0.185308 169 | 0.220577 170 | 0.223097 171 | 0.209646 172 | 0.162553 173 | 0.161816 174 | 0.129069 175 | 0.147035 176 | 0.169247 177 | 0.198658 178 | 0.191814 179 | 0.122686 180 | 0.149813 181 | 0.118638 182 | 0.216660 183 | 0.183503 184 | 0.207542 185 | 0.175028 186 | 0.177759 187 | 0.177003 188 | 0.104596 189 | 0.125972 190 | 0.172010 191 | 0.192842 192 | 0.217066 193 | 0.138576 194 | 0.131918 195 | 0.220652 196 | 0.127032 197 | 0.153160 198 | 0.157024 199 | 0.192757 200 | 0.198756 201 | 0.223669 202 | 0.150515 203 | 0.124572 204 | 0.214112 205 | 0.125580 206 | 0.172373 207 | 0.174349 208 | 0.224483 209 | 0.186318 210 | 0.199807 211 | 0.144899 212 | 0.076123 213 | 0.142938 214 | 0.220331 215 | 0.193909 216 | 0.141207 217 | 0.183473 218 | 0.092579 219 | 0.223919 220 | 0.170300 221 | 0.187452 222 | 0.107461 223 | 0.183372 224 | 0.182487 225 | 0.186491 226 | 0.098626 227 | 0.218532 228 | 0.184381 229 | 0.201695 230 | 0.195589 231 | 0.170138 232 | 0.196728 233 | 0.190135 234 | 0.194677 235 | 0.067799 236 | 0.208536 237 | 0.100170 238 | 0.167485 239 | 0.126660 240 | 0.197921 241 | 0.177370 242 | 0.158938 243 | 0.222942 244 | 0.175785 245 | 0.173076 246 | 0.187134 247 | 0.214900 248 | 0.197850 249 | 0.154038 250 | 0.196197 251 | 0.127699 252 | 0.113396 253 | 0.172357 254 | 0.208216 255 | 0.208696 256 | 0.152226 257 | 0.187261 258 | 0.204583 259 | 0.198666 260 | 0.153365 261 | 0.207813 262 | 0.177055 263 | 0.206473 264 | 0.193532 265 | 0.167372 266 | 0.223329 267 | 0.109095 268 | 0.211235 269 | 0.145639 270 | 0.122749 271 | 0.183575 272 | 0.195766 273 | 0.177277 274 | 0.180312 275 | 0.173237 276 | 0.221061 277 | 0.167833 278 | 0.205684 279 | 0.204696 280 | 0.204007 281 | 0.191160 282 | 0.223709 283 | 0.192926 284 | 0.221534 285 | 0.224353 286 | 0.221948 287 | 0.052606 288 | 0.203583 289 | 0.186787 290 | 0.212213 291 | 0.116254 292 | 0.199192 293 | 0.219754 294 | 0.219170 295 | 0.172531 296 | 0.162764 297 | 0.189735 298 | 0.185201 299 | 0.107631 300 | 0.126920 301 | 0.203914 302 | 0.185510 303 | 0.191445 304 | 0.205957 305 | 0.151720 306 | 0.193022 307 | 0.197351 308 | 0.224282 309 | 0.128297 310 | 0.218038 311 | 0.180742 312 | 0.090990 313 | 0.200931 314 | 0.217701 315 | 0.218567 316 | 0.210329 317 | 0.225048 318 | 0.144088 319 | 0.177167 320 | 0.200638 321 | 0.172118 322 | 0.214581 323 | 0.157628 324 | 0.200223 325 | 0.204057 326 | 0.205849 327 | 0.222503 328 | 0.213506 329 | 0.202002 330 | 0.152566 331 | 0.111908 332 | 0.214805 333 | 0.212034 334 | 0.180645 335 | 0.106024 336 | 0.213042 337 | 0.223584 338 | 0.187259 339 | 0.122161 340 | 0.224153 341 | 0.111300 342 | 0.223422 343 | 0.225073 344 | 0.130582 345 | 0.123337 346 | 0.138071 347 | 0.191991 348 | 0.205178 349 | 0.216982 350 | 0.218840 351 | 0.164606 352 | 0.164392 353 | 0.120474 354 | 0.113532 355 | 0.183050 356 | 0.216804 357 | 0.221632 358 | 0.214317 359 | 0.190077 360 | 0.222869 361 | 0.193916 362 | 0.204717 363 | 0.210547 364 | 0.219993 365 | 0.109608 366 | 0.206418 367 | 0.161453 368 | 0.186168 369 | 0.168419 370 | 0.190920 371 | 0.212937 372 | 0.207607 373 | 0.222115 374 | 0.220268 375 | 0.217951 376 | 0.162856 377 | 0.190378 378 | 0.216544 379 | 0.160459 380 | 0.219340 381 | 0.208021 382 | 0.157513 383 | 0.146422 384 | 0.156624 385 | 0.202615 386 | 0.190334 387 | 0.141603 388 | 0.159062 389 | 0.224391 390 | 0.144198 391 | 0.198581 392 | 0.192911 393 | 0.198589 394 | 0.116883 395 | 0.215827 396 | 0.138287 397 | 0.168176 398 | 0.219070 399 | 0.153364 400 | 0.167579 401 | 0.216689 402 | 0.191480 403 | 0.190167 404 | 0.175881 405 | 0.219562 406 | 0.215424 407 | 0.201727 408 | 0.218232 409 | 0.147570 410 | 0.187206 411 | 0.168283 412 | 0.201000 413 | 0.159293 414 | 0.201905 415 | 0.070086 416 | 0.119867 417 | 0.199252 418 | 0.126961 419 | 0.215771 420 | 0.213092 421 | 0.175949 422 | 0.199558 423 | 0.175203 424 | 0.117052 425 | 0.216055 426 | 0.144966 427 | 0.199204 428 | 0.115379 429 | 0.136635 430 | 0.186863 431 | 0.144480 432 | 0.196918 433 | 0.223952 434 | 0.204778 435 | 0.223728 436 | 0.087970 437 | 0.193230 438 | 0.215891 439 | 0.212791 440 | 0.208529 441 | 0.198154 442 | 0.165740 443 | 0.161234 444 | 0.195313 445 | 0.194684 446 | 0.203186 447 | 0.203444 448 | 0.207443 449 | 0.191921 450 | 0.216936 451 | 0.182662 452 | 0.197265 453 | 0.163843 454 | 0.159855 455 | 0.073886 456 | 0.110821 457 | 0.149798 458 | 0.161339 459 | 0.191481 460 | 0.147495 461 | 0.223590 462 | 0.056819 463 | 0.150208 464 | 0.107237 465 | 0.216016 466 | 0.224709 467 | 0.217404 468 | 0.201795 469 | 0.106529 470 | 0.112538 471 | 0.117014 472 | 0.176505 473 | 0.218574 474 | 0.181393 475 | 0.170685 476 | 0.183115 477 | 0.215719 478 | 0.152985 479 | 0.142509 480 | 0.224760 481 | 0.191322 482 | 0.196806 483 | 0.208243 484 | 0.224057 485 | 0.170115 486 | 0.156908 487 | 0.184730 488 | 0.180321 489 | 0.218511 490 | 0.171034 491 | 0.203138 492 | 0.133751 493 | 0.118485 494 | 0.055344 495 | 0.133120 496 | 0.206518 497 | 0.140282 498 | 0.199280 499 | 0.220111 500 | 0.199947 501 | 0.112613 502 | 0.212363 503 | 0.207563 504 | 0.217612 505 | 0.159734 506 | 0.218064 507 | 0.103605 508 | 0.177729 509 | 0.170625 510 | 0.216436 511 | 0.130868 512 | 0.211497 513 | 0.148482 514 | 0.127903 515 | 0.061588 516 | 0.149219 517 | 0.211788 518 | 0.088783 519 | 0.195560 520 | 0.224744 521 | 0.191067 522 | 0.195226 523 | 0.186056 524 | 0.173069 525 | 0.194328 526 | 0.205311 527 | 0.180062 528 | 0.137199 529 | 0.188230 530 | 0.198981 531 | 0.224954 532 | 0.207912 533 | 0.156956 534 | 0.155390 535 | 0.193728 536 | 0.190106 537 | 0.181430 538 | 0.152432 539 | 0.185194 540 | 0.178394 541 | 0.167584 542 | 0.159025 543 | 0.215540 544 | 0.223029 545 | 0.181254 546 | 0.211603 547 | 0.147359 548 | 0.184359 549 | 0.191919 550 | 0.148298 551 | 0.198497 552 | 0.173806 553 | 0.195783 554 | 0.170308 555 | 0.082603 556 | 0.160221 557 | 0.199294 558 | 0.224542 559 | 0.170680 560 | 0.162528 561 | 0.106073 562 | 0.144980 563 | 0.190547 564 | 0.192280 565 | 0.188036 566 | 0.137318 567 | 0.213316 568 | 0.215998 569 | 0.114412 570 | 0.199997 571 | 0.120637 572 | 0.157660 573 | 0.215691 574 | 0.167124 575 | 0.127118 576 | 0.215775 577 | 0.186355 578 | 0.210318 579 | 0.219995 580 | 0.130824 581 | 0.180007 582 | 0.188814 583 | 0.220250 584 | 0.201136 585 | 0.224108 586 | 0.157861 587 | 0.195583 588 | 0.204536 589 | 0.178958 590 | 0.167099 591 | 0.194382 592 | 0.222496 593 | 0.125080 594 | 0.198818 595 | 0.207962 596 | 0.148004 597 | 0.134249 598 | 0.085237 599 | 0.176246 600 | 0.114648 601 | 0.182457 602 | 0.119359 603 | 0.177448 604 | 0.205466 605 | 0.214301 606 | 0.193487 607 | 0.142904 608 | 0.151919 609 | 0.151600 610 | 0.145621 611 | 0.215232 612 | 0.119410 613 | 0.188737 614 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /src/als_ros/src/evaluator.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | typedef struct { 28 | double x, y; 29 | } Point; 30 | 31 | class Evaluator { 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber gtPoseSub_, reliabilitySub_, glPosesSub_, scanSub_; 35 | std::string mapFrame_, laserFrame_; 36 | tf::TransformListener tfListener_; 37 | std::vector gtPoses_; 38 | std::vector glPoses_; 39 | std::vector scans_; 40 | bool canUpdateGTPoses_, canUpdateGLPoses_, canUpdateScan_; 41 | bool saveGTPose_, saveGLPoses_, saveScan_; 42 | 43 | public: 44 | Evaluator(void): 45 | nh_("~"), 46 | mapFrame_("map"), 47 | laserFrame_("laser"), 48 | canUpdateGTPoses_(true), 49 | canUpdateGLPoses_(true), 50 | canUpdateScan_(true), 51 | tfListener_() 52 | { 53 | gtPoseSub_ = nh_.subscribe("/scanned_ground_truth_pose", 1, &Evaluator::gtPoseCB, this); 54 | reliabilitySub_ = nh_.subscribe("/reliability", 1, &Evaluator::reliabilityCB, this); 55 | glPosesSub_ = nh_.subscribe("/gl_sampled_poses", 1, &Evaluator::glPosesCB, this); 56 | scanSub_ = nh_.subscribe("/scan", 1, &Evaluator::scanCB, this); 57 | saveGTPose_ = false; 58 | saveGLPoses_ = false; 59 | saveScan_ = false; 60 | } 61 | 62 | void spin(void) { 63 | ros::spin(); 64 | } 65 | 66 | void gtPoseCB(const geometry_msgs::PoseStamped::ConstPtr &msg) { 67 | if (!canUpdateGTPoses_) 68 | return; 69 | gtPoses_.insert(gtPoses_.begin(), *msg); 70 | if (gtPoses_.size() >= 100) 71 | gtPoses_.resize(100); 72 | } 73 | 74 | int getSynchronizedGTPoses(double time) { 75 | for (int i = 0; i < (int)gtPoses_.size(); ++i) { 76 | double t = gtPoses_[i].header.stamp.toSec(); 77 | if (t < time) 78 | return i; 79 | } 80 | return -1; 81 | } 82 | 83 | void glPosesCB(const geometry_msgs::PoseArray::ConstPtr &msg) { 84 | if (!canUpdateGLPoses_) 85 | return; 86 | glPoses_.insert(glPoses_.begin(), *msg); 87 | if (glPoses_.size() >= 100) 88 | glPoses_.resize(100); 89 | } 90 | 91 | int getSynchronizedGLPoses(double time) { 92 | for (int i = 0; i < (int)glPoses_.size(); ++i) { 93 | double t = glPoses_[i].header.stamp.toSec(); 94 | if (t < time) 95 | return i; 96 | } 97 | return -1; 98 | } 99 | 100 | void scanCB(const sensor_msgs::LaserScan::ConstPtr &msg) { 101 | if (!canUpdateScan_) 102 | return; 103 | scans_.insert(scans_.begin(), *msg); 104 | if (scans_.size() >= 100) 105 | scans_.resize(100); 106 | } 107 | 108 | int getSynchronizedScan(double time) { 109 | for (int i = 0; i < (int)scans_.size(); ++i) { 110 | double t = scans_[i].header.stamp.toSec(); 111 | if (t < time) 112 | return i; 113 | } 114 | return -1; 115 | } 116 | 117 | std::vector makeArrowPoints(double x, double y, double yaw, double len) { 118 | Point p0, p1, p2, p3; 119 | p0.x = x; 120 | p0.y = y; 121 | p1.x = p0.x + len * cos(yaw); 122 | p1.y = p0.y + len * sin(yaw); 123 | p2.x = p1.x + len / 3.0 * cos(yaw + 135.0 * M_PI / 180.0); 124 | p2.y = p1.y + len / 3.0 * sin(yaw + 135.0 * M_PI / 180.0); 125 | p3.x = p1.x + len / 3.0 * cos(yaw - 135.0 * M_PI / 180.0); 126 | p3.y = p1.y + len / 3.0 * sin(yaw - 135.0 * M_PI / 180.0); 127 | 128 | std::vector arrowPoints; 129 | arrowPoints.push_back(p0); 130 | arrowPoints.push_back(p1); 131 | arrowPoints.push_back(p2); 132 | arrowPoints.push_back(p3); 133 | arrowPoints.push_back(p1); 134 | return arrowPoints; 135 | } 136 | 137 | void reliabilityCB(const geometry_msgs::Vector3Stamped::ConstPtr &msg) { 138 | static bool isFirst = true; 139 | static double firstTime; 140 | static FILE *fp = fopen("/tmp/als_ros_reliability.txt", "w"); 141 | double time = msg->header.stamp.toSec(); 142 | if (isFirst) { 143 | firstTime = time; 144 | isFirst = false; 145 | } 146 | 147 | geometry_msgs::PoseStamped gtPose; 148 | if (saveGTPose_) { 149 | canUpdateGTPoses_ = false; 150 | int gtPoseIdx = getSynchronizedGTPoses(time); 151 | if (gtPoseIdx < 0) { 152 | canUpdateGTPoses_ = true; 153 | return; 154 | } 155 | gtPose = gtPoses_[gtPoseIdx]; 156 | canUpdateGTPoses_ = true; 157 | } 158 | 159 | geometry_msgs::PoseArray glPoses; 160 | if (saveGLPoses_) { 161 | canUpdateGLPoses_ = false; 162 | int glPoseIdx = getSynchronizedGLPoses(time); 163 | if (glPoseIdx < 0) { 164 | canUpdateGLPoses_ = true; 165 | return; 166 | } 167 | glPoses = glPoses_[glPoseIdx]; 168 | canUpdateGLPoses_ = true; 169 | } 170 | 171 | sensor_msgs::LaserScan scan; 172 | if (saveScan_) { 173 | canUpdateScan_ = false; 174 | int scanIdx = getSynchronizedScan(time); 175 | if (scanIdx < 0) { 176 | canUpdateScan_ = true; 177 | return; 178 | } 179 | scan = scans_[scanIdx]; 180 | canUpdateScan_ = true; 181 | } 182 | 183 | tf::StampedTransform tfMap2Laser; 184 | try { 185 | ros::Time now = msg->header.stamp; 186 | tfListener_.waitForTransform(mapFrame_, laserFrame_, now, ros::Duration(0.2)); 187 | tfListener_.lookupTransform(mapFrame_, laserFrame_, now, tfMap2Laser); 188 | } catch (tf::TransformException ex) { 189 | return; 190 | } 191 | 192 | double gtX, gtY, gtYaw; 193 | if (saveGTPose_) { 194 | tf::Quaternion gtQuat(gtPose.pose.orientation.x, 195 | gtPose.pose.orientation.y, 196 | gtPose.pose.orientation.z, 197 | gtPose.pose.orientation.w); 198 | double gtRoll, gtPitch; 199 | tf::Matrix3x3 gtRotMat(gtQuat); 200 | gtRotMat.getRPY(gtRoll, gtPitch, gtYaw); 201 | gtX = gtPose.pose.position.x; 202 | gtY = gtPose.pose.position.y; 203 | } 204 | 205 | tf::Quaternion quat(tfMap2Laser.getRotation().x(), 206 | tfMap2Laser.getRotation().y(), 207 | tfMap2Laser.getRotation().z(), 208 | tfMap2Laser.getRotation().w()); 209 | double roll, pitch, yaw; 210 | tf::Matrix3x3 rotMat(quat); 211 | rotMat.getRPY(roll, pitch, yaw); 212 | double x = tfMap2Laser.getOrigin().x(); 213 | double y = tfMap2Laser.getOrigin().y(); 214 | 215 | if (saveGTPose_) { 216 | double dx = gtX - x; 217 | double dy = gtY - y; 218 | double dl = sqrt(dx * dx + dy * dy); 219 | double dyaw = gtYaw - yaw; 220 | while (dyaw < -M_PI) 221 | dyaw += 2.0 * M_PI; 222 | while (dyaw > M_PI) 223 | dyaw -= 2.0 * M_PI; 224 | printf("%.2lf [sec], %.3lf [m], %.3lf [m], %.3lf [m], %.3lf [deg], %lf %lf %lf\n", 225 | time - firstTime, dx, dy, dl, dyaw * 180.0 / M_PI, msg->vector.x, msg->vector.y, msg->vector.z); 226 | 227 | fprintf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", 228 | time - firstTime, gtX, gtY, gtYaw, x, y, yaw, 229 | dx, dy, dl, dyaw, msg->vector.x, msg->vector.y, msg->vector.z); 230 | } else { 231 | printf("%.2lf [sec], %.3lf [m], %.3lf [m], %.3lf [deg], %lf %lf %lf\n", 232 | time - firstTime, x, y, yaw * 180.0 / M_PI, 233 | msg->vector.x, msg->vector.y, msg->vector.z); 234 | 235 | fprintf(fp, "%lf %lf %lf %lf %lf %lf %lf\n", 236 | time - firstTime, x, y, yaw, 237 | msg->vector.x, msg->vector.y, msg->vector.z); 238 | } 239 | 240 | if (saveGLPoses_) { 241 | FILE *fp2; 242 | fp2 = fopen("/tmp/als_ros_scaned_ground_truth.txt", "w"); 243 | std::vector gtArrowPoints = makeArrowPoints(gtX, gtY, gtYaw, 1.0); 244 | for (int i = 0; i < (int)gtArrowPoints.size(); ++i) 245 | fprintf(fp2, "%lf %lf\n", gtArrowPoints[i].x, gtArrowPoints[i].y); 246 | fprintf(fp2, "\n"); 247 | fclose(fp2); 248 | 249 | fp2 = fopen("/tmp/als_ros_mcl_estimate.txt", "w"); 250 | std::vector mclArrowPoints = makeArrowPoints(x, y, yaw, 1.0); 251 | for (int i = 0; i < (int)mclArrowPoints.size(); ++i) 252 | fprintf(fp2, "%lf %lf\n", mclArrowPoints[i].x, mclArrowPoints[i].y); 253 | fprintf(fp2, "\n"); 254 | fclose(fp2); 255 | 256 | double tmpYaw = 0.0, estX = 0.0, estY = 0.0, estYaw = 0.0; 257 | double wo = 1.0 / (double)glPoses.poses.size(); 258 | fp2 = fopen("/tmp/als_ros_gl_sampled_poses.txt", "w"); 259 | for (int i = 0; i < (int)glPoses.poses.size(); ++i) { 260 | tf::Quaternion quat(glPoses.poses[i].orientation.x, 261 | glPoses.poses[i].orientation.y, 262 | glPoses.poses[i].orientation.z, 263 | glPoses.poses[i].orientation.w); 264 | double roll, pitch, yaw; 265 | tf::Matrix3x3 rotMat(quat); 266 | rotMat.getRPY(roll, pitch, yaw); 267 | double x = glPoses.poses[i].position.x; 268 | double y = glPoses.poses[i].position.y; 269 | std::vector arrowPoints = makeArrowPoints(x, y, yaw, 1.0); 270 | for (int j = 0; j < (int)arrowPoints.size(); ++j) 271 | fprintf(fp2, "%lf %lf\n", arrowPoints[j].x, arrowPoints[j].y); 272 | fprintf(fp2, "\n"); 273 | 274 | estX += x * wo; 275 | estY += y * wo; 276 | double dyaw = tmpYaw - yaw; 277 | while (dyaw < -M_PI) 278 | dyaw += 2.0 * M_PI; 279 | while (dyaw > M_PI) 280 | dyaw -= 2.0 * M_PI; 281 | estYaw += dyaw * wo; 282 | } 283 | fclose(fp2); 284 | 285 | estYaw = tmpYaw - estYaw; 286 | static FILE *fpGT = fopen("/tmp/als_ros_scaned_ground_truth_poses.txt", "w"); 287 | static FILE *fpEst = fopen("/tmp/als_ros_mcl_poses.txt", "w"); 288 | static FILE *fpGLAve = fopen("/tmp/als_ros_gl_sampled_poses_ave.txt", "w"); 289 | fprintf(fpGT, "%lf %lf %lf\n", gtX, gtY, gtYaw); 290 | fprintf(fpEst, "%lf %lf %lf\n", x, y, yaw); 291 | fprintf(fpGLAve, "%lf %lf %lf\n", estX, estY, estYaw); 292 | } 293 | 294 | if (saveScan_) { 295 | FILE *fp2 = fopen("/tmp/als_ros_scan_points.txt", "w"); 296 | for (int i = 0; i < (int)scan.ranges.size(); ++i) { 297 | double r = scan.ranges[i]; 298 | if (r < scan.range_min || scan.range_max < r) 299 | continue; 300 | double t = (double)i * scan.angle_increment + scan.angle_min + gtYaw; 301 | double x = r * cos(t) + gtX; 302 | double y = r * sin(t) + gtY; 303 | fprintf(fp2, "%lf %lf\n", x, y); 304 | } 305 | fclose(fp2); 306 | } 307 | } 308 | }; // class Evaluator 309 | 310 | int main(int argc, char **argv) { 311 | ros::init(argc, argv, "evaluator"); 312 | Evaluator node; 313 | node.spin(); 314 | return 0; 315 | } 316 | -------------------------------------------------------------------------------- /rviz/als_ros.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.5 9 | Tree Height: 545 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Scan 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 0.699999988079071 55 | Class: rviz/Map 56 | Color Scheme: map 57 | Draw Behind: false 58 | Enabled: true 59 | Name: Map 60 | Topic: /map 61 | Unreliable: false 62 | Use Timestamp: false 63 | Value: true 64 | - Class: rviz/TF 65 | Enabled: true 66 | Frame Timeout: 15 67 | Frames: 68 | All Enabled: false 69 | base_link: 70 | Value: true 71 | ground_truth: 72 | Value: true 73 | laser: 74 | Value: false 75 | map: 76 | Value: false 77 | odom: 78 | Value: true 79 | scanned_ground_truth: 80 | Value: true 81 | world: 82 | Value: true 83 | Marker Alpha: 1 84 | Marker Scale: 5 85 | Name: TF 86 | Show Arrows: false 87 | Show Axes: true 88 | Show Names: true 89 | Tree: 90 | world: 91 | map: 92 | ground_truth: 93 | {} 94 | odom: 95 | base_link: 96 | laser: 97 | {} 98 | scanned_ground_truth: 99 | {} 100 | Update Interval: 0 101 | Value: true 102 | - Alpha: 1 103 | Arrow Length: 1 104 | Axes Length: 0.30000001192092896 105 | Axes Radius: 0.009999999776482582 106 | Class: rviz/PoseArray 107 | Color: 0; 0; 255 108 | Enabled: true 109 | Head Length: 0.07000000029802322 110 | Head Radius: 0.029999999329447746 111 | Name: MCL particles 112 | Queue Size: 10 113 | Shaft Length: 0.23000000417232513 114 | Shaft Radius: 0.009999999776482582 115 | Shape: Arrow (Flat) 116 | Topic: /mcl_particles 117 | Unreliable: false 118 | Value: true 119 | - Alpha: 1 120 | Autocompute Intensity Bounds: true 121 | Autocompute Value Bounds: 122 | Max Value: 10 123 | Min Value: -10 124 | Value: true 125 | Axis: Z 126 | Channel Name: intensity 127 | Class: rviz/LaserScan 128 | Color: 239; 41; 41 129 | Color Transformer: FlatColor 130 | Decay Time: 0 131 | Enabled: true 132 | Invert Rainbow: false 133 | Max Color: 255; 255; 255 134 | Min Color: 0; 0; 0 135 | Name: Scan 136 | Position Transformer: XYZ 137 | Queue Size: 10 138 | Selectable: true 139 | Size (Pixels): 5 140 | Size (m): 0.009999999776482582 141 | Style: Points 142 | Topic: /scan 143 | Unreliable: false 144 | Use Fixed Frame: true 145 | Use rainbow: true 146 | Value: true 147 | - Alpha: 1 148 | Autocompute Intensity Bounds: true 149 | Autocompute Value Bounds: 150 | Max Value: 10 151 | Min Value: -10 152 | Value: true 153 | Axis: Z 154 | Channel Name: intensity 155 | Class: rviz/PointCloud 156 | Color: 255; 255; 255 157 | Color Transformer: Intensity 158 | Decay Time: 0 159 | Enabled: false 160 | Invert Rainbow: false 161 | Max Color: 255; 255; 255 162 | Min Color: 0; 0; 0 163 | Name: Scan points 164 | Position Transformer: XYZ 165 | Queue Size: 10 166 | Selectable: true 167 | Size (Pixels): 3 168 | Size (m): 0.009999999776482582 169 | Style: Points 170 | Topic: /scan_points 171 | Unreliable: false 172 | Use Fixed Frame: true 173 | Use rainbow: true 174 | Value: false 175 | - Alpha: 1 176 | Autocompute Intensity Bounds: true 177 | Autocompute Value Bounds: 178 | Max Value: 10 179 | Min Value: -10 180 | Value: true 181 | Axis: Z 182 | Channel Name: intensity 183 | Class: rviz/LaserScan 184 | Color: 252; 233; 79 185 | Color Transformer: FlatColor 186 | Decay Time: 0 187 | Enabled: true 188 | Invert Rainbow: false 189 | Max Color: 255; 255; 255 190 | Min Color: 0; 0; 0 191 | Name: Unknown scan 192 | Position Transformer: XYZ 193 | Queue Size: 15 194 | Selectable: true 195 | Size (Pixels): 15 196 | Size (m): 0.009999999776482582 197 | Style: Points 198 | Topic: /unknown_scan 199 | Unreliable: false 200 | Use Fixed Frame: true 201 | Use rainbow: true 202 | Value: true 203 | - Class: rviz/Marker 204 | Enabled: true 205 | Marker Topic: /reliability_marker_name 206 | Name: Reliability marker 207 | Namespaces: 208 | {} 209 | Queue Size: 100 210 | Value: true 211 | - Alpha: 1 212 | Autocompute Intensity Bounds: true 213 | Autocompute Value Bounds: 214 | Max Value: 10 215 | Min Value: -10 216 | Value: true 217 | Axis: Z 218 | Channel Name: intensity 219 | Class: rviz/LaserScan 220 | Color: 239; 41; 41 221 | Color Transformer: FlatColor 222 | Decay Time: 0 223 | Enabled: false 224 | Invert Rainbow: false 225 | Max Color: 255; 255; 255 226 | Min Color: 0; 0; 0 227 | Name: Aligned scan 228 | Position Transformer: XYZ 229 | Queue Size: 10 230 | Selectable: true 231 | Size (Pixels): 7 232 | Size (m): 0.009999999776482582 233 | Style: Points 234 | Topic: /aligned_scan_mrf 235 | Unreliable: false 236 | Use Fixed Frame: true 237 | Use rainbow: true 238 | Value: false 239 | - Alpha: 1 240 | Autocompute Intensity Bounds: true 241 | Autocompute Value Bounds: 242 | Max Value: 10 243 | Min Value: -10 244 | Value: true 245 | Axis: Z 246 | Channel Name: intensity 247 | Class: rviz/LaserScan 248 | Color: 0; 0; 255 249 | Color Transformer: FlatColor 250 | Decay Time: 0 251 | Enabled: false 252 | Invert Rainbow: false 253 | Max Color: 255; 255; 255 254 | Min Color: 0; 0; 0 255 | Name: Misaligned scan 256 | Position Transformer: XYZ 257 | Queue Size: 10 258 | Selectable: true 259 | Size (Pixels): 7 260 | Size (m): 0.009999999776482582 261 | Style: Points 262 | Topic: /misaligned_scan_mrf 263 | Unreliable: false 264 | Use Fixed Frame: true 265 | Use rainbow: true 266 | Value: false 267 | - Alpha: 1 268 | Autocompute Intensity Bounds: true 269 | Autocompute Value Bounds: 270 | Max Value: 10 271 | Min Value: -10 272 | Value: true 273 | Axis: Z 274 | Channel Name: intensity 275 | Class: rviz/LaserScan 276 | Color: 0; 255; 0 277 | Color Transformer: FlatColor 278 | Decay Time: 0 279 | Enabled: false 280 | Invert Rainbow: false 281 | Max Color: 255; 255; 255 282 | Min Color: 0; 0; 0 283 | Name: Unknown scan 284 | Position Transformer: XYZ 285 | Queue Size: 10 286 | Selectable: true 287 | Size (Pixels): 7 288 | Size (m): 0.009999999776482582 289 | Style: Points 290 | Topic: /unknown_scan_mrf 291 | Unreliable: false 292 | Use Fixed Frame: true 293 | Use rainbow: true 294 | Value: false 295 | - Class: rviz/Marker 296 | Enabled: false 297 | Marker Topic: /failure_probability_marker_name 298 | Name: Failure probability marker 299 | Namespaces: 300 | {} 301 | Queue Size: 100 302 | Value: false 303 | - Alpha: 1 304 | Arrow Length: 1.5 305 | Axes Length: 0.30000001192092896 306 | Axes Radius: 0.009999999776482582 307 | Class: rviz/PoseArray 308 | Color: 92; 53; 102 309 | Enabled: true 310 | Head Length: 0.07000000029802322 311 | Head Radius: 0.029999999329447746 312 | Name: GL sampled poses 313 | Queue Size: 10 314 | Shaft Length: 0.23000000417232513 315 | Shaft Radius: 0.009999999776482582 316 | Shape: Arrow (Flat) 317 | Topic: /gl_sampled_poses 318 | Unreliable: false 319 | Value: true 320 | - Class: rviz/Marker 321 | Enabled: true 322 | Marker Topic: /gl_sdf_keypoints 323 | Name: SDF keypoints 324 | Namespaces: 325 | {} 326 | Queue Size: 100 327 | Value: true 328 | - Alpha: 0.699999988079071 329 | Class: rviz/Map 330 | Color Scheme: map 331 | Draw Behind: false 332 | Enabled: false 333 | Name: GL local map 334 | Topic: /gl_local_map 335 | Unreliable: false 336 | Use Timestamp: false 337 | Value: false 338 | - Class: rviz/Marker 339 | Enabled: false 340 | Marker Topic: /gl_local_sdf_keypoints 341 | Name: Local SDF keypoints 342 | Namespaces: 343 | {} 344 | Queue Size: 100 345 | Value: false 346 | Enabled: true 347 | Global Options: 348 | Background Color: 48; 48; 48 349 | Default Light: true 350 | Fixed Frame: map 351 | Frame Rate: 30 352 | Name: root 353 | Tools: 354 | - Class: rviz/Interact 355 | Hide Inactive Objects: true 356 | - Class: rviz/MoveCamera 357 | - Class: rviz/Select 358 | - Class: rviz/FocusCamera 359 | - Class: rviz/Measure 360 | - Class: rviz/SetInitialPose 361 | Theta std deviation: 0.2617993950843811 362 | Topic: /initialpose 363 | X std deviation: 0.5 364 | Y std deviation: 0.5 365 | - Class: rviz/SetGoal 366 | Topic: /move_base_simple/goal 367 | - Class: rviz/PublishPoint 368 | Single click: true 369 | Topic: /clicked_point 370 | Value: true 371 | Views: 372 | Current: 373 | Angle: 0 374 | Class: rviz/TopDownOrtho 375 | Enable Stereo Rendering: 376 | Stereo Eye Separation: 0.05999999865889549 377 | Stereo Focal Distance: 1 378 | Swap Stereo Eyes: false 379 | Value: false 380 | Invert Z Axis: false 381 | Name: Current View 382 | Near Clip Distance: 0.009999999776482582 383 | Scale: 26.91925621032715 384 | Target Frame: base_link 385 | X: 0 386 | Y: 0 387 | Saved: ~ 388 | Window Geometry: 389 | Displays: 390 | collapsed: false 391 | Height: 842 392 | Hide Left Dock: false 393 | Hide Right Dock: false 394 | QMainWindow State: 000000ff00000000fd000000040000000000000199000002acfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb0000040d00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ac000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001800460072006f006e0074002000630061006d006500720061000000022d000000bc000000000000000000000001000001b0000002acfc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0049006d006100670065000000003d000000c20000000000000000fb0000000a00560069006500770073010000003d000002ac000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000009b8000001cffc0100000002fb0000001800460072006f006e0074002000630061006d0065007200610100000000000009b80000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056a0000003efc0100000002fb0000000800540069006d006501000000000000056a000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000215000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 395 | Selection: 396 | collapsed: false 397 | Time: 398 | collapsed: false 399 | Tool Properties: 400 | collapsed: false 401 | Views: 402 | collapsed: false 403 | Width: 1386 404 | X: 534 405 | Y: 150 406 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/MAEClassifier.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __MAE_CLASSIFIER_H__ 21 | #define __MAE_CLASSIFIER_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace als_ros { 32 | 33 | class MAEClassifier { 34 | private: 35 | std::string classifiersDir_; 36 | double maxResidualError_; 37 | 38 | double failureThreshold_; 39 | double successMAEMean_, successMAEStd_; 40 | double failureMAEMean_, failureMAEStd_; 41 | 42 | int truePositiveNum_, falsePositiveNum_, trueNegativeNum_, falseNegativeNum_; 43 | double dTruePositive_, dFalsePositive_, dTrueNegative_, dFalseNegative_; 44 | 45 | double maeHistogramBinWidth_; 46 | Histogram positiveMAEHistogram_, negativeMAEHistogram_; 47 | Histogram truePositiveMAEHistogram_, trueNegativeMAEHistogram_; 48 | Histogram falsePositiveMAEHistogram_, falseNegativeMAEHistogram_; 49 | 50 | template > class Container> double getMean(Container &x) { 51 | return std::accumulate(x.begin(), x.end(), 0.0) / x.size(); 52 | } 53 | 54 | template > class Container> double getVar(Container &x) { 55 | double size = x.size(); 56 | double mean = getMean(x); 57 | return (std::inner_product(x.begin(), x.end(), x.begin(), 0.0) - mean * mean * size) / (size - 1.0); 58 | } 59 | 60 | template > class Container> double getStd(Container &x) { 61 | return std::sqrt(getVar(x)); 62 | } 63 | 64 | std::vector readMAEs(std::string filePath) { 65 | FILE *fp = fopen(filePath.c_str(), "r"); 66 | if (fp == NULL) { 67 | fprintf(stderr, "cannot open %s\n", filePath.c_str()); 68 | exit(1); 69 | } 70 | double mae; 71 | std::vector maes; 72 | while (fscanf(fp, "%lf", &mae) != EOF) 73 | maes.push_back(mae); 74 | fclose(fp); 75 | return maes; 76 | } 77 | 78 | public: 79 | MAEClassifier(void): 80 | maxResidualError_(1.0), 81 | maeHistogramBinWidth_(0.01) {} 82 | 83 | inline void setClassifierDir(std::string classifiersDir) { classifiersDir_ = classifiersDir; } 84 | inline void setMaxResidualError(double maxResidualError) { maxResidualError_ = maxResidualError; } 85 | inline void setMAEHistogramBinWidth(double maeHistogramBinWidth) { maeHistogramBinWidth_ = maeHistogramBinWidth; } 86 | 87 | inline double getFailureThreshold(void) { return failureThreshold_; } 88 | 89 | double getMAE(std::vector residualErrors) { 90 | double sum = 0.0; 91 | int num = 0; 92 | for (size_t i = 0; i < residualErrors.size(); i++) { 93 | if (0.0 <= residualErrors[i] && residualErrors[i] <= maxResidualError_) { 94 | sum += residualErrors[i]; 95 | num++; 96 | } 97 | } 98 | if (num == 0) 99 | return 0.0; 100 | else 101 | return sum / (double)num; 102 | } 103 | 104 | void learnThreshold(std::vector> trainSuccessResidualErrors, std::vector> trainFailureResidualErrors) { 105 | std::vector successMAEs((int)trainSuccessResidualErrors.size()); 106 | std::vector failureMAEs((int)trainFailureResidualErrors.size()); 107 | for (int i = 0; i < (int)trainSuccessResidualErrors.size(); ++i) 108 | successMAEs[i] = getMAE(trainSuccessResidualErrors[i]); 109 | for (int i = 0; i < (int)trainFailureResidualErrors.size(); ++i) 110 | failureMAEs[i] = getMAE(trainFailureResidualErrors[i]); 111 | 112 | successMAEMean_ = getMean(successMAEs); 113 | successMAEStd_ = getStd(successMAEs); 114 | failureMAEMean_ = getMean(failureMAEs); 115 | failureMAEStd_ = getStd(failureMAEs); 116 | 117 | bool isFirst = true; 118 | double maxAcc; 119 | int totalNum = (int)trainSuccessResidualErrors.size() + (int)trainFailureResidualErrors.size(); 120 | for (double th = successMAEMean_; th <= failureMAEMean_; th += 0.005) { 121 | int correctNum = 0; 122 | for (int i = 0; i < (int)trainSuccessResidualErrors.size(); ++i) { 123 | if (successMAEs[i] <= th) 124 | correctNum++; 125 | } 126 | for (int i = 0; i < (int)trainFailureResidualErrors.size(); ++i) { 127 | if (failureMAEs[i] > th) 128 | correctNum++; 129 | } 130 | double acc = (double)correctNum / (double)totalNum; 131 | printf("maeTH = %lf [m], accuracy = %lf [%%]\n", th, acc * 100.0); 132 | if (isFirst) { 133 | failureThreshold_ = th; 134 | maxAcc = acc; 135 | isFirst = false; 136 | } else if (maxAcc < acc) { 137 | failureThreshold_ = th; 138 | maxAcc = acc; 139 | } 140 | } 141 | } 142 | 143 | void writeClassifierParams(std::vector> testSuccessResidualErrors, std::vector> testFailureResidualErrors) { 144 | std::string mkdirCmd = "mkdir -p " + classifiersDir_; 145 | int retVal = system(mkdirCmd.c_str()); 146 | 147 | std::string positiveMAEsFileName = classifiersDir_ + "positive_maes.txt"; 148 | std::string negativeMAEsFileName = classifiersDir_ + "negative_maes.txt"; 149 | std::string truePositiveMAEsFileName = classifiersDir_ + "true_positive_maes.txt"; 150 | std::string trueNegativeMAEsFileName = classifiersDir_ + "true_negative_maes.txt"; 151 | std::string falsePositiveMAEsFileName = classifiersDir_ + "false_positive_maes.txt"; 152 | std::string falseNegativeMAEsFileName = classifiersDir_ + "false_negative_maes.txt"; 153 | FILE *fpPositive = fopen(positiveMAEsFileName.c_str(), "w"); 154 | FILE *fpNegative = fopen(negativeMAEsFileName.c_str(), "w"); 155 | FILE *fpTruePositive = fopen(truePositiveMAEsFileName.c_str(), "w"); 156 | FILE *fpTrueNegative = fopen(trueNegativeMAEsFileName.c_str(), "w"); 157 | FILE *fpFalsePositive = fopen(falsePositiveMAEsFileName.c_str(), "w"); 158 | FILE *fpFalseNegative = fopen(falseNegativeMAEsFileName.c_str(), "w"); 159 | 160 | truePositiveNum_ = falseNegativeNum_ = falsePositiveNum_ = trueNegativeNum_ = 0; 161 | for (size_t i = 0; i < testSuccessResidualErrors.size(); i++) { 162 | double mae = getMAE(testSuccessResidualErrors[i]); 163 | fprintf(fpPositive, "%lf\n", mae); 164 | if (mae <= failureThreshold_) { 165 | truePositiveNum_++; 166 | fprintf(fpTruePositive, "%lf\n", mae); 167 | } else { 168 | falseNegativeNum_++; 169 | fprintf(fpFalseNegative, "%lf\n", mae); 170 | } 171 | } 172 | for (size_t i = 0; i < testFailureResidualErrors.size(); i++) { 173 | double mae = getMAE(testFailureResidualErrors[i]); 174 | fprintf(fpNegative, "%lf\n", mae); 175 | if (mae <= failureThreshold_) { 176 | falsePositiveNum_++; 177 | fprintf(fpFalsePositive, "%lf\n", mae); 178 | } else { 179 | trueNegativeNum_++; 180 | fprintf(fpTrueNegative, "%lf\n", mae); 181 | } 182 | } 183 | fclose(fpPositive); 184 | fclose(fpNegative); 185 | fclose(fpTruePositive); 186 | fclose(fpTrueNegative); 187 | fclose(fpFalsePositive); 188 | fclose(fpFalseNegative); 189 | 190 | printf("truePositiveNum = %d\n", truePositiveNum_); 191 | printf("falseNegativeNum = %d\n", falseNegativeNum_); 192 | printf("trueNegativeNum = %d\n", trueNegativeNum_); 193 | printf("falsePositiveNum = %d\n", falsePositiveNum_); 194 | 195 | std::string yamlFile = classifiersDir_ + "classifier.yaml"; 196 | FILE *fpYaml = fopen(yamlFile.c_str(), "w"); 197 | fprintf(fpYaml, "maxResidualError: %lf\n", maxResidualError_); 198 | fprintf(fpYaml, "maeHistogramBinWidth: %lf\n", maeHistogramBinWidth_); 199 | fprintf(fpYaml, "failureThreshold: %lf\n", failureThreshold_); 200 | fprintf(fpYaml, "successMAEMean: %lf\n", successMAEMean_); 201 | fprintf(fpYaml, "successMAEStd: %lf\n", successMAEStd_); 202 | fprintf(fpYaml, "failureMAEMean: %lf\n", failureMAEMean_); 203 | fprintf(fpYaml, "failureMAEStd: %lf\n", failureMAEStd_); 204 | fprintf(fpYaml, "truePositiveNum: %d\n", truePositiveNum_); 205 | fprintf(fpYaml, "falseNegativeNum: %d\n", falseNegativeNum_); 206 | fprintf(fpYaml, "trueNegativeNum: %d\n", trueNegativeNum_); 207 | fprintf(fpYaml, "falsePositiveNum: %d\n", falsePositiveNum_); 208 | fprintf(fpYaml, "positiveMAEsFileName: positive_maes.txt\n"); 209 | fprintf(fpYaml, "negativeMAEsFileName: negative_maes.txt\n"); 210 | fprintf(fpYaml, "truePositiveMAEsFileName: true_positive_maes.txt\n"); 211 | fprintf(fpYaml, "trueNegativeMAEsFileName: true_negative_maes.txt\n"); 212 | fprintf(fpYaml, "falsePositiveMAEsFileName: false_positive_maes.txt\n"); 213 | fprintf(fpYaml, "falseNegativeMAEsFileName: false_negative_maes.txt\n"); 214 | fclose(fpYaml); 215 | printf("yaml file for the MAE classifier was saved at %s\n", yamlFile.c_str()); 216 | } 217 | 218 | void readClassifierParams(void) { 219 | std::string yamlFile = classifiersDir_ + "classifier.yaml"; 220 | YAML::Node lconf = YAML::LoadFile(yamlFile); 221 | 222 | maxResidualError_ = lconf["maxResidualError"].as(); 223 | failureThreshold_ = lconf["failureThreshold"].as(); 224 | maeHistogramBinWidth_ = lconf["maeHistogramBinWidth"].as(); 225 | 226 | std::string positiveMAEsFilePath = classifiersDir_ + lconf["positiveMAEsFileName"].as(); 227 | std::string negativeMAEsFilePath = classifiersDir_ + lconf["negativeMAEsFileName"].as(); 228 | std::string truePositiveMAEsFilePath = classifiersDir_ + lconf["truePositiveMAEsFileName"].as(); 229 | std::string trueNegativeMAEsFilePath = classifiersDir_ + lconf["trueNegativeMAEsFileName"].as(); 230 | std::string falsePositiveMAEsFilePath = classifiersDir_ + lconf["falsePositiveMAEsFileName"].as(); 231 | std::string falseNegativeMAEsFilePath = classifiersDir_ + lconf["falseNegativeMAEsFileName"].as(); 232 | positiveMAEHistogram_ = Histogram(readMAEs(positiveMAEsFilePath), maeHistogramBinWidth_); 233 | negativeMAEHistogram_ = Histogram(readMAEs(negativeMAEsFilePath), maeHistogramBinWidth_); 234 | truePositiveMAEHistogram_ = Histogram(readMAEs(truePositiveMAEsFilePath), maeHistogramBinWidth_); 235 | trueNegativeMAEHistogram_ = Histogram(readMAEs(trueNegativeMAEsFilePath), maeHistogramBinWidth_); 236 | falsePositiveMAEHistogram_ = Histogram(readMAEs(falsePositiveMAEsFilePath), maeHistogramBinWidth_); 237 | falseNegativeMAEHistogram_ = Histogram(readMAEs(falseNegativeMAEsFilePath), maeHistogramBinWidth_); 238 | 239 | positiveMAEHistogram_.smoothHistogram(); 240 | negativeMAEHistogram_.smoothHistogram(); 241 | truePositiveMAEHistogram_.smoothHistogram(); 242 | trueNegativeMAEHistogram_.smoothHistogram(); 243 | falsePositiveMAEHistogram_.smoothHistogram(); 244 | falseNegativeMAEHistogram_.smoothHistogram(); 245 | 246 | int truePositiveNum = lconf["truePositiveNum"].as(); 247 | int falseNegativeNum = lconf["falseNegativeNum"].as(); 248 | int trueNegativeNum = lconf["trueNegativeNum"].as(); 249 | int falsePositiveNum = lconf["falsePositiveNum"].as(); 250 | dTruePositive_ = (double)truePositiveNum / (double)(truePositiveNum + falseNegativeNum); 251 | dFalsePositive_ = (double)falsePositiveNum / (double)(truePositiveNum + falseNegativeNum); 252 | dTrueNegative_ = (double)trueNegativeNum / (double)(trueNegativeNum + falsePositiveNum); 253 | dFalseNegative_ = (double)falseNegativeNum / (double)(trueNegativeNum + falsePositiveNum); 254 | } 255 | 256 | double calculateDecisionModel(double mae, double *reliability) { 257 | double pSuccess, pFailure; 258 | pSuccess = dTruePositive_ * positiveMAEHistogram_.getProbability(mae) + dFalsePositive_ * positiveMAEHistogram_.getProbability(mae); 259 | pFailure = dTrueNegative_ * negativeMAEHistogram_.getProbability(mae) + dFalsePositive_ * negativeMAEHistogram_.getProbability(mae); 260 | if (pSuccess < 10.0e-9) 261 | pSuccess = 10.0e-9; 262 | if (pFailure < 10.0e-9) 263 | pFailure = 10.0e-9; 264 | double rel = pSuccess * *reliability; 265 | double relInv = pFailure * (1.0 - *reliability); 266 | double p = rel + relInv; 267 | if (p > 1.0) 268 | p = 1.0; 269 | *reliability = rel / (rel + relInv); 270 | if (*reliability > 0.9999) 271 | *reliability = 0.9999; 272 | if (*reliability < 0.0001) 273 | *reliability = 0.0001; 274 | return p; 275 | } 276 | 277 | void writeDecisionLikelihoods(void) { 278 | readClassifierParams(); 279 | std::string fileName = classifiersDir_ + "mae_decision_likelihoods.txt"; 280 | FILE *fp = fopen(fileName.c_str(), "w"); 281 | for (double rel = 0.0; rel <= 1.0 + 0.05; rel += 0.05) { 282 | for (double mae = 0.0; mae <= 0.7 + maeHistogramBinWidth_; mae += maeHistogramBinWidth_) { 283 | double pSuccessPositive = dTruePositive_ * truePositiveMAEHistogram_.getProbability(mae); 284 | double pFailurePositive = dFalsePositive_ * falsePositiveMAEHistogram_.getProbability(mae); 285 | if (pSuccessPositive < 10.0e-6) 286 | pSuccessPositive = 10.0e-6; 287 | if (pFailurePositive < 10.0e-6) 288 | pFailurePositive = 10.0e-6; 289 | double relPositive = pSuccessPositive * rel; 290 | double relInvPositive = pFailurePositive * (1.0 - rel); 291 | double pPositive = relPositive + relInvPositive; 292 | 293 | double pSuccessNegative = dFalseNegative_ * falseNegativeMAEHistogram_.getProbability(mae); 294 | double pFailureNegative = dTrueNegative_ * trueNegativeMAEHistogram_.getProbability(mae); 295 | if (pSuccessNegative < 10.0e-6) 296 | pSuccessNegative = 10.0e-6; 297 | if (pFailureNegative < 10.0e-6) 298 | pFailureNegative = 10.0e-6; 299 | double relNegative = pSuccessNegative * rel; 300 | double relInvNegative = pFailureNegative * (1.0 - rel); 301 | double pNegative = relNegative + relInvNegative; 302 | 303 | fprintf(fp, "%lf %lf %lf %lf %lf\n", rel, mae, pPositive, pNegative, pPositive + pNegative); 304 | } 305 | fprintf(fp, "\n"); 306 | } 307 | fclose(fp); 308 | } 309 | }; // class MAEClassifier 310 | 311 | } // namespace als_ros 312 | 313 | #endif // __MAE_CLASSIFIER_H__ 314 | -------------------------------------------------------------------------------- /rviz/slamer.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.5 9 | Tree Height: 456 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Scan 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 0.699999988079071 55 | Class: rviz/Map 56 | Color Scheme: map 57 | Draw Behind: false 58 | Enabled: true 59 | Name: Map 60 | Topic: /map 61 | Unreliable: false 62 | Use Timestamp: false 63 | Value: true 64 | - Class: rviz/TF 65 | Enabled: true 66 | Frame Timeout: 15 67 | Frames: 68 | All Enabled: false 69 | base_link: 70 | Value: true 71 | head_camera: 72 | Value: false 73 | laser: 74 | Value: false 75 | map: 76 | Value: false 77 | odom: 78 | Value: false 79 | Marker Alpha: 1 80 | Marker Scale: 5 81 | Name: TF 82 | Show Arrows: false 83 | Show Axes: true 84 | Show Names: true 85 | Tree: 86 | map: 87 | odom: 88 | base_link: 89 | laser: 90 | head_camera: 91 | {} 92 | Update Interval: 0 93 | Value: true 94 | - Alpha: 1 95 | Arrow Length: 1 96 | Axes Length: 0.30000001192092896 97 | Axes Radius: 0.009999999776482582 98 | Class: rviz/PoseArray 99 | Color: 0; 0; 255 100 | Enabled: false 101 | Head Length: 0.07000000029802322 102 | Head Radius: 0.029999999329447746 103 | Name: MCL particles 104 | Queue Size: 10 105 | Shaft Length: 0.23000000417232513 106 | Shaft Radius: 0.009999999776482582 107 | Shape: Arrow (Flat) 108 | Topic: /mcl_particles 109 | Unreliable: false 110 | Value: false 111 | - Alpha: 1 112 | Autocompute Intensity Bounds: true 113 | Autocompute Value Bounds: 114 | Max Value: 10 115 | Min Value: -10 116 | Value: true 117 | Axis: Z 118 | Channel Name: intensity 119 | Class: rviz/LaserScan 120 | Color: 252; 233; 79 121 | Color Transformer: FlatColor 122 | Decay Time: 0 123 | Enabled: true 124 | Invert Rainbow: false 125 | Max Color: 255; 255; 255 126 | Min Color: 0; 0; 0 127 | Name: Scan 128 | Position Transformer: XYZ 129 | Queue Size: 10 130 | Selectable: true 131 | Size (Pixels): 5 132 | Size (m): 0.009999999776482582 133 | Style: Points 134 | Topic: /scan 135 | Unreliable: false 136 | Use Fixed Frame: true 137 | Use rainbow: true 138 | Value: true 139 | - Alpha: 1 140 | Autocompute Intensity Bounds: true 141 | Autocompute Value Bounds: 142 | Max Value: 10 143 | Min Value: -10 144 | Value: true 145 | Axis: Z 146 | Channel Name: intensity 147 | Class: rviz/PointCloud 148 | Color: 255; 255; 255 149 | Color Transformer: Intensity 150 | Decay Time: 0 151 | Enabled: false 152 | Invert Rainbow: false 153 | Max Color: 255; 255; 255 154 | Min Color: 0; 0; 0 155 | Name: Scan points 156 | Position Transformer: XYZ 157 | Queue Size: 10 158 | Selectable: true 159 | Size (Pixels): 3 160 | Size (m): 0.009999999776482582 161 | Style: Points 162 | Topic: /scan_points 163 | Unreliable: false 164 | Use Fixed Frame: true 165 | Use rainbow: true 166 | Value: false 167 | - Alpha: 1 168 | Autocompute Intensity Bounds: true 169 | Autocompute Value Bounds: 170 | Max Value: 10 171 | Min Value: -10 172 | Value: true 173 | Axis: Z 174 | Channel Name: intensity 175 | Class: rviz/LaserScan 176 | Color: 252; 233; 79 177 | Color Transformer: FlatColor 178 | Decay Time: 0 179 | Enabled: true 180 | Invert Rainbow: false 181 | Max Color: 255; 255; 255 182 | Min Color: 0; 0; 0 183 | Name: Unknown scan 184 | Position Transformer: XYZ 185 | Queue Size: 15 186 | Selectable: true 187 | Size (Pixels): 15 188 | Size (m): 0.009999999776482582 189 | Style: Points 190 | Topic: /unknown_scan 191 | Unreliable: false 192 | Use Fixed Frame: true 193 | Use rainbow: true 194 | Value: true 195 | - Class: rviz/Marker 196 | Enabled: false 197 | Marker Topic: /reliability_marker_name 198 | Name: Reliability marker 199 | Namespaces: 200 | {} 201 | Queue Size: 100 202 | Value: false 203 | - Alpha: 1 204 | Autocompute Intensity Bounds: true 205 | Autocompute Value Bounds: 206 | Max Value: 10 207 | Min Value: -10 208 | Value: true 209 | Axis: Z 210 | Channel Name: intensity 211 | Class: rviz/LaserScan 212 | Color: 239; 41; 41 213 | Color Transformer: FlatColor 214 | Decay Time: 0 215 | Enabled: false 216 | Invert Rainbow: false 217 | Max Color: 255; 255; 255 218 | Min Color: 0; 0; 0 219 | Name: Aligned scan 220 | Position Transformer: XYZ 221 | Queue Size: 10 222 | Selectable: true 223 | Size (Pixels): 7 224 | Size (m): 0.009999999776482582 225 | Style: Points 226 | Topic: /aligned_scan_mrf 227 | Unreliable: false 228 | Use Fixed Frame: true 229 | Use rainbow: true 230 | Value: false 231 | - Alpha: 1 232 | Autocompute Intensity Bounds: true 233 | Autocompute Value Bounds: 234 | Max Value: 10 235 | Min Value: -10 236 | Value: true 237 | Axis: Z 238 | Channel Name: intensity 239 | Class: rviz/LaserScan 240 | Color: 0; 0; 255 241 | Color Transformer: FlatColor 242 | Decay Time: 0 243 | Enabled: false 244 | Invert Rainbow: false 245 | Max Color: 255; 255; 255 246 | Min Color: 0; 0; 0 247 | Name: Misaligned scan 248 | Position Transformer: XYZ 249 | Queue Size: 10 250 | Selectable: true 251 | Size (Pixels): 7 252 | Size (m): 0.009999999776482582 253 | Style: Points 254 | Topic: /misaligned_scan_mrf 255 | Unreliable: false 256 | Use Fixed Frame: true 257 | Use rainbow: true 258 | Value: false 259 | - Alpha: 1 260 | Autocompute Intensity Bounds: true 261 | Autocompute Value Bounds: 262 | Max Value: 10 263 | Min Value: -10 264 | Value: true 265 | Axis: Z 266 | Channel Name: intensity 267 | Class: rviz/LaserScan 268 | Color: 0; 255; 0 269 | Color Transformer: FlatColor 270 | Decay Time: 0 271 | Enabled: false 272 | Invert Rainbow: false 273 | Max Color: 255; 255; 255 274 | Min Color: 0; 0; 0 275 | Name: Unknown scan 276 | Position Transformer: XYZ 277 | Queue Size: 10 278 | Selectable: true 279 | Size (Pixels): 7 280 | Size (m): 0.009999999776482582 281 | Style: Points 282 | Topic: /unknown_scan_mrf 283 | Unreliable: false 284 | Use Fixed Frame: true 285 | Use rainbow: true 286 | Value: false 287 | - Class: rviz/Marker 288 | Enabled: false 289 | Marker Topic: /failure_probability_marker_name 290 | Name: Failure probability marker 291 | Namespaces: 292 | {} 293 | Queue Size: 100 294 | Value: false 295 | - Alpha: 1 296 | Arrow Length: 1.5 297 | Axes Length: 0.30000001192092896 298 | Axes Radius: 0.009999999776482582 299 | Class: rviz/PoseArray 300 | Color: 92; 53; 102 301 | Enabled: true 302 | Head Length: 0.07000000029802322 303 | Head Radius: 0.029999999329447746 304 | Name: GL sampled poses 305 | Queue Size: 10 306 | Shaft Length: 0.23000000417232513 307 | Shaft Radius: 0.009999999776482582 308 | Shape: Arrow (Flat) 309 | Topic: /gl_sampled_poses 310 | Unreliable: false 311 | Value: true 312 | - Class: rviz/Marker 313 | Enabled: true 314 | Marker Topic: /gl_sdf_keypoints 315 | Name: SDF keypoints 316 | Namespaces: 317 | {} 318 | Queue Size: 100 319 | Value: true 320 | - Alpha: 0.699999988079071 321 | Class: rviz/Map 322 | Color Scheme: map 323 | Draw Behind: false 324 | Enabled: false 325 | Name: GL local map 326 | Topic: /gl_local_map 327 | Unreliable: false 328 | Use Timestamp: false 329 | Value: false 330 | - Class: rviz/Marker 331 | Enabled: false 332 | Marker Topic: /gl_local_sdf_keypoints 333 | Name: Local SDF keypoints 334 | Namespaces: 335 | {} 336 | Queue Size: 100 337 | Value: false 338 | - Alpha: 1 339 | Autocompute Intensity Bounds: true 340 | Autocompute Value Bounds: 341 | Max Value: 10 342 | Min Value: -10 343 | Value: true 344 | Axis: Z 345 | Channel Name: intensity 346 | Class: rviz/PointCloud2 347 | Color: 255; 255; 255 348 | Color Transformer: RGB8 349 | Decay Time: 0 350 | Enabled: true 351 | Invert Rainbow: false 352 | Max Color: 255; 255; 255 353 | Min Color: 0; 0; 0 354 | Name: ISM points 355 | Position Transformer: XYZ 356 | Queue Size: 10 357 | Selectable: true 358 | Size (Pixels): 10 359 | Size (m): 0.05000000074505806 360 | Style: Squares 361 | Topic: /ism_points 362 | Unreliable: false 363 | Use Fixed Frame: true 364 | Use rainbow: true 365 | Value: true 366 | - Alpha: 1 367 | Autocompute Intensity Bounds: true 368 | Autocompute Value Bounds: 369 | Max Value: 10 370 | Min Value: -10 371 | Value: true 372 | Axis: Z 373 | Channel Name: intensity 374 | Class: rviz/PointCloud2 375 | Color: 255; 255; 255 376 | Color Transformer: RGB8 377 | Decay Time: 0 378 | Enabled: false 379 | Invert Rainbow: false 380 | Max Color: 255; 255; 255 381 | Min Color: 0; 0; 0 382 | Name: SLAMER colored scan points 383 | Position Transformer: XYZ 384 | Queue Size: 10 385 | Selectable: true 386 | Size (Pixels): 5 387 | Size (m): 0.009999999776482582 388 | Style: Points 389 | Topic: /slamer_colored_scan_points 390 | Unreliable: false 391 | Use Fixed Frame: true 392 | Use rainbow: true 393 | Value: false 394 | - Class: rviz/Marker 395 | Enabled: true 396 | Marker Topic: /slamer_line_objects 397 | Name: SLAMER line objects 398 | Namespaces: 399 | slamer_line_objects: true 400 | Queue Size: 100 401 | Value: true 402 | - Class: rviz/Marker 403 | Enabled: true 404 | Marker Topic: /slamer_spatial_line_objects 405 | Name: SLAMER spatial line objects 406 | Namespaces: 407 | slamer_line_objects: true 408 | Queue Size: 100 409 | Value: true 410 | - Class: rviz/Camera 411 | Enabled: true 412 | Image Rendering: background and overlay 413 | Image Topic: /usb_cam_node/image_raw 414 | Name: Front camera 415 | Overlay Alpha: 0.5 416 | Queue Size: 2 417 | Transport Hint: raw 418 | Unreliable: false 419 | Value: true 420 | Visibility: 421 | Aligned scan: true 422 | Failure probability marker: true 423 | GL local map: true 424 | GL sampled poses: true 425 | Grid: true 426 | ISM points: true 427 | Local SDF keypoints: true 428 | MCL particles: true 429 | Map: true 430 | Misaligned scan: true 431 | Reliability marker: true 432 | SDF keypoints: true 433 | SLAMER colored scan points: true 434 | SLAMER line objects: true 435 | SLAMER spatial line objects: true 436 | Scan: true 437 | Scan points: true 438 | TF: true 439 | Unknown scan: true 440 | Value: true 441 | Zoom Factor: 1 442 | Enabled: true 443 | Global Options: 444 | Background Color: 48; 48; 48 445 | Default Light: true 446 | Fixed Frame: map 447 | Frame Rate: 30 448 | Name: root 449 | Tools: 450 | - Class: rviz/Interact 451 | Hide Inactive Objects: true 452 | - Class: rviz/MoveCamera 453 | - Class: rviz/Select 454 | - Class: rviz/FocusCamera 455 | - Class: rviz/Measure 456 | - Class: rviz/SetInitialPose 457 | Theta std deviation: 0.2617993950843811 458 | Topic: /initialpose 459 | X std deviation: 0.5 460 | Y std deviation: 0.5 461 | - Class: rviz/SetGoal 462 | Topic: /move_base_simple/goal 463 | - Class: rviz/PublishPoint 464 | Single click: true 465 | Topic: /clicked_point 466 | Value: true 467 | Views: 468 | Current: 469 | Angle: 0 470 | Class: rviz/TopDownOrtho 471 | Enable Stereo Rendering: 472 | Stereo Eye Separation: 0.05999999865889549 473 | Stereo Focal Distance: 1 474 | Swap Stereo Eyes: false 475 | Value: false 476 | Invert Z Axis: false 477 | Name: Current View 478 | Near Clip Distance: 0.009999999776482582 479 | Scale: 54.587589263916016 480 | Target Frame: base_link 481 | X: 0 482 | Y: 0 483 | Saved: ~ 484 | Window Geometry: 485 | Displays: 486 | collapsed: false 487 | Front camera: 488 | collapsed: false 489 | Height: 1022 490 | Hide Left Dock: false 491 | Hide Right Dock: false 492 | QMainWindow State: 000000ff00000000fd00000004000000000000026600000360fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000024b000000c900fffffffb0000001800460072006f006e0074002000630061006d006500720061010000028e0000010f0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001b000000360fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0049006d006100670065000000003d000000c20000000000000000fb0000000a00560069006500770073010000003d00000360000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000074a0000022afc0100000002fb0000001800460072006f006e0074002000630061006d0065007200610100000000000009b80000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074a0000003efc0100000002fb0000000800540069006d006501000000000000074a000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003280000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 493 | Selection: 494 | collapsed: false 495 | Time: 496 | collapsed: false 497 | Tool Properties: 498 | collapsed: false 499 | Views: 500 | collapsed: false 501 | Width: 1866 502 | X: 2895 503 | Y: 247 504 | -------------------------------------------------------------------------------- /src/als_ros/launch/mcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 56 | 57 | 58 | 60 | 61 | 62 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 77 | 78 | 79 | 85 | 86 | 87 | 88 | 89 | 91 | 92 | 93 | 98 | 99 | 100 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 114 | 115 | 116 | 119 | 120 | 121 | 122 | 123 | 124 | 127 | 128 | 129 | 132 | 133 | 134 | 135 | 136 | 138 | 139 | 140 | 143 | 144 | 145 | 149 | 150 | 151 | 156 | 157 | 158 | 159 | 163 | 164 | 165 | 167 | 168 | 169 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 182 | [0.05, 0.05, 0.1] 183 | 184 | 186 | [1.0, 0.5, 0.5, 1.5] 187 | [4.0, 1.0, 1.0, 1.0, 4.0, 1.0, 1.0, 1.0, 8.0] 188 | 189 | 192 | [0.0, 0.0] 193 | [0.0, 0.0, 0.0] 194 | 195 | 197 | [0.2, 0.2, 0.2, 0.02, -99999.0] 198 | 199 | 200 | 201 | 202 | 203 | 209 | 210 | 211 | 212 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/ClassifierDatasetGenerator.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __CLASSIFIER_DATASET_GENERATOR_H__ 21 | #define __CLASSIFIER_DATASET_GENERATOR_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace als_ros { 30 | 31 | class Obstacle { 32 | public: 33 | double x_, y_, s_; 34 | // x: x position 35 | // y: y position 36 | // s: size 37 | 38 | Obstacle(void): 39 | x_(0.0), y_(0.0), s_(1.0) {} 40 | 41 | Obstacle(double x, double y, double s): 42 | x_(x), y_(y), s_(s) {} 43 | }; // class Obstacle 44 | 45 | class ClassifierDatasetGenerator { 46 | private: 47 | ros::NodeHandle nh_; 48 | 49 | std::string mapName_; 50 | ros::Subscriber mapSub_; 51 | 52 | int generateSampleNum_; 53 | std::string saveDir_; 54 | std::vector trainDirs_, testDirs_; 55 | 56 | nav_msgs::OccupancyGrid map_; 57 | cv::Mat distMap_; 58 | double mapResolution_; 59 | Pose mapOrigin_; 60 | int mapWidth_, mapHeight_; 61 | bool gotMap_; 62 | double freeSpaceMinX_, freeSpaceMaxX_, freeSpaceMinY_, freeSpaceMaxY_; 63 | 64 | int obstaclesNum_; 65 | 66 | double angleMin_, angleMax_, angleIncrement_, rangeMin_, rangeMax_, scanAngleNoise_, scanRangeNoise_; 67 | double validScanRateTH_; 68 | 69 | double failurePositionalErrorTH_, failureAngularErrorTH_; 70 | double positionalErrorMax_, angularErrorMax_; 71 | 72 | public: 73 | ClassifierDatasetGenerator(void): 74 | nh_("~"), 75 | mapName_("/map"), 76 | generateSampleNum_(1000), 77 | saveDir_("/tmp/classifier_dataset/"), 78 | obstaclesNum_(20), 79 | angleMin_(-135.0), 80 | angleMax_(135.0), 81 | angleIncrement_(0.25), 82 | rangeMin_(0.02), 83 | rangeMax_(30.0), 84 | scanAngleNoise_(0.01), 85 | scanRangeNoise_(0.05), 86 | validScanRateTH_(0.1), 87 | failurePositionalErrorTH_(0.2), 88 | failureAngularErrorTH_(2.0), 89 | positionalErrorMax_(0.5), 90 | angularErrorMax_(5.0), 91 | gotMap_(false) {} 92 | 93 | void datasetGenerationInit(void) { 94 | srand((unsigned int)time(NULL)); 95 | 96 | nh_.param("map_name", mapName_, mapName_); 97 | nh_.param("generate_sample_num", generateSampleNum_, generateSampleNum_); 98 | nh_.param("save_dir", saveDir_, saveDir_); 99 | nh_.param("obstacles_num", obstaclesNum_, obstaclesNum_); 100 | nh_.param("angle_min", angleMin_, angleMin_); 101 | nh_.param("angle_max", angleMax_, angleMax_); 102 | nh_.param("angle_increment", angleIncrement_, angleIncrement_); 103 | nh_.param("range_min", rangeMin_, rangeMin_); 104 | nh_.param("range_max", rangeMax_, rangeMax_); 105 | nh_.param("scan_angle_noise", scanAngleNoise_, scanAngleNoise_); 106 | nh_.param("scan_range_noise", scanRangeNoise_, scanRangeNoise_); 107 | nh_.param("valid_scan_rate_th", validScanRateTH_, validScanRateTH_); 108 | nh_.param("failure_positional_error_th", failurePositionalErrorTH_, failurePositionalErrorTH_); 109 | nh_.param("failure_angular_error_th", failureAngularErrorTH_, failureAngularErrorTH_); 110 | nh_.param("positional_error_max", positionalErrorMax_, positionalErrorMax_); 111 | nh_.param("angular_error_max", angularErrorMax_, angularErrorMax_); 112 | 113 | std::string cmd; 114 | int retVal; 115 | cmd = "mkdir -p " + saveDir_; 116 | retVal = system(cmd.c_str()); 117 | cmd = "rm -rf " + saveDir_ + "/*"; 118 | retVal = system(cmd.c_str()); 119 | 120 | angleMin_ *= M_PI / 180.0; 121 | angleMax_ *= M_PI / 180.0; 122 | angleIncrement_ *= M_PI / 180.0; 123 | scanAngleNoise_ *= M_PI / 180.0; 124 | failureAngularErrorTH_ *= M_PI / 180.0; 125 | angularErrorMax_ *= M_PI / 180.0; 126 | 127 | mapSub_ = nh_.subscribe(mapName_, 1, &ClassifierDatasetGenerator::mapCB, this); 128 | 129 | ros::Rate loopRate(10.0); 130 | int cnt = 0; 131 | while (ros::ok()) { 132 | ros::spinOnce(); 133 | if (gotMap_) 134 | break; 135 | cnt++; 136 | if (cnt > 50) { 137 | ROS_ERROR("Map data might not be published." 138 | " Expected map topic name is %s", mapName_.c_str()); 139 | exit(1); 140 | } 141 | loopRate.sleep(); 142 | } 143 | } 144 | 145 | void generateDataset(void) { 146 | for (int i = 0; i < generateSampleNum_; ++i) { 147 | std::vector obstacles = generateObstacles(); 148 | nav_msgs::OccupancyGrid simMap = buildSimulationMap(obstacles); 149 | Pose gtPose, successPose, failurePose; 150 | generatePoses(gtPose, successPose, failurePose); 151 | sensor_msgs::LaserScan scan = simulateScan(gtPose, simMap); 152 | if (!isValidScan(scan)) { 153 | ROS_INFO("Simulated scan is invalid."); 154 | i--; 155 | continue; 156 | } 157 | std::vector successResidualErrors = getResidualErrors(successPose, scan); 158 | std::vector failureResidualErrors = getResidualErrors(failurePose, scan); 159 | saveData(gtPose, successPose, failurePose, scan, successResidualErrors, failureResidualErrors); 160 | if ((i + 1) % 10 == 0) 161 | ROS_INFO("%.2lf [%%] process done.", (double)(i + 1) / (double)generateSampleNum_ * 100.0); 162 | if (!ros::ok()) 163 | break; 164 | } 165 | } 166 | 167 | inline void setTrainDirs(std::vector trainDirs) { 168 | trainDirs_ = trainDirs; 169 | } 170 | 171 | inline void setTestDirs(std::vector testDirs) { 172 | testDirs_ = testDirs; 173 | } 174 | 175 | void readTrainDataset(std::vector >Poses, std::vector &successPoses, std::vector &failurePoses, 176 | std::vector &scans, std::vector> &successResidualErrors, std::vector> &failureResidualErrors) 177 | { 178 | readDataset(trainDirs_, gtPoses, successPoses, failurePoses, scans, successResidualErrors, failureResidualErrors); 179 | } 180 | 181 | void readTestDataset(std::vector >Poses, std::vector &successPoses, std::vector &failurePoses, 182 | std::vector &scans, std::vector> &successResidualErrors, std::vector> &failureResidualErrors) 183 | { 184 | readDataset(testDirs_, gtPoses, successPoses, failurePoses, scans, successResidualErrors, failureResidualErrors); 185 | } 186 | 187 | private: 188 | inline double nrand(double n) { 189 | return (n * sqrt(-2.0 * log((double)rand() / RAND_MAX)) * cos(2.0 * M_PI * rand() / RAND_MAX)); 190 | } 191 | 192 | inline double urand(double min, double max) { 193 | return ((max - min) * (double)rand() / RAND_MAX + min); 194 | } 195 | 196 | inline bool onMap(int u, int v) { 197 | if (0 <= u && u < mapWidth_ && 0 <= v && v < mapHeight_) 198 | return true; 199 | else 200 | return false; 201 | } 202 | 203 | inline void xy2uv(double x, double y, int *u, int *v) { 204 | double dx = x - mapOrigin_.getX(); 205 | double dy = y - mapOrigin_.getY(); 206 | double yaw = -mapOrigin_.getYaw(); 207 | double xx = dx * cos(yaw) - dy * sin(yaw); 208 | double yy = dx * sin(yaw) + dy * cos(yaw); 209 | *u = (int)(xx / mapResolution_); 210 | *v = (int)(yy / mapResolution_); 211 | } 212 | 213 | inline void uv2xy(int u, int v, double *x, double *y) { 214 | double xx = (double)u * mapResolution_; 215 | double yy = (double)v * mapResolution_; 216 | double yaw = mapOrigin_.getYaw(); 217 | double dx = xx * cos(yaw) - yy * sin(yaw); 218 | double dy = xx * sin(yaw) + yy * cos(yaw); 219 | *x = dx + mapOrigin_.getX(); 220 | *y = dy + mapOrigin_.getY(); 221 | } 222 | 223 | inline int xy2node(double x, double y) { 224 | int u, v; 225 | xy2uv(x, y, &u, &v); 226 | if (0 <= u && u < mapWidth_ && 0 <= v && v < mapHeight_) 227 | return v * mapWidth_ + u; 228 | else 229 | return -1; 230 | } 231 | 232 | void mapCB(const nav_msgs::OccupancyGrid::ConstPtr &msg) { 233 | map_ = *msg; 234 | 235 | // perform distance transform to build the distance field 236 | mapWidth_ = msg->info.width; 237 | mapHeight_ = msg->info.height; 238 | mapResolution_ = msg->info.resolution; 239 | tf::Quaternion q(msg->info.origin.orientation.x, 240 | msg->info.origin.orientation.y, 241 | msg->info.origin.orientation.z, 242 | msg->info.origin.orientation.w); 243 | double roll, pitch, yaw; 244 | tf::Matrix3x3 m(q); 245 | m.getRPY(roll, pitch, yaw); 246 | mapOrigin_.setX(msg->info.origin.position.x); 247 | mapOrigin_.setY(msg->info.origin.position.y); 248 | mapOrigin_.setYaw(yaw); 249 | 250 | cv::Mat binMap(mapHeight_, mapWidth_, CV_8UC1); 251 | bool isFirst = true; 252 | for (int v = 0; v < mapHeight_; v++) { 253 | for (int u = 0; u < mapWidth_; u++) { 254 | int node = v * mapWidth_ + u; 255 | int val = msg->data[node]; 256 | if (val == 100) { 257 | binMap.at(v, u) = 0; 258 | } else { 259 | binMap.at(v, u) = 1; 260 | if (val == 0) { 261 | double x, y; 262 | uv2xy(u, v, &x, &y); 263 | if (isFirst) { 264 | freeSpaceMinX_ = freeSpaceMaxX_ = x; 265 | freeSpaceMinY_ = freeSpaceMaxY_ = y; 266 | isFirst = false; 267 | } else { 268 | if (freeSpaceMinX_ > x) 269 | freeSpaceMinX_ = x; 270 | if (freeSpaceMaxX_ < x) 271 | freeSpaceMaxX_ = x; 272 | if (freeSpaceMinY_ > y) 273 | freeSpaceMinY_ = y; 274 | if (freeSpaceMaxY_ < y) 275 | freeSpaceMaxY_ = y; 276 | } 277 | } 278 | } 279 | } 280 | } 281 | cv::Mat distMap(mapHeight_, mapWidth_, CV_32FC1); 282 | cv::distanceTransform(binMap, distMap, cv::DIST_L2, 5); 283 | for (int v = 0; v < mapHeight_; v++) { 284 | for (int u = 0; u < mapWidth_; u++) { 285 | float d = distMap.at(v, u) * (float)mapResolution_; 286 | distMap.at(v, u) = d; 287 | } 288 | } 289 | distMap_ = distMap; 290 | gotMap_ = true; 291 | } 292 | 293 | std::vector generateObstacles(void) { 294 | std::vector obstacles(obstaclesNum_); 295 | for (int i = 0; i < obstaclesNum_; ++i) { 296 | obstacles[i].x_ = urand(freeSpaceMinX_, freeSpaceMaxX_); 297 | obstacles[i].y_ = urand(freeSpaceMinY_, freeSpaceMaxY_); 298 | obstacles[i].s_ = 1.0 + nrand(0.5); 299 | } 300 | return obstacles; 301 | } 302 | 303 | nav_msgs::OccupancyGrid buildSimulationMap(std::vector obstacles) { 304 | nav_msgs::OccupancyGrid simMap = map_; 305 | for (int i = 0; i < (int)obstacles.size(); ++i) { 306 | double helfSize = obstacles[i].s_ / 2.0; 307 | for (double x = obstacles[i].x_ - helfSize; x <= obstacles[i].x_ + helfSize; x += mapResolution_) { 308 | for (double y = obstacles[i].y_ - helfSize; y <= obstacles[i].y_ + helfSize; y += mapResolution_) { 309 | int node = xy2node(x, y); 310 | if (node >= 0) 311 | simMap.data[node] = 100; 312 | } 313 | } 314 | } 315 | return simMap; 316 | } 317 | 318 | void generatePoses(Pose >Pose, Pose &successPose, Pose &failurePose) { 319 | for (;;) { 320 | double x = urand(freeSpaceMinX_, freeSpaceMaxX_); 321 | double y = urand(freeSpaceMinY_, freeSpaceMaxY_); 322 | double yaw = urand(-M_PI, M_PI); 323 | int node = xy2node(x, y); 324 | if (node < 0) 325 | continue; 326 | int u, v; 327 | xy2uv(x, y, &u, &v); 328 | float dist = distMap_.at(v, u); 329 | if (map_.data[node] == 0 && dist > 0.5f) { 330 | gtPose.setX(x); 331 | gtPose.setY(y); 332 | gtPose.setYaw(yaw); 333 | break; 334 | } 335 | } 336 | 337 | double x = urand(-failurePositionalErrorTH_ * 0.99, failurePositionalErrorTH_ * 0.99); 338 | double y = urand(-failurePositionalErrorTH_ * 0.99, failurePositionalErrorTH_ * 0.99); 339 | double yaw = urand(-failureAngularErrorTH_ * 0.99, failureAngularErrorTH_ * 0.99); 340 | successPose.setX(gtPose.getX() + x); 341 | successPose.setY(gtPose.getY() + y); 342 | successPose.setYaw(gtPose.getYaw() + yaw); 343 | 344 | for (;;) { 345 | double x = urand(-positionalErrorMax_, positionalErrorMax_); 346 | double y = urand(-positionalErrorMax_, positionalErrorMax_); 347 | double yaw = urand(-angularErrorMax_, angularErrorMax_); 348 | if (fabs(x) >= failurePositionalErrorTH_ || fabs(y) >= failureAngularErrorTH_ || fabs(yaw) >= failureAngularErrorTH_) { 349 | failurePose.setX(gtPose.getX() + x); 350 | failurePose.setY(gtPose.getY() + y); 351 | failurePose.setYaw(gtPose.getYaw() + yaw); 352 | break; 353 | } 354 | } 355 | } 356 | 357 | sensor_msgs::LaserScan simulateScan(Pose gtPose, nav_msgs::OccupancyGrid simMap) { 358 | sensor_msgs::LaserScan scan; 359 | scan.angle_min = angleMin_; 360 | scan.angle_max = angleMax_; 361 | scan.angle_increment = angleIncrement_; 362 | scan.range_min = rangeMin_; 363 | scan.range_max = rangeMax_; 364 | int scanNum = (int)((angleMax_ - angleMin_) / angleIncrement_) + 1; 365 | scan.ranges.resize(scanNum, 0.0); 366 | for (int i = 0; i < scanNum; ++i) { 367 | double t = gtPose.getYaw() + (double)i * angleIncrement_ + angleMin_ + nrand(scanAngleNoise_); 368 | double x = gtPose.getX(); 369 | double y = gtPose.getY(); 370 | double dx = mapResolution_ * cos(t); 371 | double dy = mapResolution_ * sin(t); 372 | double range = -1.0; 373 | for (double r = 0.0; r <= rangeMax_; r += mapResolution_) { 374 | int node = xy2node(x, y); 375 | if (node > 0) { 376 | if (simMap.data[node] == 100) { 377 | range = r; 378 | break; 379 | } 380 | } else { 381 | break; 382 | } 383 | x += dx; 384 | y += dy; 385 | } 386 | if (range >= 0.0) 387 | scan.ranges[i] = range + nrand(scanRangeNoise_); 388 | } 389 | return scan; 390 | } 391 | 392 | bool isValidScan(sensor_msgs::LaserScan scan) { 393 | int validScanNum = 0; 394 | for (int i = 0; i < (int)scan.ranges.size(); ++i) { 395 | double r = scan.ranges[i]; 396 | if (scan.range_min <= r && r <= scan.range_max) 397 | validScanNum++; 398 | } 399 | double validScanRate = (double)validScanNum / (double)scan.ranges.size(); 400 | if (validScanRate >= validScanRateTH_) 401 | return true; 402 | else 403 | return false; 404 | } 405 | 406 | std::vector getResidualErrors(Pose pose, sensor_msgs::LaserScan scan) { 407 | int size = (int)scan.ranges.size(); 408 | std::vector residualErrors(size); 409 | for (int i = 0; i < size; ++i) { 410 | double r = scan.ranges[i]; 411 | if (r <= scan.range_min || scan.range_max <= r) { 412 | residualErrors[i] = -1.0; 413 | continue; 414 | } 415 | double t = (double)i * scan.angle_increment + scan.angle_min + pose.getYaw(); 416 | double x = r * cos(t) + pose.getX(); 417 | double y = r * sin(t) + pose.getY(); 418 | int u, v; 419 | xy2uv(x, y, &u, &v); 420 | if (onMap(u, v)) { 421 | double dist = (double)distMap_.at(v, u); 422 | residualErrors[i] = dist; 423 | } else { 424 | residualErrors[i] = -1.0; 425 | } 426 | } 427 | return residualErrors; 428 | } 429 | 430 | void saveData(Pose gtPose, Pose successPose, Pose failurePose, sensor_msgs::LaserScan scan, std::vector successResidualErrors, std::vector failureResidualErrors) { 431 | static bool isFirst = true; 432 | static int cnt = 0; 433 | std::string fname; 434 | FILE *fp; 435 | if (isFirst) { 436 | fname = saveDir_ + "/scan_param.txt"; 437 | fp = fopen(fname.c_str(), "w"); 438 | fprintf(fp, "angle_min: %lf\n", angleMin_); 439 | fprintf(fp, "angle_max: %lf\n", angleMax_); 440 | fprintf(fp, "angle_increment: %lf\n", angleIncrement_); 441 | fprintf(fp, "range_min: %lf\n", rangeMin_); 442 | fprintf(fp, "range_max: %lf\n", rangeMax_); 443 | fclose(fp); 444 | 445 | std::string cmd = "mkdir -p " + saveDir_ + "/scan_and_residual_errors/"; 446 | int retVal = system(cmd.c_str()); 447 | isFirst = false; 448 | } 449 | 450 | fname = saveDir_ + "/gt_poses.txt"; 451 | fp = fopen(fname.c_str(), "a"); 452 | fprintf(fp, "%lf %lf %lf\n", gtPose.getX(), gtPose.getY(), gtPose.getYaw()); 453 | fclose(fp); 454 | 455 | fname = saveDir_ + "/success_poses.txt"; 456 | fp = fopen(fname.c_str(), "a"); 457 | fprintf(fp, "%lf %lf %lf\n", successPose.getX(), successPose.getY(), successPose.getYaw()); 458 | fclose(fp); 459 | 460 | fname = saveDir_ + "/failure_poses.txt"; 461 | fp = fopen(fname.c_str(), "a"); 462 | fprintf(fp, "%lf %lf %lf\n", failurePose.getX(), failurePose.getY(), failurePose.getYaw()); 463 | fclose(fp); 464 | 465 | fname = saveDir_ + "/scan_and_residual_errors/" + std::to_string(cnt) + ".txt"; 466 | fp = fopen(fname.c_str(), "w"); 467 | for (int i = 0; i < (int)scan.ranges.size(); ++i) 468 | fprintf(fp, "%d %lf %lf %lf\n", i, scan.ranges[i], successResidualErrors[i], failureResidualErrors[i]); 469 | fclose(fp); 470 | 471 | cnt++; 472 | } 473 | 474 | void readDataset(std::vector dirs, std::vector >Poses, std::vector &successPoses, std::vector &failurePoses, 475 | std::vector &scans, std::vector> &successResidualErrors, std::vector> &failureResidualErrors) 476 | { 477 | FILE *fp; 478 | std::string fname; 479 | double x, y, yaw; 480 | for (int dirIdx = 0; dirIdx < (int)dirs.size(); ++dirIdx) { 481 | fname = dirs[dirIdx] + "/scan_param.txt"; 482 | fp = fopen(fname.c_str(), "r"); 483 | char buf[32]; 484 | int retVal; 485 | retVal = fscanf(fp, "%s %lf", buf, &angleMin_); 486 | retVal = fscanf(fp, "%s %lf", buf, &angleMax_); 487 | retVal = fscanf(fp, "%s %lf", buf, &angleIncrement_); 488 | retVal = fscanf(fp, "%s %lf", buf, &rangeMin_); 489 | retVal = fscanf(fp, "%s %lf", buf, &rangeMax_); 490 | retVal = fclose(fp); 491 | 492 | sensor_msgs::LaserScan scan; 493 | scan.angle_min = angleMin_; 494 | scan.angle_max = angleMax_; 495 | scan.angle_increment = angleIncrement_; 496 | scan.range_min = rangeMin_; 497 | scan.range_max = rangeMax_; 498 | int scanSize = (int)((angleMax_ - angleMin_) / angleIncrement_) + 1; 499 | scan.ranges.resize(scanSize); 500 | scan.intensities.resize(scanSize); 501 | 502 | fname = dirs[dirIdx] + "/gt_poses.txt"; 503 | fp = fopen(fname.c_str(), "r"); 504 | while (fscanf(fp, "%lf %lf %lf\n", &x, &y, &yaw) != EOF) { 505 | Pose p(x, y, yaw); 506 | gtPoses.push_back(p); 507 | } 508 | fclose(fp); 509 | 510 | fname = dirs[dirIdx] + "/success_poses.txt"; 511 | fp = fopen(fname.c_str(), "r"); 512 | while (fscanf(fp, "%lf %lf %lf\n", &x, &y, &yaw) != EOF) { 513 | Pose p(x, y, yaw); 514 | successPoses.push_back(p); 515 | } 516 | fclose(fp); 517 | 518 | fname = dirs[dirIdx] + "/failure_poses.txt"; 519 | fp = fopen(fname.c_str(), "r"); 520 | while (fscanf(fp, "%lf %lf %lf\n", &x, &y, &yaw) != EOF) { 521 | Pose p(x, y, yaw); 522 | failurePoses.push_back(p); 523 | } 524 | fclose(fp); 525 | 526 | int dataNum = (int)gtPoses.size(); 527 | for (int i = 0; i < dataNum; ++i) { 528 | fname = dirs[dirIdx] + "/scan_and_residual_errors/" + std::to_string(i) + ".txt"; 529 | fp = fopen(fname.c_str(), "r"); 530 | int idx; 531 | std::vector srErrors(scanSize), frErrors(scanSize); 532 | for (int j = 0; j < scanSize; ++j) 533 | retVal = fscanf(fp, "%d %f %lf %lf\n", &idx, &scan.ranges[j], &srErrors[j], &frErrors[j]); 534 | scans.push_back(scan); 535 | successResidualErrors.push_back(srErrors); 536 | failureResidualErrors.push_back(frErrors); 537 | fclose(fp); 538 | } 539 | } 540 | } 541 | }; // class ClassifierDatasetGenerator 542 | 543 | } // namespace als_ros 544 | 545 | #endif // __CLASSIFIER_DATASET_GENERATOR_H__ 546 | -------------------------------------------------------------------------------- /src/als_ros/include/als_ros/MRFFailureDetector.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * als_ros: An Advanced Localization System for ROS use with 2D LiDAR 3 | * Copyright (C) 2022 Naoki Akai 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the “License”); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __MRF_FAILURE_DETECTOR_H__ 21 | #define __MRF_FAILURE_DETECTOR_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace als_ros { 32 | 33 | enum MeasurementClass { 34 | ALIGNED = 0, 35 | MISALIGNED = 1, 36 | UNKNOWN = 2 37 | }; 38 | 39 | class MRFFD { 40 | private: 41 | // ros subscribers and publishers 42 | ros::NodeHandle nh_; 43 | std::string residualErrorsName_; 44 | ros::Subscriber residualErrorsSub_; 45 | std::string failureProbName_, alignedScanName_, misalignedScanName_, unknownScanName_; 46 | ros::Publisher failureProbPub_, alignedScanPub_, misalignedScanPub_, unknownScanPub_; 47 | bool publishClassifiedScans_; 48 | 49 | std::string failureProbabilityMarkerName_, markerFrame_; 50 | ros::Publisher failureProbabilityMarkerPub_; 51 | bool publishFailureProbabilityMarker_; 52 | 53 | // parametsrs 54 | double maxResidualError_; 55 | double NDMean_, NDVar_, NDNormConst_, EDLambda_; 56 | int minValidResidualErrorsNum_, maxResidualErrorsNum_; 57 | int maxLPBComputationNum_; 58 | int samplingNum_; 59 | double residualErrorReso_; 60 | double misalignmentRatioThreshold_, unknownRatioThreshold_; 61 | std::vector transitionProbMat_; 62 | 63 | sensor_msgs::LaserScan residualErrors_; 64 | std::vector usedResidualErrors_; 65 | std::vector usedScanIndices_; 66 | bool canUpdateResidualErrors_, gotResidualErrors_; 67 | double failureDetectionHz_; 68 | 69 | // results 70 | std::vector> measurementClassProbabilities_; 71 | double failureProbability_; 72 | ros::Time failureProbabilityStamp_; 73 | 74 | public: 75 | MRFFD(): 76 | nh_("~"), 77 | residualErrorsName_("/residual_errors"), 78 | failureProbName_("/failure_probability"), 79 | alignedScanName_("/aligned_scan_mrf"), 80 | misalignedScanName_("/misaligned_scan_mrf"), 81 | unknownScanName_("/unknown_scan_mrf"), 82 | publishClassifiedScans_(true), 83 | failureProbabilityMarkerName_("/failure_probability_marker"), 84 | publishFailureProbabilityMarker_(true), 85 | markerFrame_("base_link"), 86 | NDMean_(0.0), 87 | NDVar_(0.04), 88 | EDLambda_(4.0), 89 | maxResidualError_(1.0), 90 | residualErrorReso_(0.05), 91 | minValidResidualErrorsNum_(10), 92 | maxResidualErrorsNum_(200), 93 | maxLPBComputationNum_(1000), 94 | samplingNum_(1000), 95 | misalignmentRatioThreshold_(0.1), 96 | unknownRatioThreshold_(0.7), 97 | transitionProbMat_({0.8, 0.0, 0.2, 0.0, 0.8, 0.2, 0.333333, 0.333333, 0.333333}), 98 | canUpdateResidualErrors_(true), 99 | gotResidualErrors_(false), 100 | failureDetectionHz_(10.0) 101 | { 102 | // input and output message names 103 | nh_.param("residual_errors_name", residualErrorsName_, residualErrorsName_); 104 | nh_.param("failure_probability_name", failureProbName_, failureProbName_); 105 | nh_.param("publish_classified_scans", publishClassifiedScans_, publishClassifiedScans_); 106 | nh_.param("aligned_scan_mrf", alignedScanName_, alignedScanName_); 107 | nh_.param("misaligned_scan_mrf", misalignedScanName_, misalignedScanName_); 108 | nh_.param("unknown_scan_mrf", unknownScanName_, unknownScanName_); 109 | nh_.param("failure_probability_marker_name", failureProbabilityMarkerName_, failureProbabilityMarkerName_); 110 | nh_.param("publish_failure_probability_marker", publishFailureProbabilityMarker_, publishFailureProbabilityMarker_); 111 | nh_.param("marker_frame", markerFrame_, markerFrame_); 112 | 113 | // parameters 114 | nh_.param("normal_distribution_mean", NDMean_, NDMean_); 115 | nh_.param("normal_distribution_var", NDVar_, NDVar_); 116 | nh_.param("exponential_distribution_lambda", EDLambda_, EDLambda_); 117 | nh_.param("max_residual_error", maxResidualError_, maxResidualError_); 118 | nh_.param("residual_error_resolution", residualErrorReso_, residualErrorReso_); 119 | nh_.param("min_valid_residual_errors_num", minValidResidualErrorsNum_, minValidResidualErrorsNum_); 120 | nh_.param("max_residual_errors_num", maxResidualErrorsNum_, maxResidualErrorsNum_); 121 | nh_.param("max_lpb_computation_num", maxLPBComputationNum_, maxLPBComputationNum_); 122 | nh_.param("sampling_num", samplingNum_, samplingNum_); 123 | nh_.param("misalignment_ratio_threshold", misalignmentRatioThreshold_, misalignmentRatioThreshold_); 124 | nh_.param("unknown_ratio_threshold", unknownRatioThreshold_, unknownRatioThreshold_); 125 | nh_.param("transition_probability_matrix", transitionProbMat_, transitionProbMat_); 126 | 127 | // other parameters 128 | nh_.param("failure_detection_hz", failureDetectionHz_, failureDetectionHz_); 129 | 130 | // ros subscriber and publisher 131 | residualErrorsSub_ = nh_.subscribe(residualErrorsName_, 1, &MRFFD::residualErrorsCB, this); 132 | failureProbPub_ = nh_.advertise(failureProbName_, 1); 133 | if (publishClassifiedScans_) { 134 | alignedScanPub_ = nh_.advertise(alignedScanName_, 1); 135 | misalignedScanPub_ = nh_.advertise(misalignedScanName_, 1); 136 | unknownScanPub_ = nh_.advertise(unknownScanName_, 1); 137 | } 138 | if (publishFailureProbabilityMarker_) 139 | failureProbabilityMarkerPub_ = nh_.advertise(failureProbabilityMarkerName_, 1); 140 | 141 | // fixed parameters 142 | NDNormConst_ = 1.0 / sqrt(2.0 * M_PI * NDVar_); 143 | 144 | // wait for getting the residual errors 145 | ros::Rate loopRate(failureDetectionHz_); 146 | int residualErrorsFailedCnt = 0; 147 | while (!gotResidualErrors_) { 148 | ros::spinOnce(); 149 | residualErrorsFailedCnt++; 150 | if (residualErrorsFailedCnt >= 30) { 151 | ROS_ERROR("Cannot get residual errors." 152 | " Did you publish the residual errors?" 153 | " The expected topic name is %s", residualErrorsName_.c_str()); 154 | } 155 | loopRate.sleep(); 156 | } 157 | 158 | ROS_INFO("MRF failure detector is ready to perform."); 159 | } 160 | 161 | ~MRFFD() {}; 162 | 163 | inline void setMaxResidualError(double maxResidualError) { maxResidualError_ = maxResidualError; } 164 | inline void setNDMean(double NDMean) { NDMean_ = NDMean; } 165 | inline void setNDVariance(double NDVar) { NDVar_ = NDVar, NDNormConst_ = 1.0 / sqrt(2.0 * M_PI * NDVar_); } 166 | inline void setEDLambda(double EDLambda) { EDLambda_ = EDLambda; } 167 | inline void setResidualErrorReso(double residualErrorReso) { residualErrorReso_ = residualErrorReso; } 168 | inline void setMinValidResidualErrorNum(int minValidResidualErrorNum) { minValidResidualErrorsNum_ = minValidResidualErrorNum; } 169 | inline void setMaxLPBComputationNum(int maxLPBComputationNum) { maxLPBComputationNum_ = maxLPBComputationNum; } 170 | inline void setSamplingNum(int samplingNum) { samplingNum_ = samplingNum; } 171 | inline void setMisalignmentRatioThreshold(double misalignmentRatioThreshold) { misalignmentRatioThreshold_ = misalignmentRatioThreshold; } 172 | inline void setTransitionProbMat(std::vector transitionProbMat) { transitionProbMat_ = transitionProbMat; } 173 | inline void setCanUpdateResidualErrors(bool canUpdateResidualErrors) { canUpdateResidualErrors_ = canUpdateResidualErrors; } 174 | 175 | inline double getFailureProbability(void) { return failureProbability_; } 176 | inline double getMeasurementClassProbabilities(int errorIndex, int measurementClass) { return measurementClassProbabilities_[errorIndex][measurementClass]; } 177 | inline std::vector getMeasurementClassProbabilities(int errorIndex) { return measurementClassProbabilities_[errorIndex]; } 178 | inline double getFailureDetectionHz(void) { return failureDetectionHz_; } 179 | 180 | void predictFailureProbability(void) { 181 | std::vector validResidualErrors; 182 | std::vector validScanIndices; 183 | for (int i = 0; i < (int)residualErrors_.intensities.size(); ++i) { 184 | double e = residualErrors_.intensities[i]; 185 | if (0.0 <= e && e <= maxResidualError_) { 186 | validResidualErrors.push_back(e); 187 | validScanIndices.push_back(i); 188 | } 189 | } 190 | 191 | int validResidualErrorsSize = (int)validResidualErrors.size(); 192 | if (validResidualErrorsSize <= minValidResidualErrorsNum_) { 193 | std::cerr << "WARNING: Number of validResidualErrors is less than the expected threshold number." << 194 | " The threshold is " << minValidResidualErrorsNum_ << 195 | ", but the number of validResidualErrors " << validResidualErrorsSize << "." << std::endl; 196 | failureProbability_ = -1.0; 197 | return; 198 | } else if (validResidualErrorsSize <= maxResidualErrorsNum_) { 199 | usedResidualErrors_ = validResidualErrors; 200 | usedScanIndices_ = validScanIndices; 201 | } else { 202 | usedResidualErrors_.resize(maxResidualErrorsNum_); 203 | usedScanIndices_.resize(maxResidualErrorsNum_); 204 | for (int i = 0; i < maxResidualErrorsNum_; ++i) { 205 | int idx = rand() % (int)validResidualErrors.size(); 206 | usedResidualErrors_[i] = validResidualErrors[idx]; 207 | usedScanIndices_[i] = validScanIndices[idx]; 208 | validResidualErrors.erase(validResidualErrors.begin() + idx); 209 | validScanIndices.erase(validScanIndices.begin() + idx); 210 | } 211 | } 212 | 213 | std::vector> likelihoodVectors = getLikelihoodVectors(usedResidualErrors_); 214 | std::vector> measurementClassProbabilities = estimateMeasurementClassProbabilities(likelihoodVectors); 215 | setAllMeasurementClassProbabilities(usedResidualErrors_, measurementClassProbabilities); 216 | failureProbability_ = predictFailureProbabilityBySampling(measurementClassProbabilities); 217 | } 218 | 219 | void publishROSMessages(void) { 220 | geometry_msgs::Vector3Stamped failureProbability; 221 | failureProbability.header.stamp = residualErrors_.header.stamp; 222 | failureProbability.vector.x = failureProbability_; 223 | failureProbPub_.publish(failureProbability); 224 | 225 | if (publishClassifiedScans_) { 226 | std::vector residualErrorClasses = getResidualErrorClasses(); 227 | sensor_msgs::LaserScan alignedScan, misalignedScan, unknownScan; 228 | alignedScan.header = misalignedScan.header = unknownScan.header = residualErrors_.header; 229 | alignedScan.range_min = misalignedScan.range_min = unknownScan.range_min = residualErrors_.range_min; 230 | alignedScan.range_max = misalignedScan.range_max = unknownScan.range_max = residualErrors_.range_max; 231 | alignedScan.angle_min = misalignedScan.angle_min = unknownScan.angle_min = residualErrors_.angle_min; 232 | alignedScan.angle_max = misalignedScan.angle_max = unknownScan.angle_max = residualErrors_.angle_max; 233 | alignedScan.angle_increment = misalignedScan.angle_increment = unknownScan.angle_increment = residualErrors_.angle_increment; 234 | alignedScan.time_increment = misalignedScan.time_increment = unknownScan.time_increment = residualErrors_.time_increment; 235 | alignedScan.scan_time = misalignedScan.scan_time = unknownScan.scan_time = residualErrors_.scan_time; 236 | int size = (int)residualErrors_.ranges.size(); 237 | alignedScan.ranges.resize(size); 238 | misalignedScan.ranges.resize(size); 239 | unknownScan.ranges.resize(size); 240 | alignedScan.intensities.resize(size); 241 | misalignedScan.intensities.resize(size); 242 | unknownScan.intensities.resize(size); 243 | for (int i = 0; i < (int)usedResidualErrors_.size(); ++i) { 244 | int idx = usedScanIndices_[i]; 245 | if (residualErrorClasses[i] == ALIGNED) 246 | alignedScan.ranges[idx] = residualErrors_.ranges[idx]; 247 | else if (residualErrorClasses[i] == MISALIGNED) 248 | misalignedScan.ranges[idx] = residualErrors_.ranges[idx]; 249 | else 250 | unknownScan.ranges[idx] = residualErrors_.ranges[idx]; 251 | } 252 | alignedScanPub_.publish(alignedScan); 253 | misalignedScanPub_.publish(misalignedScan); 254 | unknownScanPub_.publish(unknownScan); 255 | } 256 | 257 | if (publishFailureProbabilityMarker_) { 258 | visualization_msgs::Marker marker; 259 | marker.header.frame_id = markerFrame_; 260 | marker.header.stamp = residualErrors_.header.stamp; 261 | marker.ns = "fp_marker_namespace"; 262 | marker.id = 0; 263 | marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 264 | marker.action = visualization_msgs::Marker::ADD; 265 | marker.pose.position.x = 0.0; 266 | marker.pose.position.y = -3.0; 267 | marker.pose.position.z = 0.0; 268 | marker.scale.x = 0.0; 269 | marker.scale.y = 0.0; 270 | marker.scale.z = 2.0; 271 | marker.text = "Failure Probability: " + std::to_string((int)(failureProbability_ * 100.0)) + " %"; 272 | marker.color.a = 1.0; 273 | marker.color.r = 1.0; 274 | marker.color.g = 1.0; 275 | marker.color.b = 1.0; 276 | if (failureProbability_ > 0.1) 277 | marker.color.r = marker.color.g = 0.0; 278 | failureProbabilityMarkerPub_.publish(marker); 279 | } 280 | } 281 | 282 | void printFailureProbability(void) { 283 | std::cout << "Failure probability = " << failureProbability_ * 100.0 << " [%]" << std::endl; 284 | } 285 | 286 | private: 287 | void residualErrorsCB(const sensor_msgs::LaserScan::ConstPtr &msg) { 288 | if (canUpdateResidualErrors_) 289 | residualErrors_ = *msg; 290 | if (!gotResidualErrors_) 291 | gotResidualErrors_ = true; 292 | } 293 | 294 | inline double calculateNormalDistribution(double e) { 295 | return (0.95 * (2.0 * NDNormConst_ * exp(-((e - NDMean_) * (e - NDMean_)) / (2.0 * NDVar_))) + 0.05 * (1.0 / maxResidualError_)) * residualErrorReso_; 296 | } 297 | 298 | inline double calculateExponentialDistribution(double e) { 299 | return (0.95 * (1.0 / (1.0 - exp(-EDLambda_ * maxResidualError_))) * EDLambda_ * exp(-EDLambda_ * e) + 0.05 * (1.0 / maxResidualError_)) * residualErrorReso_; 300 | } 301 | 302 | inline double calculateUniformDistribution(void) { 303 | return (1.0 / maxResidualError_) * residualErrorReso_; 304 | } 305 | 306 | inline double getSumOfVecotr(std::vector vector) { 307 | double sum = 0.0; 308 | for (int i = 0; i < (int)vector.size(); i++) 309 | sum += vector[i]; 310 | return sum; 311 | } 312 | 313 | inline std::vector getHadamardProduct(std::vector vector1, std::vector vector2) { 314 | for (int i = 0; i < (int)vector1.size(); i++) 315 | vector1[i] *= vector2[i]; 316 | return vector1; 317 | } 318 | 319 | inline std::vector normalizeVector(std::vector vector) { 320 | double sum = getSumOfVecotr(vector); 321 | for (int i = 0; i < (int)vector.size(); i++) 322 | vector[i] /= sum; 323 | return vector; 324 | } 325 | 326 | inline double getEuclideanNormOfDiffVectors(std::vector vector1, std::vector vector2) { 327 | double sum = 0.0; 328 | for (int i = 0; i < (int)vector1.size(); i++) { 329 | double diff = vector1[i] - vector2[i]; 330 | sum += diff * diff; 331 | } 332 | return sqrt(sum); 333 | } 334 | 335 | inline std::vector calculateTransitionMessage(std::vector probs) { 336 | std::vector message(3); 337 | std::vector tm = transitionProbMat_; 338 | message[ALIGNED] = tm[ALIGNED] * probs[ALIGNED] + tm[MISALIGNED] * probs[MISALIGNED] + tm[UNKNOWN] * probs[UNKNOWN]; 339 | message[MISALIGNED] = tm[ALIGNED + 3] * probs[ALIGNED] + tm[MISALIGNED + 3] * probs[MISALIGNED] + tm[UNKNOWN + 3] * probs[UNKNOWN]; 340 | message[UNKNOWN] = tm[ALIGNED + 6] * probs[ALIGNED] + tm[MISALIGNED + 6] * probs[MISALIGNED] + tm[UNKNOWN + 6] * probs[UNKNOWN]; 341 | return message; 342 | } 343 | 344 | std::vector> getLikelihoodVectors(std::vector validResidualErrors) { 345 | std::vector> likelihoodVectors((int)validResidualErrors.size()); 346 | double pud = calculateUniformDistribution(); 347 | for (int i = 0; i < (int)likelihoodVectors.size(); i++) { 348 | likelihoodVectors[i].resize(3); 349 | likelihoodVectors[i][ALIGNED] = calculateNormalDistribution(validResidualErrors[i]); 350 | likelihoodVectors[i][MISALIGNED] = calculateExponentialDistribution(validResidualErrors[i]); 351 | likelihoodVectors[i][UNKNOWN] = pud; 352 | likelihoodVectors[i] = normalizeVector(likelihoodVectors[i]); 353 | } 354 | return likelihoodVectors; 355 | } 356 | 357 | std::vector> estimateMeasurementClassProbabilities(std::vector> likelihoodVectors) { 358 | std::vector> measurementClassProbabilities = likelihoodVectors; 359 | for (int i = 0; i < (int)measurementClassProbabilities.size(); i++) { 360 | for (int j = 0; j < (int)measurementClassProbabilities.size(); j++) { 361 | if (i == j) 362 | continue; 363 | std::vector message = calculateTransitionMessage(likelihoodVectors[j]); 364 | measurementClassProbabilities[i] = getHadamardProduct(measurementClassProbabilities[i], message); 365 | measurementClassProbabilities[i] = normalizeVector(measurementClassProbabilities[i]); 366 | } 367 | measurementClassProbabilities[i] = normalizeVector(measurementClassProbabilities[i]); 368 | } 369 | 370 | double variation = 0.0; 371 | int idx1 = rand() % (int)measurementClassProbabilities.size(); 372 | std::vector message(3); 373 | message = likelihoodVectors[idx1]; 374 | int checkStep = maxLPBComputationNum_ / 20; 375 | for (int i = 0; i < maxLPBComputationNum_; i++) { 376 | int idx2 = rand() % (int)measurementClassProbabilities.size(); 377 | int cnt = 0; 378 | for (;;) { 379 | if (idx2 != idx1) 380 | break; 381 | idx2 = rand() % (int)measurementClassProbabilities.size(); 382 | cnt++; 383 | if (cnt >= 10) 384 | break; 385 | } 386 | message = calculateTransitionMessage(message); 387 | message = getHadamardProduct(likelihoodVectors[idx2], message); 388 | std::vector measurementClassProbabilitiesPrev = measurementClassProbabilities[idx2]; 389 | measurementClassProbabilities[idx2] = getHadamardProduct(measurementClassProbabilities[idx2], message); 390 | measurementClassProbabilities[idx2] = normalizeVector(measurementClassProbabilities[idx2]); 391 | double diffNorm = getEuclideanNormOfDiffVectors(measurementClassProbabilities[idx2], measurementClassProbabilitiesPrev); 392 | variation += diffNorm; 393 | if (i >= checkStep && i % checkStep == 0 && variation < 10e-6) 394 | break; 395 | else if (i >= checkStep && i % checkStep == 0) 396 | variation = 0.0; 397 | message = measurementClassProbabilities[idx2]; 398 | idx1 = idx2; 399 | } 400 | return measurementClassProbabilities; 401 | } 402 | 403 | double predictFailureProbabilityBySampling(std::vector> measurementClassProbabilities) { 404 | int failureCnt = 0; 405 | for (int i = 0; i < samplingNum_; i++) { 406 | int misalignedNum = 0, validMeasurementNum = 0; 407 | int measurementNum = (int)measurementClassProbabilities.size(); 408 | for (int j = 0; j < measurementNum; j++) { 409 | double darts = (double)rand() / ((double)RAND_MAX + 1.0); 410 | double validProb = measurementClassProbabilities[j][ALIGNED] + measurementClassProbabilities[j][MISALIGNED]; 411 | if (darts > validProb) 412 | continue; 413 | validMeasurementNum++; 414 | if (darts > measurementClassProbabilities[j][ALIGNED]) 415 | misalignedNum++; 416 | } 417 | double misalignmentRatio = (double)misalignedNum / (double)validMeasurementNum; 418 | double unknownRatio = (double)(measurementNum - validMeasurementNum) / (double)measurementNum; 419 | if (misalignmentRatio >= misalignmentRatioThreshold_ 420 | || unknownRatio >= unknownRatioThreshold_) 421 | failureCnt++; 422 | } 423 | double p = (double)failureCnt / (double)samplingNum_; 424 | return p; 425 | } 426 | 427 | void setAllMeasurementClassProbabilities(std::vector residualErrors, std::vector> measurementClassProbabilities) { 428 | measurementClassProbabilities_.resize((int)residualErrors.size()); 429 | int idx = 0; 430 | for (int i = 0; i < (int)measurementClassProbabilities_.size(); i++) { 431 | measurementClassProbabilities_[i].resize(3); 432 | if (0.0 <= residualErrors[i] && residualErrors[i] <= maxResidualError_) { 433 | measurementClassProbabilities_[i] = measurementClassProbabilities[idx]; 434 | idx++; 435 | } else { 436 | measurementClassProbabilities_[i][ALIGNED] = 0.00005; 437 | measurementClassProbabilities_[i][MISALIGNED] = 0.00005; 438 | measurementClassProbabilities_[i][UNKNOWN] = 0.9999; 439 | } 440 | } 441 | } 442 | 443 | std::vector getResidualErrorClasses(void) { 444 | int size = (int)measurementClassProbabilities_.size(); 445 | std::vector residualErrorClasses(size); 446 | for (int i = 0; i < size; i++) { 447 | double alignedProb = measurementClassProbabilities_[i][ALIGNED]; 448 | double misalignedProb = measurementClassProbabilities_[i][MISALIGNED]; 449 | double unknownProb = measurementClassProbabilities_[i][UNKNOWN]; 450 | if (alignedProb > misalignedProb && alignedProb > unknownProb) 451 | residualErrorClasses[i] = ALIGNED; 452 | else if (misalignedProb > alignedProb && misalignedProb > unknownProb) 453 | residualErrorClasses[i] = MISALIGNED; 454 | else 455 | residualErrorClasses[i] = UNKNOWN; 456 | } 457 | return residualErrorClasses; 458 | } 459 | }; // class MRFFD 460 | 461 | } // namespace als_ros 462 | 463 | #endif // __MRF_FAILURE_DETECTOR_H__ 464 | --------------------------------------------------------------------------------