diff --git a/.gitignore b/.gitignore
index 4f9c828fc38f880196ae198c5298798f83909e84..bd05f6d5f769c243b1115b6049f61f9e825644f0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,6 +2,8 @@
bin/
build*/
lib/
+doc/
+cmake_modules/
.settings/language.settings.xml
.project
.cproject
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index ba8f3c68db4d6f7f8e33420e6b35ddb1f09ac79a..5ef06947f65222b2d6cbfd52ef54c965633ee7ac 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -50,14 +50,17 @@ stages:
- if [ -f license_header_${CURRENT_YEAR}.txt ]; then
# add license headers to new files
- echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
- - ./license_manager.sh --add --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+ - ./license_manager.sh --add --path=${CI_PROJECT_DIR} --config-path=. --exclude=ci_deps
- else
- # update license headers of all files
+ # remove license headers of all files
+ - ./license_manager.sh --remove --path=${CI_PROJECT_DIR} --config-path=. --exclude=ci_deps
+ # update license header
- export PREV_YEAR=$(( CURRENT_YEAR-1 ))
- echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
- git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
- - sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
- - ./license_manager.sh --update --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+ - sed -i "s/${PREV_YEAR}/${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
+ # add new license headers to all files
+ - ./license_manager.sh --add --path=${CI_PROJECT_DIR} --config-path=. --exclude=ci_deps
- fi
- cd ..
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c83af3bd66ab395c465600fc2f604f04c1d453ed..d39a73dd07c153a8b2fa532be409da1ad1335648 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,14 +39,6 @@ IF(NOT BUILD_DEMOS)
OPTION(BUILD_DEMOS "Build demos" ON)
ENDIF(NOT BUILD_DEMOS)
-if(BUILD_TESTS)
- # Enables testing for this directory and below.
- # Note that ctest expects to find a test file in the build directory root.
- # Therefore, this command should be in the source directory root.
- #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
- enable_testing()
-endif()
-
# ============ DEPENDENCIES ============
FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
FIND_PACKAGE(falkolib QUIET)
@@ -104,6 +96,7 @@ SET(HDRS
SET(HDRS ${HDRS}
include/${PROJECT_NAME}/corner_falko_2d.h
include/${PROJECT_NAME}/loop_closure_falko.h
+ include/${PROJECT_NAME}/scene_falko_base.h
include/${PROJECT_NAME}/scene_falko.h
include/${PROJECT_NAME}/match_loop_closure_scene.h)
ENDIF(falkolib_FOUND)
@@ -171,7 +164,7 @@ ENDIF(BUILD_DEMOS)
#Build tests
IF(BUILD_TESTS)
MESSAGE("Building tests.")
- set(_LASER_SCAN_UTILS_ROOT_DIR ${CMAKE_SOURCE_DIR})
+ enable_testing()
add_subdirectory(test)
ENDIF(BUILD_TESTS)
diff --git a/examples/polyline_demo.cpp b/examples/polyline_demo.cpp
index 4bb848cab67a6a968fb76aaf79f7c57798ea43f9..7b7e095d1daa19b1cd8b8e06f64bb99a31b8ac1b 100644
--- a/examples/polyline_demo.cpp
+++ b/examples/polyline_demo.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,14 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file polyline_test.cpp
- *
- * Created on: Jul 5, 2016
- * \author: jvallve
- */
//LaserScanUtils includes
#include "laser_scan_utils/line_finder_iterative.h"
diff --git a/include/laser_scan_utils/corner_falko_2d.h b/include/laser_scan_utils/corner_falko_2d.h
index 94911a42cfc9a1344da03498eecfebf93dba936e..5137712846cc68aadde53602e9a21a5ac800f886 100644
--- a/include/laser_scan_utils/corner_falko_2d.h
+++ b/include/laser_scan_utils/corner_falko_2d.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,17 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file corner_falko_2d.h
- *
- * Created on: Jan 26, 2021
- * \author: spujol
- */
-#ifndef CORNER_FALKO_2D_H_
-#define CORNER_FALKO_2D_H_
+#pragma once
#include <fstream>
#include <iostream>
@@ -136,5 +127,3 @@ public:
};
} /* namespace laserscanutils */
-
-#endif /* LANDMARK_POLYLINE_2d_H_ */
diff --git a/include/laser_scan_utils/corner_finder.h b/include/laser_scan_utils/corner_finder.h
index fdab44af6e660639049b2a99926bdaecf5063556..43784596cb6ef4ddb2f9b3bbf54f7d2e0e2fbc36 100644
--- a/include/laser_scan_utils/corner_finder.h
+++ b/include/laser_scan_utils/corner_finder.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef CORNER_FINDER_H_
-#define CORNER_FINDER_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -120,4 +118,3 @@ class CornerFinder
};
} //namespace
-#endif
diff --git a/include/laser_scan_utils/corner_finder_inscribed_angle.h b/include/laser_scan_utils/corner_finder_inscribed_angle.h
index 6f16c9c01d6afdd5c43d73e1aae2406a5dd9c31f..db4f778fb120d6a7765ee1676806826df7f72849 100644
--- a/include/laser_scan_utils/corner_finder_inscribed_angle.h
+++ b/include/laser_scan_utils/corner_finder_inscribed_angle.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef CORNER_FINDER_INSCRIBED_ANGLE_H_
-#define CORNER_FINDER_INSCRIBED_ANGLE_H_
+
+#pragma once
//laserscanutils
#include "corner_finder.h"
@@ -104,4 +102,3 @@ class CornerFinderInscribedAngle : public CornerFinder
void print() const override;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/corner_point.h b/include/laser_scan_utils/corner_point.h
index a85ff032d7abb27f900704dfd3ca037262743cc8..4a7599fb0988543225ba25c0387d8204d5a67b65 100644
--- a/include/laser_scan_utils/corner_point.h
+++ b/include/laser_scan_utils/corner_point.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef CORNER_POINT_H_
-#define CORNER_POINT_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -92,4 +90,3 @@ class CornerPoint
void print() const;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/grid_2d.h b/include/laser_scan_utils/grid_2d.h
index 00b615435b793521e31a98cecfa5ff213cea7421..92479fe8b0bc7312336688ab95b3108bd37b84aa 100644
--- a/include/laser_scan_utils/grid_2d.h
+++ b/include/laser_scan_utils/grid_2d.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,11 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef GRID2D_H
-#define GRID2D_H
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -280,5 +277,3 @@ class Grid2D
};
}//end of namespace
-
-#endif
diff --git a/include/laser_scan_utils/grid_cluster.h b/include/laser_scan_utils/grid_cluster.h
index 026b62129957c2145f2cb75eb26e8345b2c80bf1..44551f6153bfaa1c6276c62993294e110854f52a 100644
--- a/include/laser_scan_utils/grid_cluster.h
+++ b/include/laser_scan_utils/grid_cluster.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef GRID_CLUSTER_H_
-#define GRID_CLUSTER_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -62,4 +60,3 @@ class GridCluster : public PointSet
void print() const override;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/icp.h b/include/laser_scan_utils/icp.h
index 71ba9678023671b5c2f3ceeb11887cfe39444a3a..f50bf39849dad7bc76ed550fb6c863169a8d291b 100644
--- a/include/laser_scan_utils/icp.h
+++ b/include/laser_scan_utils/icp.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,180 +17,218 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef ICP_H_
-#define ICP_H_
+
+#pragma once
#include "laser_scan.h"
#include <chrono>
#include <random>
#include <csm/csm_all.h>
-#undef max //undefine macro of csm that may interfere with std::max
-#undef min //undefine macro of csm that may interfere with std::min
-
-namespace laserscanutils{
-
-struct icpOutput{
- bool valid; // If the result is valid
- Eigen::Vector3s res_transf; // Transformation found
- Eigen::Matrix3s res_covar; // Covariance of the transformation
- int nvalid; // Number of valid correspondences in the match
- double error; // Total correspondence error
-};
+#undef max // undefine macro of csm that may interfere with std::max
+#undef min // undefine macro of csm that may interfere with std::min
-struct icpParams
+namespace laserscanutils
{
- bool verbose; // prints debug messages
-
- // Algorithm options ---------------------------------------------------
- bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false).
- double max_angular_correction_deg; // Maximum angular displacement between scans (deg)
- double max_linear_correction; // Maximum translation between scans (m)
-
- /** Maximum distance for a correspondence to be valid */
- double max_correspondence_dist;
- /** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
- bool use_corr_tricks;
- /** Checks that find_correspondences_tricks give the right answer */
- bool debug_verify_tricks;
-
- // Stopping criteria
- int max_iterations; // maximum iterations
- double epsilon_xy; // distance change
- double epsilon_theta; // angle change
-
- // Restart algorithm
- bool restart; // Enable restarting
- double restart_threshold_mean_error; // Threshold for restarting
- double restart_dt; // Displacement for restarting
- double restart_dtheta; // Displacement for restarting
-
- // Discarding points or correspondences ---------------------------------------------------
- /** discard rays outside of this interval */
- double min_reading, max_reading;
- /** Percentage of correspondences to consider: if 0.9,
- always discard the top 10% of correspondences with more error */
- double outliers_maxPerc;
-
- /** Parameters describing a simple adaptive algorithm for discarding.
- 1) Order the errors.
- 2) Choose the percentile according to outliers_adaptive_order.
- (if it is 0.7, get the 70% percentile)
- 3) Define an adaptive threshold multiplying outliers_adaptive_mult
- with the value of the error at the chosen percentile.
- 4) Discard correspondences over the threshold.
-
- This is useful to be conservative; yet remove the biggest errors.
- */
- double outliers_adaptive_order; // 0.7
- double outliers_adaptive_mult; // 2
-
- /** Do not allow two different correspondences to share a point */
- bool outliers_remove_doubles;
-
- /** If initial guess, visibility test can be done to discard points that are not visible */
- bool do_visibility_test;
-
- /** Discard correspondences based on the angles */
- bool do_alpha_test;
- double do_alpha_test_thresholdDeg;
-
- // Point orientation ------------------------------------------------------------------
- /** For now, a very simple max-distance clustering algorithm is used */
- double clustering_threshold;
- /** Number of neighbour rays used to estimate the orientation.*/
- int orientation_neighbourhood;
-
- // Weights ---------------------------------------------------------------------------
- /** If the field "true_alpha" is used to compute the incidence
- beta, and the factor (1/cos^2(beta)) used to weight the impact
- of each correspondence. This works fabolously if doing localization,
- that is the first scan has no noise.
- If "true_alpha" is not available, it uses "alpha".
- */
- bool use_ml_weights;
- /* If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
- bool use_sigma_weights;
- /** Noise in the scan */
- double sigma;
-
- // Covariance ------------------------------------------------------------------------
- bool do_compute_covariance; // Compute the matching covariance (method in http://purl.org/censi/2006/icpcov)
- double cov_factor; // Factor multiplying the cov output of csm
- double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm
-
- void print() const
+
+ struct icpOutput
+ {
+ bool valid; // If the result is valid (converged & mean_error < max_mean_error & points_ratio > min_points_ratio)
+ bool converged; // If the algorithm found a solution
+ Eigen::Vector3s res_transf; // Transformation found
+ Eigen::Matrix3s res_covar; // Covariance of the transformation
+ int nvalid; // Number of valid correspondences in the match
+ double error; // Total correspondence error
+ double mean_error; // Average error (total error / nvalid)
+ double points_ratio; // Ratio of valid correspondences of totall amount of points
+ unsigned int attempts; // Number of ICP calls to obtain a valid result (<= params.attempts)
+ };
+
+ struct icpParams
+ {
+ bool verbose; // prints debug messages
+
+ // Algorithm options ---------------------------------------------------
+ bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false).
+ double max_angular_correction_deg; // Maximum angular displacement between scans (deg)
+ double max_linear_correction; // Maximum translation between scans (m)
+
+ /** Maximum distance for a correspondence to be valid */
+ double max_correspondence_dist;
+ /** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
+ bool use_corr_tricks;
+ /** Checks that find_correspondences_tricks give the right answer */
+ bool debug_verify_tricks;
+
+ // Stopping criteria
+ int max_iterations; // maximum iterations
+ double epsilon_xy; // distance change
+ double epsilon_theta; // angle change
+
+ // Restart algorithm
+ bool restart; // Enable restarting
+ double restart_threshold_mean_error; // Threshold for restarting
+ double restart_dt; // Displacement for restarting
+ double restart_dtheta; // Displacement for restarting
+
+ // Discarding points or correspondences ---------------------------------------------------
+ /** discard rays outside of this interval */
+ double min_reading, max_reading;
+ /** Percentage of correspondences to consider: if 0.9,
+ always discard the top 10% of correspondences with more error */
+ double outliers_maxPerc;
+
+ /** Parameters describing a simple adaptive algorithm for discarding.
+ 1) Order the errors.
+ 2) Choose the percentile according to outliers_adaptive_order.
+ (if it is 0.7, get the 70% percentile)
+ 3) Define an adaptive threshold multiplying outliers_adaptive_mult
+ with the value of the error at the chosen percentile.
+ 4) Discard correspondences over the threshold.
+
+ This is useful to be conservative; yet remove the biggest errors.
+ */
+ double outliers_adaptive_order; // 0.7
+ double outliers_adaptive_mult; // 2
+
+ /** Do not allow two different correspondences to share a point */
+ bool outliers_remove_doubles;
+
+ /** If initial guess, visibility test can be done to discard points that are not visible */
+ bool do_visibility_test;
+
+ /** Discard correspondences based on the angles */
+ bool do_alpha_test;
+ double do_alpha_test_thresholdDeg;
+
+ // Point orientation ------------------------------------------------------------------
+ /** For now, a very simple max-distance clustering algorithm is used */
+ double clustering_threshold;
+ /** Number of neighbour rays used to estimate the orientation.*/
+ int orientation_neighbourhood;
+
+ // Weights ---------------------------------------------------------------------------
+ /** If the field "true_alpha" is used to compute the incidence
+ beta, and the factor (1/cos^2(beta)) used to weight the impact
+ of each correspondence. This works fabolously if doing localization,
+ that is the first scan has no noise.
+ If "true_alpha" is not available, it uses "alpha".
+ */
+ bool use_ml_weights;
+ /* If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
+ bool use_sigma_weights;
+ /** Noise in the scan */
+ double sigma;
+
+ // Covariance ------------------------------------------------------------------------
+ bool do_compute_covariance; // Compute the matching covariance (method in http://purl.org/censi/2006/icpcov)
+ double cov_factor; // Factor multiplying the cov output of csm
+ double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm
+
+ // Attempts ------------------------------------------------------------------
+ unsigned int attempts; // number of icp attempts if result fails (not valid or error > restart_threshold_mean_error)
+ double perturbation_new_attempts; // perturbation noise amplitude applied to initial guess in new attempts
+
+ // Validation ----------------------------------------------------------------
+ double max_mean_error; // mean_error threshold to consider the solution not valid
+ double min_points_ratio; // points ratio threshold to consider the solution valid
+
+ void print() const
+ {
+ std::cout << "verbose: " << std::to_string(verbose) << std::endl;
+ std::cout << "use_point_to_line_distance: " << std::to_string(use_point_to_line_distance) << std::endl;
+ std::cout << "max_angular_correction_deg: " << std::to_string(max_angular_correction_deg) << std::endl;
+ std::cout << "max_linear_correction: " << std::to_string(max_linear_correction) << std::endl;
+ std::cout << "max_correspondence_dist: " << std::to_string(max_correspondence_dist) << std::endl;
+ std::cout << "use_corr_tricks: " << std::to_string(use_corr_tricks) << std::endl;
+ std::cout << "debug_verify_tricks: " << std::to_string(debug_verify_tricks) << std::endl;
+ std::cout << "max_iterations: " << std::to_string(max_iterations) << std::endl;
+ std::cout << "epsilon_xy: " << std::to_string(epsilon_xy) << std::endl;
+ std::cout << "epsilon_theta: " << std::to_string(epsilon_theta) << std::endl;
+ std::cout << "restart: " << std::to_string(restart) << std::endl;
+ std::cout << "restart_threshold_mean_error: " << std::to_string(restart_threshold_mean_error) << std::endl;
+ std::cout << "restart_dt: " << std::to_string(restart_dt) << std::endl;
+ std::cout << "restart_dtheta: " << std::to_string(restart_dtheta) << std::endl;
+ std::cout << "min_reading: " << std::to_string(min_reading) << std::endl;
+ std::cout << "max_reading: " << std::to_string(max_reading) << std::endl;
+ std::cout << "outliers_maxPerc: " << std::to_string(outliers_maxPerc) << std::endl;
+ std::cout << "outliers_adaptive_order: " << std::to_string(outliers_adaptive_order) << std::endl;
+ std::cout << "outliers_adaptive_mult: " << std::to_string(outliers_adaptive_mult) << std::endl;
+ std::cout << "outliers_remove_doubles: " << std::to_string(outliers_remove_doubles) << std::endl;
+ std::cout << "do_visibility_test: " << std::to_string(do_visibility_test) << std::endl;
+ std::cout << "do_alpha_test: " << std::to_string(do_alpha_test) << std::endl;
+ std::cout << "do_alpha_test_thresholdDeg: " << std::to_string(do_alpha_test_thresholdDeg) << std::endl;
+ std::cout << "clustering_threshold: " << std::to_string(clustering_threshold) << std::endl;
+ std::cout << "orientation_neighbourhood: " << std::to_string(orientation_neighbourhood) << std::endl;
+ std::cout << "use_ml_weights: " << std::to_string(use_ml_weights) << std::endl;
+ std::cout << "use_sigma_weights: " << std::to_string(use_sigma_weights) << std::endl;
+ std::cout << "sigma: " << std::to_string(sigma) << std::endl;
+ std::cout << "do_compute_covariance: " << std::to_string(do_compute_covariance) << std::endl;
+ std::cout << "cov_factor: " << std::to_string(cov_factor) << std::endl;
+ std::cout << "cov_max_eigv_factor: " << std::to_string(cov_max_eigv_factor) << std::endl;
+ std::cout << "attempts: " << std::to_string(attempts) << std::endl;
+ std::cout << "perturbation_new_attempts: " << std::to_string(perturbation_new_attempts) << std::endl;
+ std::cout << "max_mean_error: " << std::to_string(max_mean_error) << std::endl;
+ std::cout << "min_points_ratio: " << std::to_string(min_points_ratio) << std::endl;
+ }
+ };
+
+ const icpParams icp_params_default = {
+ false, // bool verbose (prints debug messages)
+ true, // bool use_point_to_line_distance
+ 5.0, // double max_angular_correction_deg
+ 1, // double max_linear_correction
+ 0.5, // double max_correspondence_dist
+ false, // bool use_corr_tricks
+ false, // bool debug_verify_tricks
+ 50, // int max_iterations
+ 1e-4, // double epsilon_xy
+ 1e-3, // double epsilon_theta
+ false, // bool restart
+ 1e-1, // double restart_threshold_mean_error
+ 0, // double restart_dt
+ 0, // double restart_dtheta
+ 0.023, // double min_reading
+ 60, // max_reading
+ 1, // double outliers_maxPerc
+ 0.8, // double outliers_adaptive_order
+ 2, // double outliers_adaptive_mult
+ false, // bool outliers_remove_doubles
+ false, // bool do_visibility_test
+ false, // bool do_alpha_test
+ 10, // double do_alpha_test_thresholdDeg
+ 0.5, // double clustering_threshold
+ 4, // int orientation_neighbourhood
+ false, // bool use_ml_weights
+ false, // bool use_sigma_weights
+ 0.2, // double sigma
+ true, // bool do_compute_covariance
+ 5, // double cov_factor
+ 2, // double cov_max_eigv_factor
+ 1, // unsigned int attempts
+ 1e-1, // double perturbation_new_attempts
+ 1e-2, // max_mean_error
+ 0.75 // min_points_ratio
+ };
+
+ class ICP
{
- std::cout << "verbose: " << std::to_string(verbose) << std::endl;
- std::cout << "use_point_to_line_distance: " << std::to_string(use_point_to_line_distance) << std::endl;
- std::cout << "max_angular_correction_deg: " << std::to_string(max_angular_correction_deg) << std::endl;
- std::cout << "max_linear_correction: " << std::to_string(max_linear_correction) << std::endl;
- std::cout << "max_correspondence_dist: " << std::to_string(max_correspondence_dist) << std::endl;
- std::cout << "use_corr_tricks: " << std::to_string(use_corr_tricks) << std::endl;
- std::cout << "debug_verify_tricks: " << std::to_string(debug_verify_tricks) << std::endl;
- std::cout << "max_iterations: " << std::to_string(max_iterations) << std::endl;
- std::cout << "epsilon_xy: " << std::to_string(epsilon_xy) << std::endl;
- std::cout << "epsilon_theta: " << std::to_string(epsilon_theta) << std::endl;
- std::cout << "restart: " << std::to_string(restart) << std::endl;
- std::cout << "restart_threshold_mean_error: " << std::to_string(restart_threshold_mean_error) << std::endl;
- std::cout << "restart_dt: " << std::to_string(restart_dt) << std::endl;
- std::cout << "restart_dtheta: " << std::to_string(restart_dtheta) << std::endl;
- std::cout << "min_reading: " << std::to_string(min_reading) << std::endl;
- std::cout << "max_reading: " << std::to_string(max_reading) << std::endl;
- std::cout << "outliers_maxPerc: " << std::to_string(outliers_maxPerc) << std::endl;
- std::cout << "outliers_adaptive_order: " << std::to_string(outliers_adaptive_order) << std::endl;
- std::cout << "outliers_adaptive_mult: " << std::to_string(outliers_adaptive_mult) << std::endl;
- std::cout << "outliers_remove_doubles: " << std::to_string(outliers_remove_doubles) << std::endl;
- std::cout << "do_visibility_test: " << std::to_string(do_visibility_test) << std::endl;
- std::cout << "do_alpha_test: " << std::to_string(do_alpha_test) << std::endl;
- std::cout << "do_alpha_test_thresholdDeg: " << std::to_string(do_alpha_test_thresholdDeg) << std::endl;
- std::cout << "clustering_threshold: " << std::to_string(clustering_threshold) << std::endl;
- std::cout << "orientation_neighbourhood: " << std::to_string(orientation_neighbourhood) << std::endl;
- std::cout << "use_ml_weights: " << std::to_string(use_ml_weights) << std::endl;
- std::cout << "use_sigma_weights: " << std::to_string(use_sigma_weights) << std::endl;
- std::cout << "sigma: " << std::to_string(sigma) << std::endl;
- std::cout << "do_compute_covariance: " << std::to_string(do_compute_covariance) << std::endl;
- std::cout << "cov_factor: " << std::to_string(cov_factor) << std::endl;
- std::cout << "cov_max_eigv_factor: " << std::to_string(cov_max_eigv_factor) << std::endl;
- }
-};
-
-const icpParams icp_params_default = {
- false, //bool verbose; // prints debug messages
- true, 5.0, 1, // bool use_point_to_line_distance; double max_angular_correction_deg; double max_linear_correction;
- 0.5, false, false, // double max_correspondence_dist; bool use_corr_tricks; bool debug_verify_tricks;
- 50, 1e-4, 1e-3, // int max_iterations; double epsilon_xy; double epsilon_theta;
- false, 0, 0, 0, // bool restart; double restart_threshold_mean_error; double restart_dt; double restart_dtheta;
- 0.023, 60, // double min_reading, max_reading;
- 1, 0.8, 2, // double outliers_maxPerc; double outliers_adaptive_order; double outliers_adaptive_mult;
- false, false, false, 10, // bool outliers_remove_doubles; bool do_visibility_test; bool do_alpha_test; double do_alpha_test_thresholdDeg;
- 0.5, 4, // double clustering_threshold; int orientation_neighbourhood;
- false, false, 0.2, // bool use_ml_weights; bool use_sigma_weights; double sigma;
- true, 5, 2 // bool do_compute_covariance; double cov_factor; double cov_max_eigv_factor;
-};
-
-class ICP
-{
public:
ICP();
~ICP();
- static icpOutput align(const LaserScan &_current_ls,
- const LaserScan &_ref_ls,
- const LaserScanParams &_current_scan_params,
- const LaserScanParams &_ref_scan_params,
- const icpParams &_icp_params,
- const Eigen::Vector3s &_initial_guess);
- static icpOutput align(const LaserScan &_last_ls,
- const LaserScan &_reference_ls,
- const LaserScanParams &scan_params,
- const icpParams &icp_params,
- const Eigen::Vector3s &_initial_guess);
-
- static void printTwoLaserData(sm_params & params);
- static void printLaserData(LDP & laser_data);
- };
+ static icpOutput align(const LaserScan &_current_ls,
+ const LaserScan &_ref_ls,
+ const LaserScanParams &_current_scan_params,
+ const LaserScanParams &_ref_scan_params,
+ const icpParams &_icp_params,
+ const Eigen::Vector3s &_initial_guess);
+ static icpOutput align(const LaserScan &_last_ls,
+ const LaserScan &_reference_ls,
+ const LaserScanParams &scan_params,
+ const icpParams &icp_params,
+ const Eigen::Vector3s &_initial_guess);
+
+ static void printTwoLaserData(sm_params ¶ms);
+ static void printLaserData(LDP &laser_data);
+ };
}
-
-#endif
diff --git a/include/laser_scan_utils/laser_scan.h b/include/laser_scan_utils/laser_scan.h
index 144f031d4e46aa87dd7af207581f6d9470e162ba..397aec15e83eb7e615c98e2a05c7e925b4d125ad 100644
--- a/include/laser_scan_utils/laser_scan.h
+++ b/include/laser_scan_utils/laser_scan.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef LASER_SCAN_H_
-#define LASER_SCAN_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -195,5 +193,3 @@ class LaserScan
};
} //namespace
-#endif
-
diff --git a/include/laser_scan_utils/laser_scan_utils.h b/include/laser_scan_utils/laser_scan_utils.h
index b055283cb984abb36f27aa875fca148522fa59dc..a7270d8d36ac0b8300cf05e8feef149d2edb1b34 100644
--- a/include/laser_scan_utils/laser_scan_utils.h
+++ b/include/laser_scan_utils/laser_scan_utils.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,11 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef TYPES_LASER_SCAN_UTILS_H_
-#define TYPES_LASER_SCAN_UTILS_H_
+#pragma once
//includes from Eigen lib
#include <Eigen/Dense>
@@ -60,4 +57,3 @@ namespace Eigen
typedef Quaternion<laserscanutils::ScalarT> Quaternions; ///< Quaternion of real scalar_t type
typedef AngleAxis<laserscanutils::ScalarT> AngleAxiss; ///< Angle-Axis of real scalar_t type
}
-#endif
diff --git a/include/laser_scan_utils/laser_scan_with_params.h b/include/laser_scan_utils/laser_scan_with_params.h
index db93fc00cb13fa9794083f5607be3d989f460bff..c0144f5ff4782b5881e7dc73be39cb19ab6dfdf7 100644
--- a/include/laser_scan_utils/laser_scan_with_params.h
+++ b/include/laser_scan_utils/laser_scan_with_params.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef LASER_SCAN_WITH_PARAMS_H_
-#define LASER_SCAN_WITH_PARAMS_H_
+
+#pragma once
//laserscanutils
#include "laser_scan.h"
@@ -58,5 +56,3 @@ class LaserScanWithParams : public LaserScan
};
} //namespace
-#endif
-
diff --git a/include/laser_scan_utils/line_finder.h b/include/laser_scan_utils/line_finder.h
index 5696da18fa50106dd904a75648a8d6773d79af1a..73d0077b818f5b585d8524d993303322bd6cc84e 100644
--- a/include/laser_scan_utils/line_finder.h
+++ b/include/laser_scan_utils/line_finder.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef LINE_FINDER_H_
-#define LINE_FINDER_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -120,4 +118,3 @@ class LineFinder
virtual void print() const;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/line_finder_hough.h b/include/laser_scan_utils/line_finder_hough.h
index 1b4f8840bd5870458113263015b538cfb6354032..ae4fbc727be9edd067695409a3a7580eb01db6f3 100644
--- a/include/laser_scan_utils/line_finder_hough.h
+++ b/include/laser_scan_utils/line_finder_hough.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef LINE_FINDER_HOUGH_H_
-#define LINE_FINDER_HOUGH_H_
+
+#pragma once
//laserscanutils
#include "line_finder.h"
@@ -111,4 +109,3 @@ class LineFinderHough : public LineFinder
void print() const override;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/line_finder_iterative.h b/include/laser_scan_utils/line_finder_iterative.h
index c68055cbb5f39f81aba83ac3833861df1a1f962e..d52b42396e459a597779682e88dfd6ae88494b6f 100644
--- a/include/laser_scan_utils/line_finder_iterative.h
+++ b/include/laser_scan_utils/line_finder_iterative.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef LINE_FINDER_ITERATIVE_H_
-#define LINE_FINDER_ITERATIVE_H_
+
+#pragma once
//laserscanutils
#include "line_finder.h"
@@ -195,4 +193,3 @@ class LineFinderIterative : public LineFinder
};
} //namespace
-#endif
diff --git a/include/laser_scan_utils/line_finder_jump_fit.h b/include/laser_scan_utils/line_finder_jump_fit.h
index ed3acb1061a5e12fe82d760ab6516c63ad778656..c60c28a7ee98506ce9826494dc94d1ea1f0c738c 100644
--- a/include/laser_scan_utils/line_finder_jump_fit.h
+++ b/include/laser_scan_utils/line_finder_jump_fit.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef LINE_FINDER_JUMP_FIT_H_
-#define LINE_FINDER_JUMP_FIT_H_
+
+#pragma once
//laserscanutils
#include "line_finder.h"
@@ -106,4 +104,3 @@ class LineFinderJumpFit : public LineFinder
void print() const override;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/line_segment.h b/include/laser_scan_utils/line_segment.h
index 6b5b28a9eb73c939bf88ce5f4afdf45fe4c5383a..7951f678c19aeb8e315b9238de667601325f7ed1 100644
--- a/include/laser_scan_utils/line_segment.h
+++ b/include/laser_scan_utils/line_segment.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef LINE_SEGMENT_H_
-#define LINE_SEGMENT_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -107,4 +105,3 @@ class LineSegment
void print() const;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/loop_closure_base.h b/include/laser_scan_utils/loop_closure_base.h
index f509f869f81602860a33740618b35f622266f883..644d55b43a05341ba9b211ce24433d7c28b610eb 100644
--- a/include/laser_scan_utils/loop_closure_base.h
+++ b/include/laser_scan_utils/loop_closure_base.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,17 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file loop_closure_base_2d.h
- *
- * Created on: Feb 9, 2021
- * \author: spujol
- */
-#ifndef LOOP_CLOSURE_BASE_2D_H_
-#define LOOP_CLOSURE_BASE_2D_H_
+#pragma once
#include <fstream>
#include <iostream>
@@ -40,50 +31,50 @@
#include "match_loop_closure_scene.h"
#include "scene_base.h"
-namespace laserscanutils {
-
-/** \brief A 2base class for loop closure using falko library
- */
-class LoopClosureBase2d
+namespace laserscanutils
{
- private:
- public:
- /** \brief Constructor
- **/
- LoopClosureBase2d(){};
+ class LoopClosureBase2d;
+ typedef std::shared_ptr<LoopClosureBase2d> LoopClosureBase2dPtr;
+
+ /** \brief A 2base class for loop closure using falko library
+ */
+ class LoopClosureBase2d
+ {
+ public:
+ /** \brief Constructor
+ **/
+ LoopClosureBase2d(){};
- /** \brief Destructor
- **/
- virtual ~LoopClosureBase2d(){};
+ /** \brief Destructor
+ **/
+ virtual ~LoopClosureBase2d(){};
- /** \brief update the scene struct with keypoints and descriptors
- **/
- virtual sceneBasePtr extractScene(const LaserScan &scan, const LaserScanParams &scanParams) = 0;
+ /** \brief update the scene struct with keypoints and descriptors
+ **/
+ virtual sceneBasePtr extractScene(const LaserScan &scan, const LaserScanParams &scanParams) = 0;
- /** \brief Create and update a matchLoopClosure struct with the info that is
- *produced when matching two given scenes
- **/
- virtual MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene1, sceneBasePtr _scene2) = 0;
+ /** \brief Create and update a matchLoopClosure struct with the info that is
+ *produced when matching two given scenes
+ **/
+ virtual MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene1, sceneBasePtr _scene2) = 0;
- /** \brief It matches a target scene against a list of references scenes in order to find loop
- * closures
- **/
- virtual std::map<double, MatchLoopClosureScenePtr> findLoopClosure(std::list<std::shared_ptr<SceneBase>> _l_scenes,
- const sceneBasePtr _new_scene)
- {
- std::map<double, MatchLoopClosureScenePtr> matchings;
- for (auto ref_scene : _l_scenes)
+ /** \brief It matches a target scene against a list of references scenes in order to find loop
+ * closures
+ **/
+ virtual std::map<double, MatchLoopClosureScenePtr> findLoopClosure(std::list<std::shared_ptr<SceneBase>> _l_scenes,
+ const sceneBasePtr _new_scene)
+ {
+ std::map<double, MatchLoopClosureScenePtr> matchings;
+ for (auto ref_scene : _l_scenes)
{
auto match = matchScene(ref_scene, _new_scene);
-
- while (matchings.find(match->score)!=matchings.end())
- match->score+=0.001;
+
+ while (matchings.find(match->score) != matchings.end())
+ match->score += 0.001;
matchings.emplace(match->score, match);
}
- return matchings;
- }
-};
+ return matchings;
+ }
+ };
} /* namespace laserscanutils */
-
-#endif /* LOOP_CLOSURE_BASE_2D_H_ */
diff --git a/include/laser_scan_utils/loop_closure_falko.h b/include/laser_scan_utils/loop_closure_falko.h
index c5b3e1d2a104118f594ab4c59c929a9213574905..942ce53e25670f3459e83559a9a7814e97140205 100644
--- a/include/laser_scan_utils/loop_closure_falko.h
+++ b/include/laser_scan_utils/loop_closure_falko.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,17 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file loop_closure_base_2d.h
- *
- * Created on: Feb 9, 2021
- * \author: spujol
- */
-#ifndef LOOP_CLOSURE_FALKO_H_
-#define LOOP_CLOSURE_FALKO_H_
+#pragma once
#include <fstream>
#include <iostream>
@@ -540,5 +531,3 @@ template <class D, class Extr> class LoopClosureFalkoNn : public LoopClosureFalk
};
} /* namespace laserscanutils */
-
-#endif /* LOOP_CLOSURE_FALKO_H_ */
diff --git a/include/laser_scan_utils/match_loop_closure_scene.h b/include/laser_scan_utils/match_loop_closure_scene.h
index 98053932e2f9f7b81b16c668a4d5b39c7e2bdb1b..8e7607c7001326fe23d36eec376469a47d863bb9 100644
--- a/include/laser_scan_utils/match_loop_closure_scene.h
+++ b/include/laser_scan_utils/match_loop_closure_scene.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,17 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file match_loop_closure.h
- *
- * Created on: Feb 15, 2021
- * \author: spujol
- */
-#ifndef MATCH_LOOP_CLOSURE_SCENE_H_
-#define MATCH_LOOP_CLOSURE_SCENE_H_
+#pragma once
#include <fstream>
#include <iostream>
@@ -53,5 +44,3 @@ struct MatchLoopClosureScene {
typedef std::shared_ptr<MatchLoopClosureScene> MatchLoopClosureScenePtr;
} /* namespace laserscanutils */
-
-#endif /* MATCH_LOOP_CLOSURE_SCENE_H_ */
diff --git a/include/laser_scan_utils/point_set.h b/include/laser_scan_utils/point_set.h
index 1dc8b8e90ad819f588782693c8f451f479557ee8..1c3d8c0c279a0155444620dce0b8852ba6ec932e 100644
--- a/include/laser_scan_utils/point_set.h
+++ b/include/laser_scan_utils/point_set.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef POINT_SET_H_
-#define POINT_SET_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -131,4 +129,3 @@ class PointSet
virtual void print() const;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/polyline.h b/include/laser_scan_utils/polyline.h
index 03012f69ae513ad05b1333e4341b52f0d227a525..17702491b9513683e54c189ec3b8d9b9b7930a72 100644
--- a/include/laser_scan_utils/polyline.h
+++ b/include/laser_scan_utils/polyline.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef POLYLINE_H_
-#define POLYLINE_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -63,4 +61,3 @@ class Polyline
void computeLines();
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/scan_segment.h b/include/laser_scan_utils/scan_segment.h
index 21520f7bc801dbf295733ca0d62fe961569866bc..f4c0ef890939eec2465936fd620076aee9e40f68 100644
--- a/include/laser_scan_utils/scan_segment.h
+++ b/include/laser_scan_utils/scan_segment.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,10 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef SCAN_SEGMENT_H_
-#define SCAN_SEGMENT_H_
+
+#pragma once
//laserscanutils
#include "laser_scan_utils.h"
@@ -67,4 +65,3 @@ class ScanSegment : public PointSet
void print() const override;
};
}//namespace
-#endif
diff --git a/include/laser_scan_utils/scene_base.h b/include/laser_scan_utils/scene_base.h
index cb3d2220ca1e1cb4105e3213371c09f0d7cf546f..21ae35149205c59d99cfee8b94edc2563d08022d 100644
--- a/include/laser_scan_utils/scene_base.h
+++ b/include/laser_scan_utils/scene_base.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,17 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file scene_base.h
- *
- * Created on: Feb 9, 2021
- * \author: spujol
- */
-#ifndef SCENE_BASE_H_
-#define SCENE_BASE_H_
+#pragma once
#include <fstream>
#include <iostream>
@@ -51,5 +42,3 @@ struct SceneBase
typedef std::shared_ptr<SceneBase> sceneBasePtr;
} /* namespace laserscanutils */
-
-#endif /* SCENE_BASE_H_ */
diff --git a/include/laser_scan_utils/scene_falko.h b/include/laser_scan_utils/scene_falko.h
index 9719b465b607ba404e4f1c8b87ea9d6293841f5a..2fcb5cfc01bed5e2f781ab8efc99770497098e2a 100644
--- a/include/laser_scan_utils/scene_falko.h
+++ b/include/laser_scan_utils/scene_falko.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,17 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file scene_falko.h
- *
- * Created on: Feb 9, 2021
- * \author: spujol
- */
-#ifndef SCENE_FALKO_H_
-#define SCENE_FALKO_H_
+#pragma once
#include <fstream>
#include <iostream>
@@ -38,7 +29,7 @@
/**************************
* LaserScanUtils includes *
**************************/
-#include "scene_base.h"
+#include "scene_falko_base.h"
/**************************
* Falko includes *
@@ -53,18 +44,11 @@ namespace laserscanutils {
typedef falkolib::BSC bsc;
typedef falkolib::CGH cgh;
-template <typename D> struct SceneFalko : public SceneBase
+template <typename D> struct SceneFalko : public SceneFalkoBase
{
- std::vector<falkolib::FALKO> keypoints_list_;
- std::vector<falkolib::FALKO> keypoints_list_mid_point_;
- std::vector<falkolib::FALKO> keypoints_list_transl_rot_;
- std::vector<falkolib::FALKO> keypoints_list_rotated_;
- std::vector<falkolib::FALKO> keypoints_list_rotated_reverse_;
std::vector<D> descriptors_list_;
std::vector<D> descriptors_list_rotated;
std::vector<double> angle_rotation_;
};
} /* namespace laserscanutils */
-
-#endif /* SCENE_FALKO_H_ */
diff --git a/include/laser_scan_utils/scene_falko_base.h b/include/laser_scan_utils/scene_falko_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b78e73646e8dac7c4bd0dbcfb9d9718cff71d5e
--- /dev/null
+++ b/include/laser_scan_utils/scene_falko_base.h
@@ -0,0 +1,51 @@
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <fstream>
+#include <iostream>
+#include <iterator>
+#include <list>
+#include <memory>
+
+/**************************
+ * LaserScanUtils includes *
+ **************************/
+#include "scene_base.h"
+
+/**************************
+ * Falko includes *
+ **************************/
+
+#include <falkolib/Feature/FALKO.h>
+
+namespace laserscanutils {
+
+struct SceneFalkoBase : public SceneBase
+{
+ std::vector<falkolib::FALKO> keypoints_list_;
+ std::vector<falkolib::FALKO> keypoints_list_mid_point_;
+ std::vector<falkolib::FALKO> keypoints_list_transl_rot_;
+ std::vector<falkolib::FALKO> keypoints_list_rotated_;
+ std::vector<falkolib::FALKO> keypoints_list_rotated_reverse_;
+};
+
+} /* namespace laserscanutils */
diff --git a/scripts/license_header_2024.txt b/scripts/license_header_2025.txt
similarity index 61%
rename from scripts/license_header_2024.txt
rename to scripts/license_header_2025.txt
index 79c7f8e83b1e45193da1eef04ebc4df4a4c6a9ba..a8d222be67c589cbe37671da1f2d2bef42b8b94f 100644
--- a/scripts/license_header_2024.txt
+++ b/scripts/license_header_2025.txt
@@ -1,12 +1,19 @@
+<<<<<<<< HEAD:scripts/license_header_2024.txt
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+========
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
+>>>>>>>> origin/HEAD:scripts/license_header_2025.txt
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
diff --git a/scripts/license_manager.sh b/scripts/license_manager.sh
index 74f790a95133bb09f07b016ed55c3a7b67e2a9bc..888b156d3621ff51942080fd32649f0c4c8920a8 100755
--- a/scripts/license_manager.sh
+++ b/scripts/license_manager.sh
@@ -4,9 +4,7 @@
#
# This script is used for managing the license headers of code files (.h, .c, .cpp, .hpp)
# This file is automatically called by the CI in gitlab.
-
-line_start_mark="//--------LICENSE_START--------"
-line_end_mark="//--------LICENSE_END--------"
+echo "==== WOLF license_manager script ===="
#options
tmp=false
@@ -14,7 +12,10 @@ mode="none"
path=""
exclude_folder=""
#recursive=true
-license=""
+#license=""
+config_folder=""
+license_exclude=""
+exclude_mark="// This is not part of LaserScanUtils"
for i in "$@"; do
case $i in
@@ -26,22 +27,22 @@ for i in "$@"; do
path="${i#*=}"
shift # past argument=value
;;
- --license-header=*)
- license="${i#*=}"
+ --config-path=*)
+ config_folder="${i#*=}"
shift # past argument=value
;;
--add)
mode="add"
- if [ $mode == "update" ]; then
- echo "Error: Script cannot be called with both options: --update or --add"
+ if [ $mode == "remove" ]; then
+ echo "Script cannot be called with both options: --remove or --add">&2
exit 1
fi
shift # past argument=value
;;
- --update)
- mode="update"
+ --remove)
+ mode="remove"
if [ $mode == "add" ]; then
- echo "Error: Script cannot be called with both options: --update or --add"
+ echo "Script cannot be called with both options: --remove or --add">&2
exit 1
fi
shift # past argument=value
@@ -56,29 +57,45 @@ for i in "$@"; do
esac
done
-# check options
+# CHECKS VALID OPTIONS =================================
if [ "$path" == "" ]; then
- echo 'Please, provide the path to the folder containing the code with --path=/my/folder/code'
+ echo 'Please, provide the path to the folder containing the code with --path=/my/folder/code'>&2
exit 1
else
if [ -d "$path" ]; then
echo "Valid path: ${path}"
else
- echo "Error: ${path} not found. Can not continue."
- exit 1
+ echo "${path} not found. Can not continue.">&2
+ exit 1
fi
fi
-if [ "$license" == "" ]; then
- echo 'Error: Please, provide the path to the folder containing the code with --license-header=/my/license/header/file.txt'
+if [ "$config_folder" == "" ]; then
+ echo 'Please, provide the path to the folder containing the config files (license file "license_header*.txt" and the optional "license_exclude.txt") with --config-path=/my/license/and/exclude/files/folder'>&2
exit 1
else
- if [ -f "$license" ]; then
- echo "Valid license header file: ${license} containing:"
- cat ${license}
+ if [ -d "$config_folder" ]; then
+ echo "Valid config folder: ${config_folder}"
+
+ license=$(find $config_folder -maxdepth 1 -name 'license_header*.txt')
+ if (( ${#license[@]} != 1 )); then
+ echo "No license header file found" >&2
+ fi
+ if [ -f "$license" ]; then
+ echo "Valid license header file: ${license} containing:"
+ cat ${license}
+ else
+ echo "License header file ${license} not found.">&2
+ exit 1
+ fi
+ if [ -f "${config_folder}/license_exclude.txt" ]; then
+ echo "License_exclude file containing:"
+ cat ${config_folder}/license_exclude.txt
+ echo ""
+ fi
else
- echo "Error: License header file ${license} not found. Can not continue."
- exit 1
+ echo "Config path ${config_folder} not found.">&2
+ exit 1
fi
fi
@@ -86,17 +103,30 @@ if [ "$exclude_folder" == "" ]; then
echo "No folders will be excluded"
else
if [ -d "${path}/${exclude_folder}" ]; then
- echo "Valid remove folder path: ${path}/${exclude_folder}"
+ echo "Valid exclude folder path: ${path}/${exclude_folder}"
+ else
+ echo "exclude folder ${path}/${exclude_folder} not found. Remember that it should be relative to --path.">&2
+ exit 1
fi
fi
if [ $mode == "none" ]; then
- echo "Error: Script should be called with one of the following options: --update or --add"
+ echo "Script should be called with one of the following options: --remove or --add">&2
exit 1
else
echo "mode: ${mode}"
fi
+# START SCRIPT ==========================================
+
+# DETECT FIRST AND LAST LICENSE LINES
+sed -i -e '/./,$!d' -e :a -e '/^\n*$/{$d;N;ba' -e '}' $license #remove empty lines at the beginning and at the end
+line_start_mark=$(head -n 1 $license)
+line_end_mark=$(tail -n 1 $license)
+echo "line_start: $line_start_mark"
+echo "line_end: $line_end_mark"
+echo "excluding files starting with: $exclude_mark"
+
# PATH (AND tmp FOLDER)
folder=$path
if [ $tmp == true ]; then
@@ -110,35 +140,47 @@ fi
if [ "$exclude_folder" == "" ]; then
file_list=$(find $folder -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
else
- file_list=$(find $folder -path ${folder}/${exclude_folder} -prune -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
+ file_list=$(find $folder -path ${path}/${exclude_folder} -prune -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
fi
+# FILTER FILES IF LICENSE EXCLUDE
+if [ -f "${config_folder}/license_exclude.txt" ]; then
+ file_list=$(grep -Ev -f ${config_folder}/license_exclude.txt <(printf "%s\n" "${file_list[@]}"))
+fi
+# printf "%s\n" "${file_list[@]}"
+
# DETECT AND REMOVE EXISTING LICENSE
-if [ "$mode" == "update" ]; then
- echo "Recursely removing license header from all files (.c, .cpp, .h, .hpp)"
+if [ "$mode" == "remove" ]; then
+ echo "Recursely removing license header from all files (.c, .cpp, .h, .hpp):"
for i in $file_list
do
- if grep -Fxq ${line_start_mark} $i
- then
- echo " Removing license header from file ${i}"
- line_start="$(grep -wn $line_start_mark ${i} | head -n 1 | cut -d: -f1)"
- line_end="$(grep -wn $line_end_mark ${i} | head -n 1 | cut -d: -f1)"
- #echo ${line_start}
- #echo ${line_end}
+ if grep -m1 -Fxq "${line_start_mark}" $i; then
+ echo " - ${i}"
+ line_start="$(grep -Fxn "$line_start_mark" ${i} | head -n 1 | cut -d: -f1)"
+ line_end="$(grep -Fxn "$line_end_mark" ${i} | head -n 1 | cut -d: -f1)"
+ # echo ${line_start}
+ # echo ${line_end}
awk -v m=$line_start -v n=$line_end 'm <= NR && NR <= n {next} {print}' $i > tmpfile && mv tmpfile $i
- #cat $i
+ # cat $i
fi
done
fi
# ADD CONTENT OF license-file AT THE BEGINNING OF CODE FILES
-echo "Recursively adding license header to all files (.c, .cpp, .h, .hpp)"
-for i in $file_list
-do
- if grep -Fxq ${line_start_mark} $i; then
- echo "skippping ${i}"
- else
- ( echo ${line_start_mark}$'\n//'; cat ${license}; echo $'//\n'${line_end_mark}; cat $i ) > temp_file
- mv temp_file $i
- fi
-done
+if [ "$mode" == "add" ]; then
+ echo "Recursively adding license header to all files (.c, .cpp, .h, .hpp):"
+ for i in $file_list
+ do
+ if grep -m1 -Fxq "${exclude_mark}" $i; then
+ echo "excluding ${i} since it contains exclude mark"
+ else
+ if grep -m1 -Fxq "${line_start_mark}" $i; then
+ :
+ else
+ echo " - ${i}"
+ ( cat ${license}; cat $i ) > temp_file
+ mv temp_file $i
+ fi
+ fi
+ done
+fi
diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp
index 1b49e3316eaaa757e39ddda3ab15e6e461e8deee..b501a995c6233e790fddc393046727dc6e654c5d 100644
--- a/src/corner_falko_2d.cpp
+++ b/src/corner_falko_2d.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,14 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file corner_falko_2d.cpp
- *
- * Created on: Jan 26, 2021
- * \author: spujol
- */
#include "laser_scan_utils/corner_falko_2d.h"
diff --git a/src/corner_finder.cpp b/src/corner_finder.cpp
index 023b6cd817a1d140b5c5dbf481da9b26793a9459..68aaf99be7a86b43598e210c7123035fec201413 100644
--- a/src/corner_finder.cpp
+++ b/src/corner_finder.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/corner_finder.h"
namespace laserscanutils
diff --git a/src/corner_finder_inscribed_angle.cpp b/src/corner_finder_inscribed_angle.cpp
index 5e8d7e4823504f8950ee685637a1b56ec49012c3..2fabedec12e61ea124373c2eb6e508a172431790 100644
--- a/src/corner_finder_inscribed_angle.cpp
+++ b/src/corner_finder_inscribed_angle.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/corner_finder_inscribed_angle.h"
namespace laserscanutils
diff --git a/src/corner_point.cpp b/src/corner_point.cpp
index ebc5338616e21e0373211b7ecf95941f809aaf0b..6b82c682aec69a94ea1047dafd36ddfdc26804ed 100644
--- a/src/corner_point.cpp
+++ b/src/corner_point.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/corner_point.h"
//open namespace
diff --git a/src/grid_2d.cpp b/src/grid_2d.cpp
index a9cafde5fd2d3ae32624c43bc62602949d133b31..49282c0d064a044f517b29741e5e8a1b4ac912a2 100644
--- a/src/grid_2d.cpp
+++ b/src/grid_2d.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/grid_2d.h"
diff --git a/src/grid_cluster.cpp b/src/grid_cluster.cpp
index aa129436facb83c3b02a58c923cab7b7e0e7d6f2..288a5d6289da4b8c44f1f25524162e1f5e31be58 100644
--- a/src/grid_cluster.cpp
+++ b/src/grid_cluster.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/grid_cluster.h"
namespace laserscanutils
diff --git a/src/icp.cpp b/src/icp.cpp
index 354a905d86521fac7a72650b6cac461a32849e03..14f5c0185fe127f9d43095139bd4ce16d27fdec1 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/icp.h"
#include <algorithm>
#include <unistd.h>
@@ -26,66 +25,63 @@
using namespace laserscanutils;
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
-std::mt19937 generator (seed);
+std::mt19937 generator(seed);
std::uniform_real_distribution<double> dis(0.0, 1.0);
class LDWrapper
{
- public:
- LDP laser_data;
+public:
+ LDP laser_data;
- LDWrapper(const LaserScan& scan, const LaserScanParams& scan_params)
+ LDWrapper(const LaserScan &scan, const LaserScanParams &scan_params)
+ {
+ int num_rays = scan.ranges_raw_.size();
+ laser_data = ld_alloc_new(num_rays);
+ int i = 0;
+ for (auto it : scan.ranges_raw_)
{
- int num_rays = scan.ranges_raw_.size();
- laser_data = ld_alloc_new(num_rays);
- int i = 0;
- for(auto it : scan.ranges_raw_)
- {
- laser_data->theta[i] = scan_params.angle_min_ + i*scan_params.angle_step_;
+ laser_data->theta[i] = scan_params.angle_min_ + i * scan_params.angle_step_;
- if (scan_params.range_min_ <= it and
- it <= scan_params.range_max_ and
- 0 < it and it < 100)
- {
- laser_data->readings[i] = it;
- laser_data->valid[i] = 1;
- }
- else
- {
- laser_data->readings[i] = NAN;
- laser_data->valid[i] = 0;
- }
- laser_data->cluster[i] = -1;
- ++i;
+ if (scan_params.range_min_ <= it and
+ it <= scan_params.range_max_ and
+ 0 < it and it < 100)
+ {
+ laser_data->readings[i] = it;
+ laser_data->valid[i] = 1;
}
+ else
+ {
+ laser_data->readings[i] = NAN;
+ laser_data->valid[i] = 0;
+ }
+ laser_data->cluster[i] = -1;
+ ++i;
+ }
- laser_data->min_theta = laser_data->theta[0];
- laser_data->max_theta = laser_data->theta[num_rays-1];
-
- laser_data->odometry[0] = 0.0;
- laser_data->odometry[1] = 0.0;
- laser_data->odometry[2] = 0.0;
+ laser_data->min_theta = laser_data->theta[0];
+ laser_data->max_theta = laser_data->theta[num_rays - 1];
- laser_data->true_pose[0] = 0.0;
- laser_data->true_pose[1] = 0.0;
- laser_data->true_pose[2] = 0.0;
+ laser_data->odometry[0] = 0.0;
+ laser_data->odometry[1] = 0.0;
+ laser_data->odometry[2] = 0.0;
- }
+ laser_data->true_pose[0] = 0.0;
+ laser_data->true_pose[1] = 0.0;
+ laser_data->true_pose[2] = 0.0;
+ }
- ~LDWrapper()
- {
- ld_free(laser_data);
- }
+ ~LDWrapper()
+ {
+ ld_free(laser_data);
+ }
};
ICP::ICP()
{
-
}
ICP::~ICP()
{
-
}
icpOutput ICP::align(const LaserScan &_current_ls,
@@ -147,22 +143,73 @@ icpOutput ICP::align(const LaserScan &_current_ls,
csm_input.use_ml_weights = _icp_params.use_ml_weights ? 1 : 0;
csm_input.use_sigma_weights = _icp_params.use_sigma_weights ? 1 : 0;
- try
- {
- sm_icp(&csm_input, &csm_output);
- }
- catch(...)
+ icpOutput result{};
+ result.attempts = 0;
+ do
{
- icpOutput result{};
- result.valid = false;
- return result;
- }
+ // perturb from 2nd attempt
+ if (result.attempts > 0)
+ {
+ Eigen::Vector3d initial_guess_perturbed = _initial_guess + Eigen::Vector3d::Random() * _icp_params.perturbation_new_attempts;
+ csm_input.first_guess[0] = initial_guess_perturbed(0);
+ csm_input.first_guess[1] = initial_guess_perturbed(1);
+ csm_input.first_guess[2] = initial_guess_perturbed(2);
+ }
- icpOutput result{};
- result.nvalid = csm_output.nvalid;
- result.valid = csm_output.valid == 1;
- result.error = csm_output.error;
+ // call icp
+ result.attempts++;
+ if (_icp_params.verbose)
+ {
+ std::cout << "Calling ICP, attempt: " << result.attempts << std::endl;
+ }
+ try
+ {
+ sm_icp(&csm_input, &csm_output);
+ result.converged = csm_output.valid == 1;
+ result.valid = result.converged;
+ // Further validation
+ if (result.converged)
+ {
+ result.nvalid = csm_output.nvalid;
+ result.error = csm_output.error;
+ result.mean_error = csm_output.error / csm_output.nvalid;
+ result.points_ratio = ((double)csm_output.nvalid) / ((double)num_rays);
+
+ if (result.mean_error > _icp_params.max_mean_error or
+ result.points_ratio < _icp_params.min_points_ratio)
+ {
+ result.valid = false;
+ if (_icp_params.verbose)
+ std::cout << "Validation NOT passed! mean_error: "
+ << result.mean_error
+ << " (should be < "
+ << _icp_params.max_mean_error
+ << "). points_ratio: "
+ << result.points_ratio
+ << " (should be > "
+ << _icp_params.min_points_ratio
+ << ")." << std::endl;
+ }
+ }
+ }
+ catch (...)
+ {
+ if (_icp_params.verbose)
+ std::cout << "ICP failed (catch)" << std::endl;
+
+ result.converged = false;
+ result.valid = false;
+ }
+
+ if (_icp_params.verbose and not result.valid)
+ {
+ std::cout << (result.attempts < _icp_params.attempts ? "Invalid result, trying again!" : "Invalid result") << std::endl;
+ }
+
+ } while (not result.valid and result.attempts < _icp_params.attempts);
+
+ // if valid, copy transformation values and grow covariance
if (result.valid)
{
result.res_transf(0) = csm_output.x[0];
@@ -174,12 +221,12 @@ icpOutput ICP::align(const LaserScan &_current_ls,
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
result.res_covar(i, j) = _icp_params.cov_factor *
- csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];
-
+ csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];
+
// Grow covariance in the biggest eigenvalue direction
if (_icp_params.cov_max_eigv_factor - 1 > 1e-6)
{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(result.res_covar.topLeftCorner<2,2>());
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(result.res_covar.topLeftCorner<2, 2>());
if (eigensolver.info() == Eigen::Success)
{
@@ -188,16 +235,18 @@ icpOutput ICP::align(const LaserScan &_current_ls,
float max_eigv = eigvs.maxCoeff(&maxRow, &maxCol);
eigvs(maxRow) = _icp_params.cov_max_eigv_factor * max_eigv;
- result.res_covar.topLeftCorner<2,2>() = eigensolver.eigenvectors() *
- eigvs.asDiagonal() *
- eigensolver.eigenvectors().transpose();
+ result.res_covar.topLeftCorner<2, 2>() = eigensolver.eigenvectors() *
+ eigvs.asDiagonal() *
+ eigensolver.eigenvectors().transpose();
}
}
}
}
else
{
- //std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
+ if (_icp_params.verbose)
+ std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
+
result.res_transf = _initial_guess;
result.res_covar = Eigen::Matrix3s::Identity();
}
@@ -205,11 +254,11 @@ icpOutput ICP::align(const LaserScan &_current_ls,
return result;
}
-//Legacy code
-icpOutput ICP::align(const LaserScan &_current_ls,
- const LaserScan &_ref_ls,
- const LaserScanParams& scan_params,
- const icpParams &icp_params,
+// Legacy code
+icpOutput ICP::align(const LaserScan &_current_ls,
+ const LaserScan &_ref_ls,
+ const LaserScanParams &scan_params,
+ const icpParams &icp_params,
const Eigen::Vector3s &_initial_guess)
{
return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _initial_guess);
@@ -223,11 +272,10 @@ void ICP::printLaserData(LDP &laser_data)
void ICP::printTwoLaserData(sm_params ¶ms)
{
- for (int ii=0; ii<params.laser_ref->nrays-1; ++ii)
+ for (int ii = 0; ii < params.laser_ref->nrays - 1; ++ii)
{
std::cout << "Theta: " << params.laser_ref->theta[ii] << "; Readings: "
- << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
- << '\n';
+ << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
+ << '\n';
}
-
}
diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 18a0cb2e05583856b9228f35e56c44b22bd6262b..317452f46f437728722ac236973d8adfde4e53de 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/laser_scan.h"
namespace laserscanutils
diff --git a/src/laser_scan_with_params.cpp b/src/laser_scan_with_params.cpp
index 7bfe8b2c787a8d4a81ce32da6c65eba2656d1c43..5f8c0263b213612b85998567627f3507c3327e62 100644
--- a/src/laser_scan_with_params.cpp
+++ b/src/laser_scan_with_params.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/laser_scan_with_params.h"
diff --git a/src/line_finder.cpp b/src/line_finder.cpp
index 964482ab09deec7cd9f16191771a47b4ed0f3714..8df668784f341979715ca43be1243a32bfab2779 100644
--- a/src/line_finder.cpp
+++ b/src/line_finder.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/line_finder.h"
namespace laserscanutils
diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp
index 79a2e3c25f620b02e5a66a789804d8b3d8b94c4f..143c96a663759e0e7c6e6819674c3fc9a10beabd 100644
--- a/src/line_finder_hough.cpp
+++ b/src/line_finder_hough.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/line_finder_hough.h"
namespace laserscanutils
diff --git a/src/line_finder_iterative.cpp b/src/line_finder_iterative.cpp
index bd6150c17d16b50c217bae5a1e60ee5f89454354..b59a3ae246ba0ced18fc651f5781daa317082e9d 100644
--- a/src/line_finder_iterative.cpp
+++ b/src/line_finder_iterative.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/line_finder_iterative.h"
namespace laserscanutils
diff --git a/src/line_finder_jump_fit.cpp b/src/line_finder_jump_fit.cpp
index 614049c06e0e3207bc01d3f54d6e3e1d9de903ad..9e284db4fc922ad25893af272499792050c21a13 100644
--- a/src/line_finder_jump_fit.cpp
+++ b/src/line_finder_jump_fit.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/line_finder_jump_fit.h"
namespace laserscanutils
diff --git a/src/line_segment.cpp b/src/line_segment.cpp
index 66d3a557f8834c6adee4b854d3af245d4320ec2f..90971f6815ee298393d1ced7a0a06f17d308f8f0 100644
--- a/src/line_segment.cpp
+++ b/src/line_segment.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/line_segment.h"
namespace laserscanutils
diff --git a/src/loop_closure_base.cpp b/src/loop_closure_base.cpp
index 68c76bf53d699b2c2e4bf7f5d13b4960d9fd332f..26632d39e0df1c59b804e870b5f6fd2f19a2aa64 100644
--- a/src/loop_closure_base.cpp
+++ b/src/loop_closure_base.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,14 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file loop_closure_base_2d.h
- *
- * Created on: Feb 9, 2021
- * \author: spujol
- */
#include "laser_scan_utils/loop_closure_base.h"
diff --git a/src/point_set.cpp b/src/point_set.cpp
index 80b1eb262fb1cd99b13d8ea10879920c98d12ff1..b28171017328b749b718c968996d817a76b0e6bd 100644
--- a/src/point_set.cpp
+++ b/src/point_set.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/scan_segment.h"
namespace laserscanutils
diff --git a/src/polyline.cpp b/src/polyline.cpp
index 75e1ab852b3d8c3b74bcd8056ab54d72f8027d9a..59fee5e47ef97d63afad96b783fea0c245d13aa4 100644
--- a/src/polyline.cpp
+++ b/src/polyline.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/polyline.h"
namespace laserscanutils
diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp
index 7abb6eabd282e940e010839b0c11d0a8350149e8..e19bc7e7471c9a40caad0ba7d63e92d3e9f0056e 100644
--- a/src/scan_segment.cpp
+++ b/src/scan_segment.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "laser_scan_utils/scan_segment.h"
namespace laserscanutils
diff --git a/test/data/scan_data.h b/test/data/scan_data.h
index 4c48f052634d163bc24f6de10ca5c8a5119827ea..9b5805dfc651a2df9b66a57226ebaa22ac088c21 100644
--- a/test/data/scan_data.h
+++ b/test/data/scan_data.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,27 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
- * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
- *
- * This file is part of FALKOLib.
- *
- * FALKOLib is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * FALKOLib is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
- */
+
std::vector<float> testRanges1 = {250,
250,
250,
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 3b52f512ade635f2beb371d13805ab1faefabc82..68dd3a7d26c9651e9fddd530861e0aeb876475a0 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,11 +1,13 @@
include(FetchContent)
+SET(BUILD_GMOCK OFF) # Disable gmock
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+
FetchContent_Declare(
googletest
GIT_REPOSITORY https://github.com/google/googletest.git
GIT_TAG main)
-SET(INSTALL_GTEST OFF) # Disable installation of googletest
FetchContent_MakeAvailable(googletest)
function(add_gtest target)
diff --git a/test/gtest/utils_gtest.h b/test/gtest/utils_gtest.h
index 15ab2385027cb84056c986e181ca68227b48acff..0c39d4df9e34502480172d008699eaa14fc106c6 100644
--- a/test/gtest/utils_gtest.h
+++ b/test/gtest/utils_gtest.h
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,18 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file utils_gtest.h
- * \brief Some utils for gtest
- * \author Jeremie Deray
- * Created on: 26/09/2016
- * Eigen macros extension by: Joan Sola on 26/04/2017
- */
-#ifndef GNSSUTILS_UTILS_GTEST_H
-#define GNSSUTILS_UTILS_GTEST_H
+#pragma once
#include <gtest/gtest.h>
@@ -163,5 +153,3 @@ TEST(Test, Foo)
C_expect, C_actual);
} // namespace testing
-
-#endif /* GNSSUTILS_UTILS_GTEST_H */
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 3fd271cc234acfa3854a7b7ab964a11d4b8700d9..5c5c860d4bb5f472c76382eb2b8a42b699eacca9 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "gtest/utils_gtest.h"
TEST(TestTest, DummyTestExample)
diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index e3ec9b44c73ce89e62f9abd068651abaffb673d6..d4b9e668810a85e42641b0958e5e5e8bdf64dfd8 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,39 +17,33 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "gtest/utils_gtest.h"
#include "laser_scan_utils/laser_scan.h"
#include "laser_scan_utils/icp.h"
-#define TEST_PLOTS false
-#if TEST_PLOTS
-#include "matplotlibcpp.h"
-namespace plt = matplotlibcpp;
-#endif
-
using namespace laserscanutils;
+int N = 50;
+unsigned int n_attempts = 10;
+
const Eigen::Vector2d A = Eigen::Vector2d::Zero();
const Eigen::Vector2d B = (Eigen::Vector2d() << 0, 40).finished();
const Eigen::Vector2d C = (Eigen::Vector2d() << 30, 0).finished();
-const Eigen::Vector2d CB = B-C;
-double AB_ang = atan2((A-B).norm(),(A-C).norm());
+const Eigen::Vector2d CB = B - C;
+double AB_ang = atan2((A - B).norm(), (A - C).norm());
double dist_min = 2;
-int color_id=0;
-const std::vector<std::string> colors({"b","r","g","c","m","y"});
-/* Synthetic scans are created from this simple scenario with three orthogonal walls:
+/* Synthetic scans are created from this simple scenario with three vertical walls:
*
- * B
+ * B
* +
* |\
- * | \
- * | \
- * | \
- * | \
- * | \
+ * | \
+ * | \
+ * | \
+ * | \
+ * | \
* | the \
* | laser \
* | can be \
@@ -59,23 +53,23 @@ const std::vector<std::string> colors({"b","r","g","c","m","y"});
* | \
* +-------------+
* A C
- *
-*/
+ *
+ */
-double pi2pi(const double& _angle)
+double pi2pi(const double &_angle)
{
- double angle = _angle;
- while (angle > M_PI )
- angle -= 2 * M_PI;
- while (angle <= -M_PI)
- angle += 2 * M_PI ;
+ double angle = _angle;
+ while (angle > M_PI)
+ angle -= 2 * M_PI;
+ while (angle <= -M_PI)
+ angle += 2 * M_PI;
- return angle;
+ return angle;
}
-bool insideTriangle(const Eigen::Vector3d& laser_pose)
+bool insideTriangle(const Eigen::Vector3d &laser_pose)
{
- return laser_pose(0) > 0 + dist_min and laser_pose(1) > 0 + dist_min and laser_pose(0) / (C(0)-dist_min/sin(AB_ang)) + laser_pose(1) / (B(1)-dist_min/cos(AB_ang)) < 1;
+ return laser_pose(0) > 0 + dist_min and laser_pose(1) > 0 + dist_min and laser_pose(0) / (C(0) - dist_min / sin(AB_ang)) + laser_pose(1) / (B(1) - dist_min / cos(AB_ang)) < 1;
}
Eigen::Vector3d generateRandomPoseInsideTriangle()
@@ -92,28 +86,28 @@ Eigen::Vector3d generateRandomPoseInsideTriangle()
return pose;
}
-LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams& params)
+LaserScan simulateScan(const Eigen::Vector3d &laser_pose, const LaserScanParams ¶ms)
{
// assert inside triangle
assert(insideTriangle(laser_pose));
// create scan
- int n_beams = int((params.angle_max_-params.angle_min_)/params.angle_step_)+1;
+ int n_beams = int((params.angle_max_ - params.angle_min_) / params.angle_step_) + 1;
LaserScan scan = LaserScan(std::vector<float>(n_beams, 0));
// compute angle limits for the three segments (in non rotated reference)
- double theta_A = pi2pi(atan2(A(1)-laser_pose(1), A(0)-laser_pose(0)));
- double theta_B = pi2pi(atan2(B(1)-laser_pose(1), B(0)-laser_pose(0)));
- double theta_C = pi2pi(atan2(C(1)-laser_pose(1), C(0)-laser_pose(0)));
+ double theta_A = pi2pi(atan2(A(1) - laser_pose(1), A(0) - laser_pose(0)));
+ double theta_B = pi2pi(atan2(B(1) - laser_pose(1), B(0) - laser_pose(0)));
+ double theta_C = pi2pi(atan2(C(1) - laser_pose(1), C(0) - laser_pose(0)));
// std::cout << "theta_A: " << theta_A << std::endl;
// std::cout << "theta_B: " << theta_B << std::endl;
// std::cout << "theta_C: " << theta_C << std::endl;
// compute distance to diagonal via area of parallelogram
- double d_p_BC = (Eigen::Matrix2d() << CB(0), laser_pose(0)-C(0), CB(1), laser_pose(1)-C(1)).finished().determinant() / CB.norm();
+ double d_p_BC = (Eigen::Matrix2d() << CB(0), laser_pose(0) - C(0), CB(1), laser_pose(1) - C(1)).finished().determinant() / CB.norm();
// double d_p_BC = (CB.cross(Eigen::Vector2d(laser_pose.head<2>()-C))).norm() / CB.norm();
double theta = params.angle_min_ + laser_pose(2);
- for (auto i = 0; i < n_beams; i++, theta+=params.angle_step_)
+ for (auto i = 0; i < n_beams; i++, theta += params.angle_step_)
{
// Ensure theta in (-M_PI, M_PI]
theta = pi2pi(theta);
@@ -135,7 +129,7 @@ LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams&
else if (theta > theta_A and theta < theta_C)
{
// std::cout << "A-C segment" << std::endl;
- scan.ranges_raw_[i] = laser_pose(1) / cos(theta + M_PI/2);
+ scan.ranges_raw_[i] = laser_pose(1) / cos(theta + M_PI / 2);
}
else
throw std::runtime_error("bad theta angle..!");
@@ -151,40 +145,25 @@ LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams&
}
}
- // // print ranges
- // std::cout << "Scan:" << std::endl;
- // std::cout << " angle_min: " << params.angle_min_ << std::endl;
- // std::cout << " angle_step_: " << params.angle_step_ << std::endl;
- // std::cout << " ranges: ";
- // for (auto range : scan.ranges_raw_)
- // std::cout << range << ", ";
- // std::cout << std::endl;
-
return scan;
};
-void generateRandomProblem(Eigen::Vector3d& pose_ref,
- Eigen::Vector3d& pose_tar,
- Eigen::Vector3d& pose_d,
- const LaserScanParams& scan_params,
- LaserScan& scan_ref,
- LaserScan& scan_tar,
- double perturbation = 1)
+void generateRandomProblem(Eigen::Vector3d &pose_ref,
+ Eigen::Vector3d &pose_tar,
+ Eigen::Vector3d &pose_d,
+ const LaserScanParams &scan_params,
+ LaserScan &scan_ref,
+ LaserScan &scan_tar,
+ double magnitude = 1)
{
pose_ref = generateRandomPoseInsideTriangle();
- pose_d = Eigen::Vector3d::Random() * perturbation;
- while (pose_d(2) > M_PI)
- pose_d(2) -= 2*M_PI;
- while (pose_d(2) <= -M_PI)
- pose_d(2) += 2*M_PI;
- pose_tar.head<2>() = pose_ref.head<2>() + Eigen::Rotation2Dd(pose_ref(2)) * pose_d.head<2>();
- pose_tar(2) = pose_ref(2) + pose_d(2);
- while (not insideTriangle(pose_tar))
+ do
{
- pose_d = Eigen::Vector3d::Random();
+ pose_d = Eigen::Vector3d::Random() * magnitude;
+ pose_d(2) = pi2pi(pose_d(2));
pose_tar.head<2>() = pose_ref.head<2>() + Eigen::Rotation2Dd(pose_ref(2)) * pose_d.head<2>();
pose_tar(2) = pose_ref(2) + pose_d(2);
- }
+ } while (not insideTriangle(pose_tar));
scan_ref = simulateScan(pose_ref, scan_params);
scan_tar = simulateScan(pose_tar, scan_params);
@@ -196,62 +175,17 @@ LaserScanParams generateLaserScanParams(double angle_min_deg, double angle_step_
LaserScanParams scan_params;
scan_params.angle_min_ = angle_min_deg * M_PI / 180;
scan_params.angle_step_ = angle_step_deg * M_PI / 180;
- auto n_ranges = int (2*M_PI / scan_params.angle_step_);
- scan_params.angle_max_ = scan_params.angle_min_ + (n_ranges-1) * scan_params.angle_step_;
+ auto n_ranges = int(2 * M_PI / scan_params.angle_step_);
+ scan_params.angle_max_ = scan_params.angle_min_ + (n_ranges - 1) * scan_params.angle_step_;
scan_params.scan_time_ = 0;
scan_params.range_min_ = 0;
scan_params.range_max_ = 1e2;
scan_params.range_std_dev_ = 0;
scan_params.angle_std_dev_ = 0;
- //scan_params.print();
return scan_params;
}
-void initPlot()
-{
-#if TEST_PLOTS
- plt::figure();
-#endif
-}
-
-void showPlot()
-{
-#if TEST_PLOTS
- plt::show();
-#endif
-}
-
-void plotScan(const LaserScan& scan, const LaserScanParams& scan_params, const Eigen::Vector3d pose, bool fig_created = false)
-{
-#if TEST_PLOTS
- // Create figure
- if (not fig_created)
- plt::figure();
- plt::axis("scaled");
-
- std::vector<double> x(scan.ranges_raw_.size());
- std::vector<double> y(scan.ranges_raw_.size());
- for (auto i = 0; i < scan.ranges_raw_.size(); i++)
- {
- x.at(i) = pose(0) + cos(scan_params.angle_min_ + i*scan_params.angle_step_ + pose(2)) * scan.ranges_raw_.at(i);
- y.at(i) = pose(1) + sin(scan_params.angle_min_ + i*scan_params.angle_step_ + pose(2)) * scan.ranges_raw_.at(i);
- }
- plt::plot(x, y,"."+colors.at(color_id));
-
- std::vector<double> pose_x{pose(0)-sin(pose(2)), pose(0), pose(0)+cos(pose(2))};
- std::vector<double> pose_y{pose(1)+cos(pose(2)), pose(1), pose(1)+sin(pose(2))};
- plt::plot(pose_x, pose_y,colors.at(color_id)+"-");
-
- if (not fig_created)
- plt::show();
-
- color_id++;
- if (color_id >= colors.size())
- color_id = 0;
-#endif
-}
-
///////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////// TESTS ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -259,7 +193,7 @@ void plotScan(const LaserScan& scan, const LaserScanParams& scan_params, const E
TEST(TestIcp, TestSimulateScan)
{
// 4 beams: 0º, 90º, 180ª, 270ª
- LaserScanParams scan_params = generateLaserScanParams(0,90);
+ LaserScanParams scan_params = generateLaserScanParams(0, 90);
Eigen::Vector3d laser_pose;
laser_pose << C(0) / 4, B(1) / 4, 0;
@@ -275,7 +209,7 @@ TEST(TestIcp, TestSimulateScan)
ASSERT_NEAR(scan1.ranges_raw_[3], laser_pose(1), 1e-9);
// theta = 90ª
- laser_pose(2) = M_PI/2;
+ laser_pose(2) = M_PI / 2;
std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
auto scan2 = simulateScan(laser_pose, scan_params);
@@ -295,7 +229,7 @@ TEST(TestIcp, TestSimulateScan)
ASSERT_NEAR(scan3.ranges_raw_[3], B(1) / 2, 1e-9);
// theta = 270ª
- laser_pose(2) = 3*M_PI/2;
+ laser_pose(2) = 3 * M_PI / 2;
std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
auto scan4 = simulateScan(laser_pose, scan_params);
@@ -308,188 +242,221 @@ TEST(TestIcp, TestSimulateScan)
TEST(TestIcp, TestGenerateRandomPose)
{
// 0-2M_PI (5 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(-180,1);
-
- initPlot();
+ LaserScanParams scan_params = generateLaserScanParams(-180, 1);
// 100 random poses and scans
- for (auto i=0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
auto laser_pose = generateRandomPoseInsideTriangle();
ASSERT_TRUE(insideTriangle(laser_pose));
-
- auto scan = simulateScan(laser_pose, scan_params);
- plotScan(scan, scan_params, laser_pose, true);
+ auto scan = simulateScan(laser_pose, scan_params);
}
-
- showPlot();
}
TEST(TestIcp, TestIcpSame1)
{
// -180,180 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(-180,1);
+ LaserScanParams scan_params = generateLaserScanParams(-180, 1);
+ double pert = 0.0;
- for (auto i = 0; i<10; i++)
+ for (auto i = 0; i < N; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
// icp
auto icp_params = icp_params_default;
+ icp_params.attempts = n_attempts;
+ // icp_params.verbose = true;
// no perturbation
- std::cout << "//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
- std::cout << "//////////// NO perturbation" << std::endl;
- auto icp_output = ICP::align(scan,
+ std::cout << "\n//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
+ std::cout << "------ NO perturbation" << std::endl;
+ auto icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
Eigen::Vector3d::Zero());
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
+
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
// perturbation
- std::cout << "//////////// WITH perturbation" << std::endl;
- icp_output = ICP::align(scan,
+ std::cout << "------ WITH perturbation: " << pert << std::endl;
+ Eigen::Vector3d initial_guess = Eigen::Vector3d::Random() * pert;
+ icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
- Eigen::Vector3d::Random()*0.1);
+ initial_guess);
- if (not icp_output.valid)
- icp_output = ICP::align(scan,
- scan,
- scan_params,
- icp_params,
- Eigen::Vector3d::Random()*0.1);
+ std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
+
+ pert += 0.5 / N;
}
}
TEST(TestIcp, TestIcpSame2)
{
// 0,360 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(0,1);
+ LaserScanParams scan_params = generateLaserScanParams(0, 1);
+ double pert = 0.0;
+ auto icp_params = icp_params_default;
+ icp_params.attempts = n_attempts;
- for (auto i = 0; i<10; i++)
+ for (auto i = 0; i < N; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
- // icp
- auto icp_params = icp_params_default;
-
// no perturbation
- std::cout << "//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
- std::cout << "//////////// NO perturbation" << std::endl;
- auto icp_output = ICP::align(scan,
+ std::cout << "\n//////////// TestIcpSame2: random problem " << i << " pose: " << pose.transpose() << std::endl;
+ std::cout << "------ NO perturbation" << std::endl;
+ auto icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
Eigen::Vector3d::Zero());
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
+
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
// perturbation
- std::cout << "//////////// WITH perturbation" << std::endl;
- icp_output = ICP::align(scan,
+ std::cout << "------ WITH perturbation: " << pert << std::endl;
+ Eigen::Vector3d initial_guess = Eigen::Vector3d::Random() * pert;
+ icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
- Eigen::Vector3d::Random()*0.1);
+ initial_guess);
- if (not icp_output.valid)
- icp_output = ICP::align(scan,
- scan,
- scan_params,
- icp_params,
- Eigen::Vector3d::Random()*0.1);
+ std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
+
+ pert += 0.5 / N;
}
}
TEST(TestIcp, TestIcp1)
{
// 0,360 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(0,1);
+ LaserScanParams scan_params = generateLaserScanParams(0, 1);
- Eigen::Vector3d pose_ref, pose_tar, pose_d;
+ Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
- for (auto i=0; i < 10; i++)
- {
+ auto icp_params = icp_params_default;
+ icp_params.attempts = n_attempts;
+
+ double pert = 0.0;
+
+ for (auto i = 0; i < N; i++)
+ {
// Random problem
generateRandomProblem(pose_ref, pose_tar, pose_d, scan_params, scan_ref, scan_tar);
- std::cout << "//////////// TestIcp1: random problem " << i << std::endl;
- std::cout << "\tpose_ref: " << pose_ref.transpose() << std::endl;
- std::cout << "\tpose_tar: " << pose_tar.transpose() << std::endl;
+ initial_guess = pose_d + pert * Eigen::Vector3d::Random();
// icp
- auto icp_params = icp_params_default;
-
- auto icp_output = ICP::align(scan_tar,
+ auto icp_output = ICP::align(scan_tar,
scan_ref,
- scan_params,
+ scan_params,
icp_params,
- //Eigen::Vector3d::Zero());
- pose_d);
+ initial_guess);
+
+ std::cout << "\n//////////// TestIcp1: random problem " << i << " - perturbation: " << pert << std::endl;
+ std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
+ std::cout << " pose_tar: " << pose_tar.transpose() << std::endl;
+ std::cout << " groundtruth: " << pose_d.transpose() << std::endl;
+ std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
+ std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
- initPlot();
- plotScan(scan_ref,scan_params,pose_ref,true);
- plotScan(scan_tar,scan_params,pose_tar,true);
- showPlot();
+ pert += 0.5 / N;
}
}
-
TEST(TestIcp, TestIcp10)
{
// -180,180 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(-180,1);
+ LaserScanParams scan_params = generateLaserScanParams(-180, 1);
- Eigen::Vector3d pose_ref, pose_tar, pose_d;
+ Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
- for (auto i=0; i < 10; i++)
- {
+ auto icp_params = icp_params_default;
+ icp_params.attempts = n_attempts;
+ icp_params.verbose = true;
+
+ double pert = 0.0;
+
+ for (auto i = 0; i < N; i++)
+ {
// Random problem
generateRandomProblem(pose_ref, pose_tar, pose_d, scan_params, scan_ref, scan_tar, 10);
- std::cout << "//////////// TestIcp1: random problem " << i << std::endl;
- std::cout << "\tpose_ref: " << pose_ref.transpose() << std::endl;
- std::cout << "\tpose_tar: " << pose_tar.transpose() << std::endl;
+ initial_guess = pose_d + pert * Eigen::Vector3d::Random();
// icp
- auto icp_params = icp_params_default;
-
- auto icp_output = ICP::align(scan_tar,
+ auto icp_output = ICP::align(scan_tar,
scan_ref,
- scan_params,
+ scan_params,
icp_params,
- pose_d);
+ initial_guess);
+
+ std::cout << "\n//////////// TestIcp10: random problem " << i << " - perturbation: " << pert << std::endl;
+ std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
+ std::cout << " pose_tar: " << pose_tar.transpose() << std::endl;
+ std::cout << " groundtruth: " << pose_d.transpose() << std::endl;
+ std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
+ std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
- initPlot();
- plotScan(scan_ref,scan_params,pose_ref,true);
- plotScan(scan_tar,scan_params,pose_tar,true);
- showPlot();
+ pert += 0.5 / N;
}
-}//*/
-
+} //*/
int main(int argc, char **argv)
{
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index 6225f5fa6c909f4d7de69ca15bd0775c8c5eb0d4..31d72890ba9903d336ad3376d92202b0744d1ceb 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -1,14 +1,14 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// LaserScanUtils - Copyright (C) 2020-2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu),
+// Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved.
//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
+// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -17,8 +17,7 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
+
#include "gtest/utils_gtest.h"
#include "data/scan_data.h"
#include "laser_scan_utils/loop_closure_base.h"
diff --git a/test/matplotlibcpp.h b/test/matplotlibcpp.h
deleted file mode 100644
index c1b2d9b41949609046d286843d9e99d5f872ae8a..0000000000000000000000000000000000000000
--- a/test/matplotlibcpp.h
+++ /dev/null
@@ -1,3007 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#pragma once
-
-// Python headers must be included before any system headers, since
-// they define _POSIX_C_SOURCE
-#include <Python.h>
-
-#include <vector>
-#include <map>
-#include <array>
-#include <numeric>
-#include <algorithm>
-#include <stdexcept>
-#include <iostream>
-#include <cstdint> // <cstdint> requires c++11 support
-#include <functional>
-#include <string> // std::stod
-
-#ifndef WITHOUT_NUMPY
-# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
-# include <numpy/arrayobject.h>
-
-# ifdef WITH_OPENCV
-# include <opencv2/opencv.hpp>
-# endif // WITH_OPENCV
-
-/*
- * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so
- * define the ones we need here.
- */
-# if CV_MAJOR_VERSION > 3
-# define CV_BGR2RGB cv::COLOR_BGR2RGB
-# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA
-# endif
-#endif // WITHOUT_NUMPY
-
-#if PY_MAJOR_VERSION >= 3
-# define PyString_FromString PyUnicode_FromString
-# define PyInt_FromLong PyLong_FromLong
-# define PyString_FromString PyUnicode_FromString
-#endif
-
-
-namespace matplotlibcpp {
-namespace detail {
-
-static std::string s_backend;
-
-struct _interpreter {
- PyObject* s_python_function_arrow;
- PyObject *s_python_function_show;
- PyObject *s_python_function_close;
- PyObject *s_python_function_draw;
- PyObject *s_python_function_pause;
- PyObject *s_python_function_save;
- PyObject *s_python_function_figure;
- PyObject *s_python_function_fignum_exists;
- PyObject *s_python_function_plot;
- PyObject *s_python_function_quiver;
- PyObject* s_python_function_contour;
- PyObject *s_python_function_semilogx;
- PyObject *s_python_function_semilogy;
- PyObject *s_python_function_loglog;
- PyObject *s_python_function_fill;
- PyObject *s_python_function_fill_between;
- PyObject *s_python_function_hist;
- PyObject *s_python_function_imshow;
- PyObject *s_python_function_scatter;
- PyObject *s_python_function_boxplot;
- PyObject *s_python_function_subplot;
- PyObject *s_python_function_subplot2grid;
- PyObject *s_python_function_legend;
- PyObject *s_python_function_xlim;
- PyObject *s_python_function_ion;
- PyObject *s_python_function_ginput;
- PyObject *s_python_function_ylim;
- PyObject *s_python_function_title;
- PyObject *s_python_function_axis;
- PyObject *s_python_function_axhline;
- PyObject *s_python_function_axvline;
- PyObject *s_python_function_axvspan;
- PyObject *s_python_function_xlabel;
- PyObject *s_python_function_ylabel;
- PyObject *s_python_function_gca;
- PyObject *s_python_function_xticks;
- PyObject *s_python_function_yticks;
- PyObject* s_python_function_margins;
- PyObject *s_python_function_tick_params;
- PyObject *s_python_function_grid;
- PyObject* s_python_function_cla;
- PyObject *s_python_function_clf;
- PyObject *s_python_function_errorbar;
- PyObject *s_python_function_annotate;
- PyObject *s_python_function_tight_layout;
- PyObject *s_python_colormap;
- PyObject *s_python_empty_tuple;
- PyObject *s_python_function_stem;
- PyObject *s_python_function_xkcd;
- PyObject *s_python_function_text;
- PyObject *s_python_function_suptitle;
- PyObject *s_python_function_bar;
- PyObject *s_python_function_barh;
- PyObject *s_python_function_colorbar;
- PyObject *s_python_function_subplots_adjust;
- PyObject *s_python_function_rcparams;
- PyObject *s_python_function_spy;
-
- /* For now, _interpreter is implemented as a singleton since its currently not possible to have
- multiple independent embedded python interpreters without patching the python source code
- or starting a separate process for each. [1]
- Furthermore, many python objects expect that they are destructed in the same thread as they
- were constructed. [2] So for advanced usage, a `kill()` function is provided so that library
- users can manually ensure that the interpreter is constructed and destroyed within the
- same thread.
-
- 1: http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program
- 2: https://github.com/lava/matplotlib-cpp/pull/202#issue-436220256
- */
-
- static _interpreter& get() {
- return interkeeper(false);
- }
-
- static _interpreter& kill() {
- return interkeeper(true);
- }
-
- // Stores the actual singleton object referenced by `get()` and `kill()`.
- static _interpreter& interkeeper(bool should_kill) {
- static _interpreter ctx;
- if (should_kill)
- ctx.~_interpreter();
- return ctx;
- }
-
- PyObject* safe_import(PyObject* module, std::string fname) {
- PyObject* fn = PyObject_GetAttrString(module, fname.c_str());
-
- if (!fn)
- throw std::runtime_error(std::string("Couldn't find required function: ") + fname);
-
- if (!PyFunction_Check(fn))
- throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction."));
-
- return fn;
- }
-
-private:
-
-#ifndef WITHOUT_NUMPY
-# if PY_MAJOR_VERSION >= 3
-
- void *import_numpy() {
- import_array(); // initialize C-API
- return NULL;
- }
-
-# else
-
- void import_numpy() {
- import_array(); // initialize C-API
- }
-
-# endif
-#endif
-
- _interpreter() {
-
- // optional but recommended
-#if PY_MAJOR_VERSION >= 3
- wchar_t name[] = L"plotting";
-#else
- char name[] = "plotting";
-#endif
- Py_SetProgramName(name);
- Py_Initialize();
-
- wchar_t const *dummy_args[] = {L"Python", NULL}; // const is needed because literals must not be modified
- wchar_t const **argv = dummy_args;
- int argc = sizeof(dummy_args)/sizeof(dummy_args[0])-1;
-
-#if PY_MAJOR_VERSION >= 3
- PySys_SetArgv(argc, const_cast<wchar_t **>(argv));
-#else
- PySys_SetArgv(argc, (char **)(argv));
-#endif
-
-#ifndef WITHOUT_NUMPY
- import_numpy(); // initialize numpy C-API
-#endif
-
- PyObject* matplotlibname = PyString_FromString("matplotlib");
- PyObject* pyplotname = PyString_FromString("matplotlib.pyplot");
- PyObject* cmname = PyString_FromString("matplotlib.cm");
- PyObject* pylabname = PyString_FromString("pylab");
- if (!pyplotname || !pylabname || !matplotlibname || !cmname) {
- throw std::runtime_error("couldnt create string");
- }
-
- PyObject* matplotlib = PyImport_Import(matplotlibname);
-
- Py_DECREF(matplotlibname);
- if (!matplotlib) {
- PyErr_Print();
- throw std::runtime_error("Error loading module matplotlib!");
- }
-
- // matplotlib.use() must be called *before* pylab, matplotlib.pyplot,
- // or matplotlib.backends is imported for the first time
- if (!s_backend.empty()) {
- PyObject_CallMethod(matplotlib, const_cast<char*>("use"), const_cast<char*>("s"), s_backend.c_str());
- }
-
-
-
- PyObject* pymod = PyImport_Import(pyplotname);
- Py_DECREF(pyplotname);
- if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); }
-
- s_python_colormap = PyImport_Import(cmname);
- Py_DECREF(cmname);
- if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); }
-
- PyObject* pylabmod = PyImport_Import(pylabname);
- Py_DECREF(pylabname);
- if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); }
-
- s_python_function_arrow = safe_import(pymod, "arrow");
- s_python_function_show = safe_import(pymod, "show");
- s_python_function_close = safe_import(pymod, "close");
- s_python_function_draw = safe_import(pymod, "draw");
- s_python_function_pause = safe_import(pymod, "pause");
- s_python_function_figure = safe_import(pymod, "figure");
- s_python_function_fignum_exists = safe_import(pymod, "fignum_exists");
- s_python_function_plot = safe_import(pymod, "plot");
- s_python_function_quiver = safe_import(pymod, "quiver");
- s_python_function_contour = safe_import(pymod, "contour");
- s_python_function_semilogx = safe_import(pymod, "semilogx");
- s_python_function_semilogy = safe_import(pymod, "semilogy");
- s_python_function_loglog = safe_import(pymod, "loglog");
- s_python_function_fill = safe_import(pymod, "fill");
- s_python_function_fill_between = safe_import(pymod, "fill_between");
- s_python_function_hist = safe_import(pymod,"hist");
- s_python_function_scatter = safe_import(pymod,"scatter");
- s_python_function_boxplot = safe_import(pymod,"boxplot");
- s_python_function_subplot = safe_import(pymod, "subplot");
- s_python_function_subplot2grid = safe_import(pymod, "subplot2grid");
- s_python_function_legend = safe_import(pymod, "legend");
- s_python_function_xlim = safe_import(pymod, "xlim");
- s_python_function_ylim = safe_import(pymod, "ylim");
- s_python_function_title = safe_import(pymod, "title");
- s_python_function_axis = safe_import(pymod, "axis");
- s_python_function_axhline = safe_import(pymod, "axhline");
- s_python_function_axvline = safe_import(pymod, "axvline");
- s_python_function_axvspan = safe_import(pymod, "axvspan");
- s_python_function_xlabel = safe_import(pymod, "xlabel");
- s_python_function_ylabel = safe_import(pymod, "ylabel");
- s_python_function_gca = safe_import(pymod, "gca");
- s_python_function_xticks = safe_import(pymod, "xticks");
- s_python_function_yticks = safe_import(pymod, "yticks");
- s_python_function_margins = safe_import(pymod, "margins");
- s_python_function_tick_params = safe_import(pymod, "tick_params");
- s_python_function_grid = safe_import(pymod, "grid");
- s_python_function_ion = safe_import(pymod, "ion");
- s_python_function_ginput = safe_import(pymod, "ginput");
- s_python_function_save = safe_import(pylabmod, "savefig");
- s_python_function_annotate = safe_import(pymod,"annotate");
- s_python_function_cla = safe_import(pymod, "cla");
- s_python_function_clf = safe_import(pymod, "clf");
- s_python_function_errorbar = safe_import(pymod, "errorbar");
- s_python_function_tight_layout = safe_import(pymod, "tight_layout");
- s_python_function_stem = safe_import(pymod, "stem");
- s_python_function_xkcd = safe_import(pymod, "xkcd");
- s_python_function_text = safe_import(pymod, "text");
- s_python_function_suptitle = safe_import(pymod, "suptitle");
- s_python_function_bar = safe_import(pymod,"bar");
- s_python_function_barh = safe_import(pymod, "barh");
- s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar");
- s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust");
- s_python_function_rcparams = PyObject_GetAttrString(pymod, "rcParams");
- s_python_function_spy = PyObject_GetAttrString(pymod, "spy");
-#ifndef WITHOUT_NUMPY
- s_python_function_imshow = safe_import(pymod, "imshow");
-#endif
- s_python_empty_tuple = PyTuple_New(0);
- }
-
- ~_interpreter() {
- Py_Finalize();
- }
-};
-
-} // end namespace detail
-
-/// Select the backend
-///
-/// **NOTE:** This must be called before the first plot command to have
-/// any effect.
-///
-/// Mainly useful to select the non-interactive 'Agg' backend when running
-/// matplotlibcpp in headless mode, for example on a machine with no display.
-///
-/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use
-inline void backend(const std::string& name)
-{
- detail::s_backend = name;
-}
-
-inline bool annotate(std::string annotation, double x, double y)
-{
- detail::_interpreter::get();
-
- PyObject * xy = PyTuple_New(2);
- PyObject * str = PyString_FromString(annotation.c_str());
-
- PyTuple_SetItem(xy,0,PyFloat_FromDouble(x));
- PyTuple_SetItem(xy,1,PyFloat_FromDouble(y));
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "xy", xy);
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, str);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
-
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-namespace detail {
-
-#ifndef WITHOUT_NUMPY
-// Type selector for numpy array conversion
-template <typename T> struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default
-template <> struct select_npy_type<double> { const static NPY_TYPES type = NPY_DOUBLE; };
-template <> struct select_npy_type<float> { const static NPY_TYPES type = NPY_FLOAT; };
-template <> struct select_npy_type<bool> { const static NPY_TYPES type = NPY_BOOL; };
-template <> struct select_npy_type<int8_t> { const static NPY_TYPES type = NPY_INT8; };
-template <> struct select_npy_type<int16_t> { const static NPY_TYPES type = NPY_SHORT; };
-template <> struct select_npy_type<int32_t> { const static NPY_TYPES type = NPY_INT; };
-template <> struct select_npy_type<int64_t> { const static NPY_TYPES type = NPY_INT64; };
-template <> struct select_npy_type<uint8_t> { const static NPY_TYPES type = NPY_UINT8; };
-template <> struct select_npy_type<uint16_t> { const static NPY_TYPES type = NPY_USHORT; };
-template <> struct select_npy_type<uint32_t> { const static NPY_TYPES type = NPY_ULONG; };
-template <> struct select_npy_type<uint64_t> { const static NPY_TYPES type = NPY_UINT64; };
-
-// Sanity checks; comment them out or change the numpy type below if you're compiling on
-// a platform where they don't apply
-static_assert(sizeof(long long) == 8);
-template <> struct select_npy_type<long long> { const static NPY_TYPES type = NPY_INT64; };
-static_assert(sizeof(unsigned long long) == 8);
-template <> struct select_npy_type<unsigned long long> { const static NPY_TYPES type = NPY_UINT64; };
-
-template<typename Numeric>
-PyObject* get_array(const std::vector<Numeric>& v)
-{
- npy_intp vsize = v.size();
- NPY_TYPES type = select_npy_type<Numeric>::type;
- if (type == NPY_NOTYPE) {
- size_t memsize = v.size()*sizeof(double);
- double* dp = static_cast<double*>(::malloc(memsize));
- for (size_t i=0; i<v.size(); ++i)
- dp[i] = v[i];
- PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, NPY_DOUBLE, dp);
- PyArray_UpdateFlags(reinterpret_cast<PyArrayObject*>(varray), NPY_ARRAY_OWNDATA);
- return varray;
- }
-
- PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data()));
- return varray;
-}
-
-
-template<typename Numeric>
-PyObject* get_2darray(const std::vector<::std::vector<Numeric>>& v)
-{
- if (v.size() < 1) throw std::runtime_error("get_2d_array v too small");
-
- npy_intp vsize[2] = {static_cast<npy_intp>(v.size()),
- static_cast<npy_intp>(v[0].size())};
-
- PyArrayObject *varray =
- (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE);
-
- double *vd_begin = static_cast<double *>(PyArray_DATA(varray));
-
- for (const ::std::vector<Numeric> &v_row : v) {
- if (v_row.size() != static_cast<size_t>(vsize[1]))
- throw std::runtime_error("Missmatched array size");
- std::copy(v_row.begin(), v_row.end(), vd_begin);
- vd_begin += vsize[1];
- }
-
- return reinterpret_cast<PyObject *>(varray);
-}
-
-#else // fallback if we don't have numpy: copy every element of the given vector
-
-template<typename Numeric>
-PyObject* get_array(const std::vector<Numeric>& v)
-{
- PyObject* list = PyList_New(v.size());
- for(size_t i = 0; i < v.size(); ++i) {
- PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i)));
- }
- return list;
-}
-
-#endif // WITHOUT_NUMPY
-
-// sometimes, for labels and such, we need string arrays
-inline PyObject * get_array(const std::vector<std::string>& strings)
-{
- PyObject* list = PyList_New(strings.size());
- for (std::size_t i = 0; i < strings.size(); ++i) {
- PyList_SetItem(list, i, PyString_FromString(strings[i].c_str()));
- }
- return list;
-}
-
-// not all matplotlib need 2d arrays, some prefer lists of lists
-template<typename Numeric>
-PyObject* get_listlist(const std::vector<std::vector<Numeric>>& ll)
-{
- PyObject* listlist = PyList_New(ll.size());
- for (std::size_t i = 0; i < ll.size(); ++i) {
- PyList_SetItem(listlist, i, get_array(ll[i]));
- }
- return listlist;
-}
-
-} // namespace detail
-
-/// Plot a line through the given x and y data points..
-///
-/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html
-template<typename Numeric>
-bool plot(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords)
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- // using numpy arrays
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- // construct positional args
- PyObject* args = PyTuple_New(2);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, yarray);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-// TODO - it should be possible to make this work by implementing
-// a non-numpy alternative for `detail::get_2darray()`.
-#ifndef WITHOUT_NUMPY
-template <typename Numeric>
-void plot_surface(const std::vector<::std::vector<Numeric>> &x,
- const std::vector<::std::vector<Numeric>> &y,
- const std::vector<::std::vector<Numeric>> &z,
- const std::map<std::string, std::string> &keywords =
- std::map<std::string, std::string>(),
- const long fig_number=0)
-{
- detail::_interpreter::get();
-
- // We lazily load the modules here the first time this function is called
- // because I'm not sure that we can assume "matplotlib installed" implies
- // "mpl_toolkits installed" on all platforms, and we don't want to require
- // it for people who don't need 3d plots.
- static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
- if (!mpl_toolkitsmod) {
- detail::_interpreter::get();
-
- PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
- PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
- if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
-
- mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
- Py_DECREF(mpl_toolkits);
- if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
-
- axis3dmod = PyImport_Import(axis3d);
- Py_DECREF(axis3d);
- if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
- }
-
- assert(x.size() == y.size());
- assert(y.size() == z.size());
-
- // using numpy arrays
- PyObject *xarray = detail::get_2darray(x);
- PyObject *yarray = detail::get_2darray(y);
- PyObject *zarray = detail::get_2darray(z);
-
- // construct positional args
- PyObject *args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, yarray);
- PyTuple_SetItem(args, 2, zarray);
-
- // Build up the kw args.
- PyObject *kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1));
- PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1));
-
- PyObject *python_colormap_coolwarm = PyObject_GetAttrString(
- detail::_interpreter::get().s_python_colormap, "coolwarm");
-
- PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm);
-
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
- it != keywords.end(); ++it) {
- if (it->first == "linewidth" || it->first == "alpha") {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyFloat_FromDouble(std::stod(it->second)));
- } else {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyString_FromString(it->second.c_str()));
- }
- }
-
- PyObject *fig_args = PyTuple_New(1);
- PyObject* fig = nullptr;
- PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number));
- PyObject *fig_exists =
- PyObject_CallObject(
- detail::_interpreter::get().s_python_function_fignum_exists, fig_args);
- if (!PyObject_IsTrue(fig_exists)) {
- fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
- detail::_interpreter::get().s_python_empty_tuple);
- } else {
- fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
- fig_args);
- }
- Py_DECREF(fig_exists);
- if (!fig) throw std::runtime_error("Call to figure() failed.");
-
- PyObject *gca_kwargs = PyDict_New();
- PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
-
- PyObject *gca = PyObject_GetAttrString(fig, "gca");
- if (!gca) throw std::runtime_error("No gca");
- Py_INCREF(gca);
- PyObject *axis = PyObject_Call(
- gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
-
- if (!axis) throw std::runtime_error("No axis");
- Py_INCREF(axis);
-
- Py_DECREF(gca);
- Py_DECREF(gca_kwargs);
-
- PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface");
- if (!plot_surface) throw std::runtime_error("No surface");
- Py_INCREF(plot_surface);
- PyObject *res = PyObject_Call(plot_surface, args, kwargs);
- if (!res) throw std::runtime_error("failed surface");
- Py_DECREF(plot_surface);
-
- Py_DECREF(axis);
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if (res) Py_DECREF(res);
-}
-
-template <typename Numeric>
-void contour(const std::vector<::std::vector<Numeric>> &x,
- const std::vector<::std::vector<Numeric>> &y,
- const std::vector<::std::vector<Numeric>> &z,
- const std::map<std::string, std::string> &keywords = {})
-{
- detail::_interpreter::get();
-
- // using numpy arrays
- PyObject *xarray = detail::get_2darray(x);
- PyObject *yarray = detail::get_2darray(y);
- PyObject *zarray = detail::get_2darray(z);
-
- // construct positional args
- PyObject *args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, yarray);
- PyTuple_SetItem(args, 2, zarray);
-
- // Build up the kw args.
- PyObject *kwargs = PyDict_New();
-
- PyObject *python_colormap_coolwarm = PyObject_GetAttrString(
- detail::_interpreter::get().s_python_colormap, "coolwarm");
-
- PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm);
-
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
- it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyString_FromString(it->second.c_str()));
- }
-
- PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_contour, args, kwargs);
- if (!res)
- throw std::runtime_error("failed contour");
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if (res) Py_DECREF(res);
-}
-
-template <typename Numeric>
-void spy(const std::vector<::std::vector<Numeric>> &x,
- const double markersize = -1, // -1 for default matplotlib size
- const std::map<std::string, std::string> &keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject *xarray = detail::get_2darray(x);
-
- PyObject *kwargs = PyDict_New();
- if (markersize != -1) {
- PyDict_SetItemString(kwargs, "markersize", PyFloat_FromDouble(markersize));
- }
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
- it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyString_FromString(it->second.c_str()));
- }
-
- PyObject *plot_args = PyTuple_New(1);
- PyTuple_SetItem(plot_args, 0, xarray);
-
- PyObject *res = PyObject_Call(
- detail::_interpreter::get().s_python_function_spy, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if (res) Py_DECREF(res);
-}
-#endif // WITHOUT_NUMPY
-
-template <typename Numeric>
-void plot3(const std::vector<Numeric> &x,
- const std::vector<Numeric> &y,
- const std::vector<Numeric> &z,
- const std::map<std::string, std::string> &keywords =
- std::map<std::string, std::string>(),
- const long fig_number=0)
-{
- detail::_interpreter::get();
-
- // Same as with plot_surface: We lazily load the modules here the first time
- // this function is called because I'm not sure that we can assume "matplotlib
- // installed" implies "mpl_toolkits installed" on all platforms, and we don't
- // want to require it for people who don't need 3d plots.
- static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
- if (!mpl_toolkitsmod) {
- detail::_interpreter::get();
-
- PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
- PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
- if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
-
- mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
- Py_DECREF(mpl_toolkits);
- if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
-
- axis3dmod = PyImport_Import(axis3d);
- Py_DECREF(axis3d);
- if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
- }
-
- assert(x.size() == y.size());
- assert(y.size() == z.size());
-
- PyObject *xarray = detail::get_array(x);
- PyObject *yarray = detail::get_array(y);
- PyObject *zarray = detail::get_array(z);
-
- // construct positional args
- PyObject *args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, yarray);
- PyTuple_SetItem(args, 2, zarray);
-
- // Build up the kw args.
- PyObject *kwargs = PyDict_New();
-
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
- it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyString_FromString(it->second.c_str()));
- }
-
- PyObject *fig_args = PyTuple_New(1);
- PyObject* fig = nullptr;
- PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number));
- PyObject *fig_exists =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, fig_args);
- if (!PyObject_IsTrue(fig_exists)) {
- fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
- detail::_interpreter::get().s_python_empty_tuple);
- } else {
- fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
- fig_args);
- }
- if (!fig) throw std::runtime_error("Call to figure() failed.");
-
- PyObject *gca_kwargs = PyDict_New();
- PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
-
- PyObject *gca = PyObject_GetAttrString(fig, "gca");
- if (!gca) throw std::runtime_error("No gca");
- Py_INCREF(gca);
- PyObject *axis = PyObject_Call(
- gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
-
- if (!axis) throw std::runtime_error("No axis");
- Py_INCREF(axis);
-
- Py_DECREF(gca);
- Py_DECREF(gca_kwargs);
-
- PyObject *plot3 = PyObject_GetAttrString(axis, "plot");
- if (!plot3) throw std::runtime_error("No 3D line plot");
- Py_INCREF(plot3);
- PyObject *res = PyObject_Call(plot3, args, kwargs);
- if (!res) throw std::runtime_error("Failed 3D line plot");
- Py_DECREF(plot3);
-
- Py_DECREF(axis);
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if (res) Py_DECREF(res);
-}
-
-template<typename Numeric>
-bool stem(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords)
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- // using numpy arrays
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- // construct positional args
- PyObject* args = PyTuple_New(2);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, yarray);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for (std::map<std::string, std::string>::const_iterator it =
- keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyString_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(
- detail::_interpreter::get().s_python_function_stem, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if (res)
- Py_DECREF(res);
-
- return res;
-}
-
-template< typename Numeric >
-bool fill(const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::map<std::string, std::string>& keywords)
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- // using numpy arrays
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- // construct positional args
- PyObject* args = PyTuple_New(2);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, yarray);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
-
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-template< typename Numeric >
-bool fill_between(const std::vector<Numeric>& x, const std::vector<Numeric>& y1, const std::vector<Numeric>& y2, const std::map<std::string, std::string>& keywords)
-{
- assert(x.size() == y1.size());
- assert(x.size() == y2.size());
-
- detail::_interpreter::get();
-
- // using numpy arrays
- PyObject* xarray = detail::get_array(x);
- PyObject* y1array = detail::get_array(y1);
- PyObject* y2array = detail::get_array(y2);
-
- // construct positional args
- PyObject* args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, y1array);
- PyTuple_SetItem(args, 2, y2array);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template <typename Numeric>
-bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r",
- const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) {
- PyObject* obj_x = PyFloat_FromDouble(x);
- PyObject* obj_y = PyFloat_FromDouble(y);
- PyObject* obj_end_x = PyFloat_FromDouble(end_x);
- PyObject* obj_end_y = PyFloat_FromDouble(end_y);
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str()));
- PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str()));
- PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width));
- PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length));
-
- PyObject* plot_args = PyTuple_New(4);
- PyTuple_SetItem(plot_args, 0, obj_x);
- PyTuple_SetItem(plot_args, 1, obj_y);
- PyTuple_SetItem(plot_args, 2, obj_end_x);
- PyTuple_SetItem(plot_args, 3, obj_end_y);
-
- PyObject* res =
- PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if (res)
- Py_DECREF(res);
-
- return res;
-}
-
-template< typename Numeric>
-bool hist(const std::vector<Numeric>& y, long bins=10,std::string color="b",
- double alpha=1.0, bool cumulative=false)
-{
- detail::_interpreter::get();
-
- PyObject* yarray = detail::get_array(y);
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins));
- PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str()));
- PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha));
- PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False);
-
- PyObject* plot_args = PyTuple_New(1);
-
- PyTuple_SetItem(plot_args, 0, yarray);
-
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs);
-
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-#ifndef WITHOUT_NUMPY
-namespace detail {
-
-inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map<std::string, std::string> &keywords, PyObject** out)
-{
- assert(type == NPY_UINT8 || type == NPY_FLOAT);
- assert(colors == 1 || colors == 3 || colors == 4);
-
- detail::_interpreter::get();
-
- // construct args
- npy_intp dims[3] = { rows, columns, colors };
- PyObject *args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr));
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs);
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if (!res)
- throw std::runtime_error("Call to imshow() failed");
- if (out)
- *out = res;
- else
- Py_DECREF(res);
-}
-
-} // namespace detail
-
-inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map<std::string, std::string> &keywords = {}, PyObject** out = nullptr)
-{
- detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out);
-}
-
-inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map<std::string, std::string> &keywords = {}, PyObject** out = nullptr)
-{
- detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out);
-}
-
-#ifdef WITH_OPENCV
-void imshow(const cv::Mat &image, const std::map<std::string, std::string> &keywords = {})
-{
- // Convert underlying type of matrix, if needed
- cv::Mat image2;
- NPY_TYPES npy_type = NPY_UINT8;
- switch (image.type() & CV_MAT_DEPTH_MASK) {
- case CV_8U:
- image2 = image;
- break;
- case CV_32F:
- image2 = image;
- npy_type = NPY_FLOAT;
- break;
- default:
- image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels()));
- }
-
- // If color image, convert from BGR to RGB
- switch (image2.channels()) {
- case 3:
- cv::cvtColor(image2, image2, CV_BGR2RGB);
- break;
- case 4:
- cv::cvtColor(image2, image2, CV_BGRA2RGBA);
- }
-
- detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords);
-}
-#endif // WITH_OPENCV
-#endif // WITHOUT_NUMPY
-
-template<typename NumericX, typename NumericY>
-bool scatter(const std::vector<NumericX>& x,
- const std::vector<NumericY>& y,
- const double s=1.0, // The marker size in points**2
- const std::map<std::string, std::string> & keywords = {})
-{
- detail::_interpreter::get();
-
- assert(x.size() == y.size());
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s));
- for (const auto& it : keywords)
- {
- PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));
- }
-
- PyObject* plot_args = PyTuple_New(2);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY, typename NumericColors>
- bool scatter_colored(const std::vector<NumericX>& x,
- const std::vector<NumericY>& y,
- const std::vector<NumericColors>& colors,
- const double s=1.0, // The marker size in points**2
- const std::map<std::string, std::string> & keywords = {})
- {
- detail::_interpreter::get();
-
- assert(x.size() == y.size());
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
- PyObject* colors_array = detail::get_array(colors);
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s));
- PyDict_SetItemString(kwargs, "c", colors_array);
-
- for (const auto& it : keywords)
- {
- PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));
- }
-
- PyObject* plot_args = PyTuple_New(2);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if(res) Py_DECREF(res);
-
- return res;
- }
-
-
-template<typename NumericX, typename NumericY, typename NumericZ>
-bool scatter(const std::vector<NumericX>& x,
- const std::vector<NumericY>& y,
- const std::vector<NumericZ>& z,
- const double s=1.0, // The marker size in points**2
- const std::map<std::string, std::string> & keywords = {},
- const long fig_number=0) {
- detail::_interpreter::get();
-
- // Same as with plot_surface: We lazily load the modules here the first time
- // this function is called because I'm not sure that we can assume "matplotlib
- // installed" implies "mpl_toolkits installed" on all platforms, and we don't
- // want to require it for people who don't need 3d plots.
- static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
- if (!mpl_toolkitsmod) {
- detail::_interpreter::get();
-
- PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
- PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
- if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
-
- mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
- Py_DECREF(mpl_toolkits);
- if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
-
- axis3dmod = PyImport_Import(axis3d);
- Py_DECREF(axis3d);
- if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
- }
-
- assert(x.size() == y.size());
- assert(y.size() == z.size());
-
- PyObject *xarray = detail::get_array(x);
- PyObject *yarray = detail::get_array(y);
- PyObject *zarray = detail::get_array(z);
-
- // construct positional args
- PyObject *args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, xarray);
- PyTuple_SetItem(args, 1, yarray);
- PyTuple_SetItem(args, 2, zarray);
-
- // Build up the kw args.
- PyObject *kwargs = PyDict_New();
-
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
- it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyString_FromString(it->second.c_str()));
- }
- PyObject *fig_args = PyTuple_New(1);
- PyObject* fig = nullptr;
- PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number));
- PyObject *fig_exists =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, fig_args);
- if (!PyObject_IsTrue(fig_exists)) {
- fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
- detail::_interpreter::get().s_python_empty_tuple);
- } else {
- fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
- fig_args);
- }
- Py_DECREF(fig_exists);
- if (!fig) throw std::runtime_error("Call to figure() failed.");
-
- PyObject *gca_kwargs = PyDict_New();
- PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
-
- PyObject *gca = PyObject_GetAttrString(fig, "gca");
- if (!gca) throw std::runtime_error("No gca");
- Py_INCREF(gca);
- PyObject *axis = PyObject_Call(
- gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
-
- if (!axis) throw std::runtime_error("No axis");
- Py_INCREF(axis);
-
- Py_DECREF(gca);
- Py_DECREF(gca_kwargs);
-
- PyObject *plot3 = PyObject_GetAttrString(axis, "scatter");
- if (!plot3) throw std::runtime_error("No 3D line plot");
- Py_INCREF(plot3);
- PyObject *res = PyObject_Call(plot3, args, kwargs);
- if (!res) throw std::runtime_error("Failed 3D line plot");
- Py_DECREF(plot3);
-
- Py_DECREF(axis);
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(fig);
- if (res) Py_DECREF(res);
- return res;
-
-}
-
-template<typename Numeric>
-bool boxplot(const std::vector<std::vector<Numeric>>& data,
- const std::vector<std::string>& labels = {},
- const std::map<std::string, std::string> & keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject* listlist = detail::get_listlist(data);
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, listlist);
-
- PyObject* kwargs = PyDict_New();
-
- // kwargs needs the labels, if there are (the correct number of) labels
- if (!labels.empty() && labels.size() == data.size()) {
- PyDict_SetItemString(kwargs, "labels", detail::get_array(labels));
- }
-
- // take care of the remaining keywords
- for (const auto& it : keywords)
- {
- PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
-
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename Numeric>
-bool boxplot(const std::vector<Numeric>& data,
- const std::map<std::string, std::string> & keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject* vector = detail::get_array(data);
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, vector);
-
- PyObject* kwargs = PyDict_New();
- for (const auto& it : keywords)
- {
- PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
-
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template <typename Numeric>
-bool bar(const std::vector<Numeric> & x,
- const std::vector<Numeric> & y,
- std::string ec = "black",
- std::string ls = "-",
- double lw = 1.0,
- const std::map<std::string, std::string> & keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject * xarray = detail::get_array(x);
- PyObject * yarray = detail::get_array(y);
-
- PyObject * kwargs = PyDict_New();
-
- PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str()));
- PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str()));
- PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw));
-
- for (std::map<std::string, std::string>::const_iterator it =
- keywords.begin();
- it != keywords.end();
- ++it) {
- PyDict_SetItemString(
- kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject * plot_args = PyTuple_New(2);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
-
- PyObject * res = PyObject_Call(
- detail::_interpreter::get().s_python_function_bar, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-template <typename Numeric>
-bool bar(const std::vector<Numeric> & y,
- std::string ec = "black",
- std::string ls = "-",
- double lw = 1.0,
- const std::map<std::string, std::string> & keywords = {})
-{
- using T = typename std::remove_reference<decltype(y)>::type::value_type;
-
- detail::_interpreter::get();
-
- std::vector<T> x;
- for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); }
-
- return bar(x, y, ec, ls, lw, keywords);
-}
-
-
-template<typename Numeric>
-bool barh(const std::vector<Numeric> &x, const std::vector<Numeric> &y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map<std::string, std::string> &keywords = { }) {
- PyObject *xarray = detail::get_array(x);
- PyObject *yarray = detail::get_array(y);
-
- PyObject *kwargs = PyDict_New();
-
- PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str()));
- PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str()));
- PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw));
-
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject *plot_args = PyTuple_New(2);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
-
- PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_barh, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-
-inline bool subplots_adjust(const std::map<std::string, double>& keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject* kwargs = PyDict_New();
- for (std::map<std::string, double>::const_iterator it =
- keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyFloat_FromDouble(it->second));
- }
-
-
- PyObject* plot_args = PyTuple_New(0);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template< typename Numeric>
-bool named_hist(std::string label,const std::vector<Numeric>& y, long bins=10, std::string color="b", double alpha=1.0)
-{
- detail::_interpreter::get();
-
- PyObject* yarray = detail::get_array(y);
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str()));
- PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins));
- PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str()));
- PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha));
-
-
- PyObject* plot_args = PyTuple_New(1);
- PyTuple_SetItem(plot_args, 0, yarray);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs);
-
- Py_DECREF(plot_args);
- Py_DECREF(kwargs);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool plot(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(s.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args);
-
- Py_DECREF(plot_args);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template <typename NumericX, typename NumericY, typename NumericZ>
-bool contour(const std::vector<NumericX>& x, const std::vector<NumericY>& y,
- const std::vector<NumericZ>& z,
- const std::map<std::string, std::string>& keywords = {}) {
- assert(x.size() == y.size() && x.size() == z.size());
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
- PyObject* zarray = detail::get_array(z);
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, zarray);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
- it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res =
- PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res)
- Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY, typename NumericU, typename NumericW>
-bool quiver(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::vector<NumericU>& u, const std::vector<NumericW>& w, const std::map<std::string, std::string>& keywords = {})
-{
- assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size());
-
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
- PyObject* uarray = detail::get_array(u);
- PyObject* warray = detail::get_array(w);
-
- PyObject* plot_args = PyTuple_New(4);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, uarray);
- PyTuple_SetItem(plot_args, 3, warray);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(
- detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res)
- Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY, typename NumericZ, typename NumericU, typename NumericW, typename NumericV>
-bool quiver(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::vector<NumericZ>& z, const std::vector<NumericU>& u, const std::vector<NumericW>& w, const std::vector<NumericV>& v, const std::map<std::string, std::string>& keywords = {})
-{
- //set up 3d axes stuff
- static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
- if (!mpl_toolkitsmod) {
- detail::_interpreter::get();
-
- PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
- PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
- if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
-
- mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
- Py_DECREF(mpl_toolkits);
- if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
-
- axis3dmod = PyImport_Import(axis3d);
- Py_DECREF(axis3d);
- if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
- }
-
- //assert sizes match up
- assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size() && x.size() == z.size() && x.size() == v.size() && u.size() == v.size());
-
- //set up parameters
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
- PyObject* zarray = detail::get_array(z);
- PyObject* uarray = detail::get_array(u);
- PyObject* warray = detail::get_array(w);
- PyObject* varray = detail::get_array(v);
-
- PyObject* plot_args = PyTuple_New(6);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, zarray);
- PyTuple_SetItem(plot_args, 3, uarray);
- PyTuple_SetItem(plot_args, 4, warray);
- PyTuple_SetItem(plot_args, 5, varray);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- //get figure gca to enable 3d projection
- PyObject *fig =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
- detail::_interpreter::get().s_python_empty_tuple);
- if (!fig) throw std::runtime_error("Call to figure() failed.");
-
- PyObject *gca_kwargs = PyDict_New();
- PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
-
- PyObject *gca = PyObject_GetAttrString(fig, "gca");
- if (!gca) throw std::runtime_error("No gca");
- Py_INCREF(gca);
- PyObject *axis = PyObject_Call(
- gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
-
- if (!axis) throw std::runtime_error("No axis");
- Py_INCREF(axis);
- Py_DECREF(gca);
- Py_DECREF(gca_kwargs);
-
- //plot our boys bravely, plot them strongly, plot them with a wink and clap
- PyObject *plot3 = PyObject_GetAttrString(axis, "quiver");
- if (!plot3) throw std::runtime_error("No 3D line plot");
- Py_INCREF(plot3);
- PyObject* res = PyObject_Call(
- plot3, plot_args, kwargs);
- if (!res) throw std::runtime_error("Failed 3D plot");
- Py_DECREF(plot3);
- Py_DECREF(axis);
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res)
- Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool stem(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(s.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_CallObject(
- detail::_interpreter::get().s_python_function_stem, plot_args);
-
- Py_DECREF(plot_args);
- if (res)
- Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool semilogx(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(s.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args);
-
- Py_DECREF(plot_args);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool semilogy(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(s.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args);
-
- Py_DECREF(plot_args);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool loglog(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(s.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args);
-
- Py_DECREF(plot_args);
- if(res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool errorbar(const std::vector<NumericX> &x, const std::vector<NumericY> &y, const std::vector<NumericX> &yerr, const std::map<std::string, std::string> &keywords = {})
-{
- assert(x.size() == y.size());
-
- detail::_interpreter::get();
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
- PyObject* yerrarray = detail::get_array(yerr);
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyDict_SetItemString(kwargs, "yerr", yerrarray);
-
- PyObject *plot_args = PyTuple_New(2);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
-
- PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
-
- if (res)
- Py_DECREF(res);
- else
- throw std::runtime_error("Call to errorbar() failed.");
-
- return res;
-}
-
-template<typename Numeric>
-bool named_plot(const std::string& name, const std::vector<Numeric>& y, const std::string& format = "")
-{
- detail::_interpreter::get();
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
-
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(format.c_str());
-
- PyObject* plot_args = PyTuple_New(2);
-
- PyTuple_SetItem(plot_args, 0, yarray);
- PyTuple_SetItem(plot_args, 1, pystring);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool named_plot(const std::string& name, const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& format = "")
-{
- detail::_interpreter::get();
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(format.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool named_semilogx(const std::string& name, const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& format = "")
-{
- detail::_interpreter::get();
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(format.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool named_semilogy(const std::string& name, const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& format = "")
-{
- detail::_interpreter::get();
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(format.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename NumericX, typename NumericY>
-bool named_loglog(const std::string& name, const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& format = "")
-{
- detail::_interpreter::get();
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(format.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
- if (res) Py_DECREF(res);
-
- return res;
-}
-
-template<typename Numeric>
-bool plot(const std::vector<Numeric>& y, const std::string& format = "")
-{
- std::vector<Numeric> x(y.size());
- for(size_t i=0; i<x.size(); ++i) x.at(i) = i;
- return plot(x,y,format);
-}
-
-template<typename Numeric>
-bool plot(const std::vector<Numeric>& y, const std::map<std::string, std::string>& keywords)
-{
- std::vector<Numeric> x(y.size());
- for(size_t i=0; i<x.size(); ++i) x.at(i) = i;
- return plot(x,y,keywords);
-}
-
-template<typename Numeric>
-bool stem(const std::vector<Numeric>& y, const std::string& format = "")
-{
- std::vector<Numeric> x(y.size());
- for (size_t i = 0; i < x.size(); ++i) x.at(i) = i;
- return stem(x, y, format);
-}
-
-template<typename Numeric>
-void text(Numeric x, Numeric y, const std::string& s = "")
-{
- detail::_interpreter::get();
-
- PyObject* args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(x));
- PyTuple_SetItem(args, 1, PyFloat_FromDouble(y));
- PyTuple_SetItem(args, 2, PyString_FromString(s.c_str()));
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args);
- if(!res) throw std::runtime_error("Call to text() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-inline void colorbar(PyObject* mappable = NULL, const std::map<std::string, float>& keywords = {})
-{
- if (mappable == NULL)
- throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc.");
-
- detail::_interpreter::get();
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, mappable);
-
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, float>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs);
- if(!res) throw std::runtime_error("Call to colorbar() failed.");
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(res);
-}
-
-
-inline long figure(long number = -1)
-{
- detail::_interpreter::get();
-
- PyObject *res;
- if (number == -1)
- res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple);
- else {
- assert(number > 0);
-
- // Make sure interpreter is initialised
- detail::_interpreter::get();
-
- PyObject *args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyLong_FromLong(number));
- res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args);
- Py_DECREF(args);
- }
-
- if(!res) throw std::runtime_error("Call to figure() failed.");
-
- PyObject* num = PyObject_GetAttrString(res, "number");
- if (!num) throw std::runtime_error("Could not get number attribute of figure object");
- const long figureNumber = PyLong_AsLong(num);
-
- Py_DECREF(num);
- Py_DECREF(res);
-
- return figureNumber;
-}
-
-inline bool fignum_exists(long number)
-{
- detail::_interpreter::get();
-
- PyObject *args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyLong_FromLong(number));
- PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args);
- if(!res) throw std::runtime_error("Call to fignum_exists() failed.");
-
- bool ret = PyObject_IsTrue(res);
- Py_DECREF(res);
- Py_DECREF(args);
-
- return ret;
-}
-
-inline void figure_size(size_t w, size_t h)
-{
- detail::_interpreter::get();
-
- const size_t dpi = 100;
- PyObject* size = PyTuple_New(2);
- PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi));
- PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi));
-
- PyObject* kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "figsize", size);
- PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi));
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure,
- detail::_interpreter::get().s_python_empty_tuple, kwargs);
-
- Py_DECREF(kwargs);
-
- if(!res) throw std::runtime_error("Call to figure_size() failed.");
- Py_DECREF(res);
-}
-
-inline void legend()
-{
- detail::_interpreter::get();
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple);
- if(!res) throw std::runtime_error("Call to legend() failed.");
-
- Py_DECREF(res);
-}
-
-inline void legend(const std::map<std::string, std::string>& keywords)
-{
- detail::_interpreter::get();
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs);
- if(!res) throw std::runtime_error("Call to legend() failed.");
-
- Py_DECREF(kwargs);
- Py_DECREF(res);
-}
-
-template<typename Numeric>
-inline void set_aspect(Numeric ratio)
-{
- detail::_interpreter::get();
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(ratio));
- PyObject* kwargs = PyDict_New();
-
- PyObject *ax =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_gca,
- detail::_interpreter::get().s_python_empty_tuple);
- if (!ax) throw std::runtime_error("Call to gca() failed.");
- Py_INCREF(ax);
-
- PyObject *set_aspect = PyObject_GetAttrString(ax, "set_aspect");
- if (!set_aspect) throw std::runtime_error("Attribute set_aspect not found.");
- Py_INCREF(set_aspect);
-
- PyObject *res = PyObject_Call(set_aspect, args, kwargs);
- if (!res) throw std::runtime_error("Call to set_aspect() failed.");
- Py_DECREF(set_aspect);
-
- Py_DECREF(ax);
- Py_DECREF(args);
- Py_DECREF(kwargs);
-}
-
-inline void set_aspect_equal()
-{
- // expect ratio == "equal". Leaving error handling to matplotlib.
- detail::_interpreter::get();
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyString_FromString("equal"));
- PyObject* kwargs = PyDict_New();
-
- PyObject *ax =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_gca,
- detail::_interpreter::get().s_python_empty_tuple);
- if (!ax) throw std::runtime_error("Call to gca() failed.");
- Py_INCREF(ax);
-
- PyObject *set_aspect = PyObject_GetAttrString(ax, "set_aspect");
- if (!set_aspect) throw std::runtime_error("Attribute set_aspect not found.");
- Py_INCREF(set_aspect);
-
- PyObject *res = PyObject_Call(set_aspect, args, kwargs);
- if (!res) throw std::runtime_error("Call to set_aspect() failed.");
- Py_DECREF(set_aspect);
-
- Py_DECREF(ax);
- Py_DECREF(args);
- Py_DECREF(kwargs);
-}
-
-template<typename Numeric>
-void ylim(Numeric left, Numeric right)
-{
- detail::_interpreter::get();
-
- PyObject* list = PyList_New(2);
- PyList_SetItem(list, 0, PyFloat_FromDouble(left));
- PyList_SetItem(list, 1, PyFloat_FromDouble(right));
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, list);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args);
- if(!res) throw std::runtime_error("Call to ylim() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-template<typename Numeric>
-void xlim(Numeric left, Numeric right)
-{
- detail::_interpreter::get();
-
- PyObject* list = PyList_New(2);
- PyList_SetItem(list, 0, PyFloat_FromDouble(left));
- PyList_SetItem(list, 1, PyFloat_FromDouble(right));
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, list);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args);
- if(!res) throw std::runtime_error("Call to xlim() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-
-inline std::array<double, 2> xlim()
-{
- PyObject* args = PyTuple_New(0);
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args);
-
- if(!res) throw std::runtime_error("Call to xlim() failed.");
-
- Py_DECREF(res);
-
- PyObject* left = PyTuple_GetItem(res,0);
- PyObject* right = PyTuple_GetItem(res,1);
- return { PyFloat_AsDouble(left), PyFloat_AsDouble(right) };
-}
-
-
-inline std::array<double, 2> ylim()
-{
- PyObject* args = PyTuple_New(0);
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args);
-
- if(!res) throw std::runtime_error("Call to ylim() failed.");
-
- Py_DECREF(res);
-
- PyObject* left = PyTuple_GetItem(res,0);
- PyObject* right = PyTuple_GetItem(res,1);
- return { PyFloat_AsDouble(left), PyFloat_AsDouble(right) };
-}
-
-template<typename Numeric>
-inline void xticks(const std::vector<Numeric> &ticks, const std::vector<std::string> &labels = {}, const std::map<std::string, std::string>& keywords = {})
-{
- assert(labels.size() == 0 || ticks.size() == labels.size());
-
- detail::_interpreter::get();
-
- // using numpy array
- PyObject* ticksarray = detail::get_array(ticks);
-
- PyObject* args;
- if(labels.size() == 0) {
- // construct positional args
- args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, ticksarray);
- } else {
- // make tuple of tick labels
- PyObject* labelstuple = PyTuple_New(labels.size());
- for (size_t i = 0; i < labels.size(); i++)
- PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str()));
-
- // construct positional args
- args = PyTuple_New(2);
- PyTuple_SetItem(args, 0, ticksarray);
- PyTuple_SetItem(args, 1, labelstuple);
- }
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if(!res) throw std::runtime_error("Call to xticks() failed");
-
- Py_DECREF(res);
-}
-
-template<typename Numeric>
-inline void xticks(const std::vector<Numeric> &ticks, const std::map<std::string, std::string>& keywords)
-{
- xticks(ticks, {}, keywords);
-}
-
-template<typename Numeric>
-inline void yticks(const std::vector<Numeric> &ticks, const std::vector<std::string> &labels = {}, const std::map<std::string, std::string>& keywords = {})
-{
- assert(labels.size() == 0 || ticks.size() == labels.size());
-
- detail::_interpreter::get();
-
- // using numpy array
- PyObject* ticksarray = detail::get_array(ticks);
-
- PyObject* args;
- if(labels.size() == 0) {
- // construct positional args
- args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, ticksarray);
- } else {
- // make tuple of tick labels
- PyObject* labelstuple = PyTuple_New(labels.size());
- for (size_t i = 0; i < labels.size(); i++)
- PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str()));
-
- // construct positional args
- args = PyTuple_New(2);
- PyTuple_SetItem(args, 0, ticksarray);
- PyTuple_SetItem(args, 1, labelstuple);
- }
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if(!res) throw std::runtime_error("Call to yticks() failed");
-
- Py_DECREF(res);
-}
-
-template<typename Numeric>
-inline void yticks(const std::vector<Numeric> &ticks, const std::map<std::string, std::string>& keywords)
-{
- yticks(ticks, {}, keywords);
-}
-
-template <typename Numeric> inline void margins(Numeric margin)
-{
- // construct positional args
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin));
-
- PyObject* res =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args);
- if (!res)
- throw std::runtime_error("Call to margins() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-template <typename Numeric> inline void margins(Numeric margin_x, Numeric margin_y)
-{
- // construct positional args
- PyObject* args = PyTuple_New(2);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x));
- PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y));
-
- PyObject* res =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args);
- if (!res)
- throw std::runtime_error("Call to margins() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-
-inline void tick_params(const std::map<std::string, std::string>& keywords, const std::string axis = "both")
-{
- detail::_interpreter::get();
-
- // construct positional args
- PyObject* args;
- args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str()));
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for (std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if (!res) throw std::runtime_error("Call to tick_params() failed");
-
- Py_DECREF(res);
-}
-
-inline void subplot(long nrows, long ncols, long plot_number)
-{
- detail::_interpreter::get();
-
- // construct positional args
- PyObject* args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows));
- PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols));
- PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number));
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args);
- if(!res) throw std::runtime_error("Call to subplot() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1)
-{
- detail::_interpreter::get();
-
- PyObject* shape = PyTuple_New(2);
- PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows));
- PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols));
-
- PyObject* loc = PyTuple_New(2);
- PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid));
- PyTuple_SetItem(loc, 1, PyLong_FromLong(colid));
-
- PyObject* args = PyTuple_New(4);
- PyTuple_SetItem(args, 0, shape);
- PyTuple_SetItem(args, 1, loc);
- PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan));
- PyTuple_SetItem(args, 3, PyLong_FromLong(colspan));
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args);
- if(!res) throw std::runtime_error("Call to subplot2grid() failed.");
-
- Py_DECREF(shape);
- Py_DECREF(loc);
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-inline void title(const std::string &titlestr, const std::map<std::string, std::string> &keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject* pytitlestr = PyString_FromString(titlestr.c_str());
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, pytitlestr);
-
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs);
- if(!res) throw std::runtime_error("Call to title() failed.");
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(res);
-}
-
-inline void suptitle(const std::string &suptitlestr, const std::map<std::string, std::string> &keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str());
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, pysuptitlestr);
-
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs);
- if(!res) throw std::runtime_error("Call to suptitle() failed.");
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(res);
-}
-
-inline void axis(const std::string &axisstr)
-{
- detail::_interpreter::get();
-
- PyObject* str = PyString_FromString(axisstr.c_str());
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, str);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args);
- if(!res) throw std::runtime_error("Call to title() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-inline void axhline(double y, double xmin = 0., double xmax = 1., const std::map<std::string, std::string>& keywords = std::map<std::string, std::string>())
-{
- detail::_interpreter::get();
-
- // construct positional args
- PyObject* args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(y));
- PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmin));
- PyTuple_SetItem(args, 2, PyFloat_FromDouble(xmax));
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axhline, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
-
- if(res) Py_DECREF(res);
-}
-
-inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map<std::string, std::string>& keywords = std::map<std::string, std::string>())
-{
- detail::_interpreter::get();
-
- // construct positional args
- PyObject* args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(x));
- PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin));
- PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax));
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs);
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
-
- if(res) Py_DECREF(res);
-}
-
-inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map<std::string, std::string>& keywords = std::map<std::string, std::string>())
-{
- // construct positional args
- PyObject* args = PyTuple_New(4);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin));
- PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax));
- PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin));
- PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax));
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- if (it->first == "linewidth" || it->first == "alpha") {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyFloat_FromDouble(std::stod(it->second)));
- } else {
- PyDict_SetItemString(kwargs, it->first.c_str(),
- PyString_FromString(it->second.c_str()));
- }
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs);
- Py_DECREF(args);
- Py_DECREF(kwargs);
-
- if(res) Py_DECREF(res);
-}
-
-inline void xlabel(const std::string &str, const std::map<std::string, std::string> &keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject* pystr = PyString_FromString(str.c_str());
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, pystr);
-
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs);
- if(!res) throw std::runtime_error("Call to xlabel() failed.");
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(res);
-}
-
-inline void ylabel(const std::string &str, const std::map<std::string, std::string>& keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject* pystr = PyString_FromString(str.c_str());
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, pystr);
-
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs);
- if(!res) throw std::runtime_error("Call to ylabel() failed.");
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(res);
-}
-
-inline void set_zlabel(const std::string &str, const std::map<std::string, std::string>& keywords = {})
-{
- detail::_interpreter::get();
-
- // Same as with plot_surface: We lazily load the modules here the first time
- // this function is called because I'm not sure that we can assume "matplotlib
- // installed" implies "mpl_toolkits installed" on all platforms, and we don't
- // want to require it for people who don't need 3d plots.
- static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
- if (!mpl_toolkitsmod) {
- PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
- PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
- if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
-
- mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
- Py_DECREF(mpl_toolkits);
- if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
-
- axis3dmod = PyImport_Import(axis3d);
- Py_DECREF(axis3d);
- if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
- }
-
- PyObject* pystr = PyString_FromString(str.c_str());
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, pystr);
-
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject *ax =
- PyObject_CallObject(detail::_interpreter::get().s_python_function_gca,
- detail::_interpreter::get().s_python_empty_tuple);
- if (!ax) throw std::runtime_error("Call to gca() failed.");
- Py_INCREF(ax);
-
- PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel");
- if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found.");
- Py_INCREF(zlabel);
-
- PyObject *res = PyObject_Call(zlabel, args, kwargs);
- if (!res) throw std::runtime_error("Call to set_zlabel() failed.");
- Py_DECREF(zlabel);
-
- Py_DECREF(ax);
- Py_DECREF(args);
- Py_DECREF(kwargs);
- if (res) Py_DECREF(res);
-}
-
-inline void grid(bool flag)
-{
- detail::_interpreter::get();
-
- PyObject* pyflag = flag ? Py_True : Py_False;
- Py_INCREF(pyflag);
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, pyflag);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args);
- if(!res) throw std::runtime_error("Call to grid() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-inline void show(const bool block = true)
-{
- detail::_interpreter::get();
-
- PyObject* res;
- if(block)
- {
- res = PyObject_CallObject(
- detail::_interpreter::get().s_python_function_show,
- detail::_interpreter::get().s_python_empty_tuple);
- }
- else
- {
- PyObject *kwargs = PyDict_New();
- PyDict_SetItemString(kwargs, "block", Py_False);
- res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs);
- Py_DECREF(kwargs);
- }
-
-
- if (!res) throw std::runtime_error("Call to show() failed.");
-
- Py_DECREF(res);
-}
-
-inline void close()
-{
- detail::_interpreter::get();
-
- PyObject* res = PyObject_CallObject(
- detail::_interpreter::get().s_python_function_close,
- detail::_interpreter::get().s_python_empty_tuple);
-
- if (!res) throw std::runtime_error("Call to close() failed.");
-
- Py_DECREF(res);
-}
-
-inline void xkcd() {
- detail::_interpreter::get();
-
- PyObject* res;
- PyObject *kwargs = PyDict_New();
-
- res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd,
- detail::_interpreter::get().s_python_empty_tuple, kwargs);
-
- Py_DECREF(kwargs);
-
- if (!res)
- throw std::runtime_error("Call to show() failed.");
-
- Py_DECREF(res);
-}
-
-inline void draw()
-{
- detail::_interpreter::get();
-
- PyObject* res = PyObject_CallObject(
- detail::_interpreter::get().s_python_function_draw,
- detail::_interpreter::get().s_python_empty_tuple);
-
- if (!res) throw std::runtime_error("Call to draw() failed.");
-
- Py_DECREF(res);
-}
-
-template<typename Numeric>
-inline void pause(Numeric interval)
-{
- detail::_interpreter::get();
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval));
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args);
- if(!res) throw std::runtime_error("Call to pause() failed.");
-
- Py_DECREF(args);
- Py_DECREF(res);
-}
-
-inline void save(const std::string& filename, const int dpi=0)
-{
- detail::_interpreter::get();
-
- PyObject* pyfilename = PyString_FromString(filename.c_str());
-
- PyObject* args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, pyfilename);
-
- PyObject* kwargs = PyDict_New();
-
- if(dpi > 0)
- {
- PyDict_SetItemString(kwargs, "dpi", PyLong_FromLong(dpi));
- }
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_save, args, kwargs);
- if (!res) throw std::runtime_error("Call to save() failed.");
-
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(res);
-}
-
-inline void rcparams(const std::map<std::string, std::string>& keywords = {}) {
- detail::_interpreter::get();
- PyObject* args = PyTuple_New(0);
- PyObject* kwargs = PyDict_New();
- for (auto it = keywords.begin(); it != keywords.end(); ++it) {
- if ("text.usetex" == it->first)
- PyDict_SetItemString(kwargs, it->first.c_str(), PyLong_FromLong(std::stoi(it->second.c_str())));
- else PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
- }
-
- PyObject * update = PyObject_GetAttrString(detail::_interpreter::get().s_python_function_rcparams, "update");
- PyObject * res = PyObject_Call(update, args, kwargs);
- if(!res) throw std::runtime_error("Call to rcParams.update() failed.");
- Py_DECREF(args);
- Py_DECREF(kwargs);
- Py_DECREF(update);
- Py_DECREF(res);
-}
-
-inline void clf() {
- detail::_interpreter::get();
-
- PyObject *res = PyObject_CallObject(
- detail::_interpreter::get().s_python_function_clf,
- detail::_interpreter::get().s_python_empty_tuple);
-
- if (!res) throw std::runtime_error("Call to clf() failed.");
-
- Py_DECREF(res);
-}
-
-inline void cla() {
- detail::_interpreter::get();
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_cla,
- detail::_interpreter::get().s_python_empty_tuple);
-
- if (!res)
- throw std::runtime_error("Call to cla() failed.");
-
- Py_DECREF(res);
-}
-
-inline void ion() {
- detail::_interpreter::get();
-
- PyObject *res = PyObject_CallObject(
- detail::_interpreter::get().s_python_function_ion,
- detail::_interpreter::get().s_python_empty_tuple);
-
- if (!res) throw std::runtime_error("Call to ion() failed.");
-
- Py_DECREF(res);
-}
-
-inline std::vector<std::array<double, 2>> ginput(const int numClicks = 1, const std::map<std::string, std::string>& keywords = {})
-{
- detail::_interpreter::get();
-
- PyObject *args = PyTuple_New(1);
- PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks));
-
- // construct keyword args
- PyObject* kwargs = PyDict_New();
- for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
- {
- PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
- }
-
- PyObject* res = PyObject_Call(
- detail::_interpreter::get().s_python_function_ginput, args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(args);
- if (!res) throw std::runtime_error("Call to ginput() failed.");
-
- const size_t len = PyList_Size(res);
- std::vector<std::array<double, 2>> out;
- out.reserve(len);
- for (size_t i = 0; i < len; i++) {
- PyObject *current = PyList_GetItem(res, i);
- std::array<double, 2> position;
- position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0));
- position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1));
- out.push_back(position);
- }
- Py_DECREF(res);
-
- return out;
-}
-
-// Actually, is there any reason not to call this automatically for every plot?
-inline void tight_layout() {
- detail::_interpreter::get();
-
- PyObject *res = PyObject_CallObject(
- detail::_interpreter::get().s_python_function_tight_layout,
- detail::_interpreter::get().s_python_empty_tuple);
-
- if (!res) throw std::runtime_error("Call to tight_layout() failed.");
-
- Py_DECREF(res);
-}
-
-// Support for variadic plot() and initializer lists:
-
-namespace detail {
-
-template<typename T>
-using is_function = typename std::is_function<std::remove_pointer<std::remove_reference<T>>>::type;
-
-template<bool obj, typename T>
-struct is_callable_impl;
-
-template<typename T>
-struct is_callable_impl<false, T>
-{
- typedef is_function<T> type;
-}; // a non-object is callable iff it is a function
-
-template<typename T>
-struct is_callable_impl<true, T>
-{
- struct Fallback { void operator()(); };
- struct Derived : T, Fallback { };
-
- template<typename U, U> struct Check;
-
- template<typename U>
- static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match
-
- template<typename U>
- static std::false_type test( Check<void(Fallback::*)(), &U::operator()>* );
-
-public:
- typedef decltype(test<Derived>(nullptr)) type;
- typedef decltype(&Fallback::operator()) dtype;
- static constexpr bool value = type::value;
-}; // an object is callable iff it defines operator()
-
-template<typename T>
-struct is_callable
-{
- // dispatch to is_callable_impl<true, T> or is_callable_impl<false, T> depending on whether T is of class type or not
- typedef typename is_callable_impl<std::is_class<T>::value, T>::type type;
-};
-
-template<typename IsYDataCallable>
-struct plot_impl { };
-
-template<>
-struct plot_impl<std::false_type>
-{
- template<typename IterableX, typename IterableY>
- bool operator()(const IterableX& x, const IterableY& y, const std::string& format)
- {
- detail::_interpreter::get();
-
- // 2-phase lookup for distance, begin, end
- using std::distance;
- using std::begin;
- using std::end;
-
- auto xs = distance(begin(x), end(x));
- auto ys = distance(begin(y), end(y));
- assert(xs == ys && "x and y data must have the same number of elements!");
-
- PyObject* xlist = PyList_New(xs);
- PyObject* ylist = PyList_New(ys);
- PyObject* pystring = PyString_FromString(format.c_str());
-
- auto itx = begin(x), ity = begin(y);
- for(size_t i = 0; i < xs; ++i) {
- PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++));
- PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++));
- }
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xlist);
- PyTuple_SetItem(plot_args, 1, ylist);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args);
-
- Py_DECREF(plot_args);
- if(res) Py_DECREF(res);
-
- return res;
- }
-};
-
-template<>
-struct plot_impl<std::true_type>
-{
- template<typename Iterable, typename Callable>
- bool operator()(const Iterable& ticks, const Callable& f, const std::string& format)
- {
- if(begin(ticks) == end(ticks)) return true;
-
- // We could use additional meta-programming to deduce the correct element type of y,
- // but all values have to be convertible to double anyways
- std::vector<double> y;
- for(auto x : ticks) y.push_back(f(x));
- return plot_impl<std::false_type>()(ticks,y,format);
- }
-};
-
-} // end namespace detail
-
-// recursion stop for the above
-template<typename... Args>
-bool plot() { return true; }
-
-template<typename A, typename B, typename... Args>
-bool plot(const A& a, const B& b, const std::string& format, Args... args)
-{
- return detail::plot_impl<typename detail::is_callable<B>::type>()(a,b,format) && plot(args...);
-}
-
-/*
- * This group of plot() functions is needed to support initializer lists, i.e. calling
- * plot( {1,2,3,4} )
- */
-inline bool plot(const std::vector<double>& x, const std::vector<double>& y, const std::string& format = "") {
- return plot<double,double>(x,y,format);
-}
-
-inline bool plot(const std::vector<double>& y, const std::string& format = "") {
- return plot<double>(y,format);
-}
-
-inline bool plot(const std::vector<double>& x, const std::vector<double>& y, const std::map<std::string, std::string>& keywords) {
- return plot<double>(x,y,keywords);
-}
-
-/*
- * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting
- */
-class Plot
-{
-public:
- // default initialization with plot label, some data and format
- template<typename Numeric>
- Plot(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "") {
- detail::_interpreter::get();
-
- assert(x.size() == y.size());
-
- PyObject* kwargs = PyDict_New();
- if(name != "")
- PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
-
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* pystring = PyString_FromString(format.c_str());
-
- PyObject* plot_args = PyTuple_New(3);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
- PyTuple_SetItem(plot_args, 2, pystring);
-
- PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);
-
- Py_DECREF(kwargs);
- Py_DECREF(plot_args);
-
- if(res)
- {
- line= PyList_GetItem(res, 0);
-
- if(line)
- set_data_fct = PyObject_GetAttrString(line,"set_data");
- else
- Py_DECREF(line);
- Py_DECREF(res);
- }
- }
-
- // shorter initialization with name or format only
- // basically calls line, = plot([], [])
- Plot(const std::string& name = "", const std::string& format = "")
- : Plot(name, std::vector<double>(), std::vector<double>(), format) {}
-
- template<typename Numeric>
- bool update(const std::vector<Numeric>& x, const std::vector<Numeric>& y) {
- assert(x.size() == y.size());
- if(set_data_fct)
- {
- PyObject* xarray = detail::get_array(x);
- PyObject* yarray = detail::get_array(y);
-
- PyObject* plot_args = PyTuple_New(2);
- PyTuple_SetItem(plot_args, 0, xarray);
- PyTuple_SetItem(plot_args, 1, yarray);
-
- PyObject* res = PyObject_CallObject(set_data_fct, plot_args);
- if (res) Py_DECREF(res);
- return res;
- }
- return false;
- }
-
- // clears the plot but keep it available
- bool clear() {
- return update(std::vector<double>(), std::vector<double>());
- }
-
- // definitely remove this line
- void remove() {
- if(line)
- {
- auto remove_fct = PyObject_GetAttrString(line,"remove");
- PyObject* args = PyTuple_New(0);
- PyObject* res = PyObject_CallObject(remove_fct, args);
- if (res) Py_DECREF(res);
- }
- decref();
- }
-
- ~Plot() {
- decref();
- }
-private:
-
- void decref() {
- if(line)
- Py_DECREF(line);
- if(set_data_fct)
- Py_DECREF(set_data_fct);
- }
-
-
- PyObject* line = nullptr;
- PyObject* set_data_fct = nullptr;
-};
-
-} // end namespace matplotlibcpp