Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • labrobotica/algorithms/laser_scan_utils
1 result
Show changes
Commits on Source (33)
Showing
with 394 additions and 430 deletions
...@@ -2,6 +2,8 @@ ...@@ -2,6 +2,8 @@
bin/ bin/
build*/ build*/
lib/ lib/
doc/
cmake_modules/
.settings/language.settings.xml .settings/language.settings.xml
.project .project
.cproject .cproject
......
...@@ -50,14 +50,17 @@ stages: ...@@ -50,14 +50,17 @@ stages:
- if [ -f license_header_${CURRENT_YEAR}.txt ]; then - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
# add license headers to new files # 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..." - 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 - 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 )) - export PREV_YEAR=$(( CURRENT_YEAR-1 ))
- echo "Creating new file license_header_${CURRENT_YEAR}.txt..." - echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
- git mv license_header_${PREV_YEAR}.txt 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 - sed -i "s/${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 # add new license headers to all files
- ./license_manager.sh --add --path=${CI_PROJECT_DIR} --config-path=. --exclude=ci_deps
- fi - fi
- cd .. - cd ..
......
...@@ -39,14 +39,6 @@ IF(NOT BUILD_DEMOS) ...@@ -39,14 +39,6 @@ IF(NOT BUILD_DEMOS)
OPTION(BUILD_DEMOS "Build demos" ON) OPTION(BUILD_DEMOS "Build demos" ON)
ENDIF(NOT BUILD_DEMOS) 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 ============ # ============ DEPENDENCIES ============
FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG) FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
FIND_PACKAGE(falkolib QUIET) FIND_PACKAGE(falkolib QUIET)
...@@ -104,6 +96,7 @@ SET(HDRS ...@@ -104,6 +96,7 @@ SET(HDRS
SET(HDRS ${HDRS} SET(HDRS ${HDRS}
include/${PROJECT_NAME}/corner_falko_2d.h include/${PROJECT_NAME}/corner_falko_2d.h
include/${PROJECT_NAME}/loop_closure_falko.h include/${PROJECT_NAME}/loop_closure_falko.h
include/${PROJECT_NAME}/scene_falko_base.h
include/${PROJECT_NAME}/scene_falko.h include/${PROJECT_NAME}/scene_falko.h
include/${PROJECT_NAME}/match_loop_closure_scene.h) include/${PROJECT_NAME}/match_loop_closure_scene.h)
ENDIF(falkolib_FOUND) ENDIF(falkolib_FOUND)
...@@ -171,7 +164,7 @@ ENDIF(BUILD_DEMOS) ...@@ -171,7 +164,7 @@ ENDIF(BUILD_DEMOS)
#Build tests #Build tests
IF(BUILD_TESTS) IF(BUILD_TESTS)
MESSAGE("Building tests.") MESSAGE("Building tests.")
set(_LASER_SCAN_UTILS_ROOT_DIR ${CMAKE_SOURCE_DIR}) enable_testing()
add_subdirectory(test) add_subdirectory(test)
ENDIF(BUILD_TESTS) ENDIF(BUILD_TESTS)
......
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,14 +17,6 @@ ...@@ -17,14 +17,6 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // 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 //LaserScanUtils includes
#include "laser_scan_utils/line_finder_iterative.h" #include "laser_scan_utils/line_finder_iterative.h"
......
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,17 +17,8 @@ ...@@ -17,17 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // 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_ #pragma once
#define CORNER_FALKO_2D_H_
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
...@@ -136,5 +127,3 @@ public: ...@@ -136,5 +127,3 @@ public:
}; };
} /* namespace laserscanutils */ } /* namespace laserscanutils */
#endif /* LANDMARK_POLYLINE_2d_H_ */
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef CORNER_FINDER_H_
#define CORNER_FINDER_H_
//laserscanutils //laserscanutils
#include "laser_scan_utils.h" #include "laser_scan_utils.h"
...@@ -120,4 +118,3 @@ class CornerFinder ...@@ -120,4 +118,3 @@ class CornerFinder
}; };
} //namespace } //namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef CORNER_FINDER_INSCRIBED_ANGLE_H_
#define CORNER_FINDER_INSCRIBED_ANGLE_H_
//laserscanutils //laserscanutils
#include "corner_finder.h" #include "corner_finder.h"
...@@ -104,4 +102,3 @@ class CornerFinderInscribedAngle : public CornerFinder ...@@ -104,4 +102,3 @@ class CornerFinderInscribedAngle : public CornerFinder
void print() const override; void print() const override;
}; };
}//namespace }//namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef CORNER_POINT_H_
#define CORNER_POINT_H_
//laserscanutils //laserscanutils
#include "laser_scan_utils.h" #include "laser_scan_utils.h"
...@@ -92,4 +90,3 @@ class CornerPoint ...@@ -92,4 +90,3 @@ class CornerPoint
void print() const; void print() const;
}; };
}//namespace }//namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,11 +17,8 @@ ...@@ -17,11 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef GRID2D_H
#define GRID2D_H
#pragma once
//laserscanutils //laserscanutils
#include "laser_scan_utils.h" #include "laser_scan_utils.h"
...@@ -280,5 +277,3 @@ class Grid2D ...@@ -280,5 +277,3 @@ class Grid2D
}; };
}//end of namespace }//end of namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef GRID_CLUSTER_H_
#define GRID_CLUSTER_H_
//laserscanutils //laserscanutils
#include "laser_scan_utils.h" #include "laser_scan_utils.h"
...@@ -62,4 +60,3 @@ class GridCluster : public PointSet ...@@ -62,4 +60,3 @@ class GridCluster : public PointSet
void print() const override; void print() const override;
}; };
}//namespace }//namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,180 +17,218 @@ ...@@ -17,180 +17,218 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef ICP_H_
#define ICP_H_
#include "laser_scan.h" #include "laser_scan.h"
#include <chrono> #include <chrono>
#include <random> #include <random>
#include <csm/csm_all.h> #include <csm/csm_all.h>
#undef max //undefine macro of csm that may interfere with std::max #undef max // undefine macro of csm that may interfere with std::max
#undef min //undefine macro of csm that may interfere with std::min #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
};
struct icpParams namespace laserscanutils
{ {
bool verbose; // prints debug messages
struct icpOutput
// Algorithm options --------------------------------------------------- {
bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false). bool valid; // If the result is valid (converged & mean_error < max_mean_error & points_ratio > min_points_ratio)
double max_angular_correction_deg; // Maximum angular displacement between scans (deg) bool converged; // If the algorithm found a solution
double max_linear_correction; // Maximum translation between scans (m) Eigen::Vector3s res_transf; // Transformation found
Eigen::Matrix3s res_covar; // Covariance of the transformation
/** Maximum distance for a correspondence to be valid */ int nvalid; // Number of valid correspondences in the match
double max_correspondence_dist; double error; // Total correspondence error
/** Use smart tricks for finding correspondences. Only influences speed; not convergence. */ double mean_error; // Average error (total error / nvalid)
bool use_corr_tricks; double points_ratio; // Ratio of valid correspondences of totall amount of points
/** Checks that find_correspondences_tricks give the right answer */ unsigned int attempts; // Number of ICP calls to obtain a valid result (<= params.attempts)
bool debug_verify_tricks; };
// Stopping criteria struct icpParams
int max_iterations; // maximum iterations {
double epsilon_xy; // distance change bool verbose; // prints debug messages
double epsilon_theta; // angle change
// Algorithm options ---------------------------------------------------
// Restart algorithm bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false).
bool restart; // Enable restarting double max_angular_correction_deg; // Maximum angular displacement between scans (deg)
double restart_threshold_mean_error; // Threshold for restarting double max_linear_correction; // Maximum translation between scans (m)
double restart_dt; // Displacement for restarting
double restart_dtheta; // Displacement for restarting /** Maximum distance for a correspondence to be valid */
double max_correspondence_dist;
// Discarding points or correspondences --------------------------------------------------- /** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
/** discard rays outside of this interval */ bool use_corr_tricks;
double min_reading, max_reading; /** Checks that find_correspondences_tricks give the right answer */
/** Percentage of correspondences to consider: if 0.9, bool debug_verify_tricks;
always discard the top 10% of correspondences with more error */
double outliers_maxPerc; // Stopping criteria
int max_iterations; // maximum iterations
/** Parameters describing a simple adaptive algorithm for discarding. double epsilon_xy; // distance change
1) Order the errors. double epsilon_theta; // angle change
2) Choose the percentile according to outliers_adaptive_order.
(if it is 0.7, get the 70% percentile) // Restart algorithm
3) Define an adaptive threshold multiplying outliers_adaptive_mult bool restart; // Enable restarting
with the value of the error at the chosen percentile. double restart_threshold_mean_error; // Threshold for restarting
4) Discard correspondences over the threshold. double restart_dt; // Displacement for restarting
double restart_dtheta; // Displacement for restarting
This is useful to be conservative; yet remove the biggest errors.
*/ // Discarding points or correspondences ---------------------------------------------------
double outliers_adaptive_order; // 0.7 /** discard rays outside of this interval */
double outliers_adaptive_mult; // 2 double min_reading, max_reading;
/** Percentage of correspondences to consider: if 0.9,
/** Do not allow two different correspondences to share a point */ always discard the top 10% of correspondences with more error */
bool outliers_remove_doubles; double outliers_maxPerc;
/** If initial guess, visibility test can be done to discard points that are not visible */ /** Parameters describing a simple adaptive algorithm for discarding.
bool do_visibility_test; 1) Order the errors.
2) Choose the percentile according to outliers_adaptive_order.
/** Discard correspondences based on the angles */ (if it is 0.7, get the 70% percentile)
bool do_alpha_test; 3) Define an adaptive threshold multiplying outliers_adaptive_mult
double do_alpha_test_thresholdDeg; with the value of the error at the chosen percentile.
4) Discard correspondences over the threshold.
// Point orientation ------------------------------------------------------------------
/** For now, a very simple max-distance clustering algorithm is used */ This is useful to be conservative; yet remove the biggest errors.
double clustering_threshold; */
/** Number of neighbour rays used to estimate the orientation.*/ double outliers_adaptive_order; // 0.7
int orientation_neighbourhood; double outliers_adaptive_mult; // 2
// Weights --------------------------------------------------------------------------- /** Do not allow two different correspondences to share a point */
/** If the field "true_alpha" is used to compute the incidence bool outliers_remove_doubles;
beta, and the factor (1/cos^2(beta)) used to weight the impact
of each correspondence. This works fabolously if doing localization, /** If initial guess, visibility test can be done to discard points that are not visible */
that is the first scan has no noise. bool do_visibility_test;
If "true_alpha" is not available, it uses "alpha".
*/ /** Discard correspondences based on the angles */
bool use_ml_weights; bool do_alpha_test;
/* If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */ double do_alpha_test_thresholdDeg;
bool use_sigma_weights;
/** Noise in the scan */ // Point orientation ------------------------------------------------------------------
double sigma; /** For now, a very simple max-distance clustering algorithm is used */
double clustering_threshold;
// Covariance ------------------------------------------------------------------------ /** Number of neighbour rays used to estimate the orientation.*/
bool do_compute_covariance; // Compute the matching covariance (method in http://purl.org/censi/2006/icpcov) int orientation_neighbourhood;
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 // Weights ---------------------------------------------------------------------------
/** If the field "true_alpha" is used to compute the incidence
void print() const 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: public:
ICP(); ICP();
~ICP(); ~ICP();
static icpOutput align(const LaserScan &_current_ls, static icpOutput align(const LaserScan &_current_ls,
const LaserScan &_ref_ls, const LaserScan &_ref_ls,
const LaserScanParams &_current_scan_params, const LaserScanParams &_current_scan_params,
const LaserScanParams &_ref_scan_params, const LaserScanParams &_ref_scan_params,
const icpParams &_icp_params, const icpParams &_icp_params,
const Eigen::Vector3s &_initial_guess); const Eigen::Vector3s &_initial_guess);
static icpOutput align(const LaserScan &_last_ls, static icpOutput align(const LaserScan &_last_ls,
const LaserScan &_reference_ls, const LaserScan &_reference_ls,
const LaserScanParams &scan_params, const LaserScanParams &scan_params,
const icpParams &icp_params, const icpParams &icp_params,
const Eigen::Vector3s &_initial_guess); const Eigen::Vector3s &_initial_guess);
static void printTwoLaserData(sm_params & params); static void printTwoLaserData(sm_params &params);
static void printLaserData(LDP & laser_data); static void printLaserData(LDP &laser_data);
}; };
} }
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef LASER_SCAN_H_
#define LASER_SCAN_H_
//laserscanutils //laserscanutils
#include "laser_scan_utils.h" #include "laser_scan_utils.h"
...@@ -195,5 +193,3 @@ class LaserScan ...@@ -195,5 +193,3 @@ class LaserScan
}; };
} //namespace } //namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,11 +17,8 @@ ...@@ -17,11 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef TYPES_LASER_SCAN_UTILS_H_ #pragma once
#define TYPES_LASER_SCAN_UTILS_H_
//includes from Eigen lib //includes from Eigen lib
#include <Eigen/Dense> #include <Eigen/Dense>
...@@ -60,4 +57,3 @@ namespace Eigen ...@@ -60,4 +57,3 @@ namespace Eigen
typedef Quaternion<laserscanutils::ScalarT> Quaternions; ///< Quaternion of real scalar_t type typedef Quaternion<laserscanutils::ScalarT> Quaternions; ///< Quaternion of real scalar_t type
typedef AngleAxis<laserscanutils::ScalarT> AngleAxiss; ///< Angle-Axis of real scalar_t type typedef AngleAxis<laserscanutils::ScalarT> AngleAxiss; ///< Angle-Axis of real scalar_t type
} }
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef LASER_SCAN_WITH_PARAMS_H_
#define LASER_SCAN_WITH_PARAMS_H_
//laserscanutils //laserscanutils
#include "laser_scan.h" #include "laser_scan.h"
...@@ -58,5 +56,3 @@ class LaserScanWithParams : public LaserScan ...@@ -58,5 +56,3 @@ class LaserScanWithParams : public LaserScan
}; };
} //namespace } //namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef LINE_FINDER_H_
#define LINE_FINDER_H_
//laserscanutils //laserscanutils
#include "laser_scan_utils.h" #include "laser_scan_utils.h"
...@@ -120,4 +118,3 @@ class LineFinder ...@@ -120,4 +118,3 @@ class LineFinder
virtual void print() const; virtual void print() const;
}; };
}//namespace }//namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef LINE_FINDER_HOUGH_H_
#define LINE_FINDER_HOUGH_H_
//laserscanutils //laserscanutils
#include "line_finder.h" #include "line_finder.h"
...@@ -111,4 +109,3 @@ class LineFinderHough : public LineFinder ...@@ -111,4 +109,3 @@ class LineFinderHough : public LineFinder
void print() const override; void print() const override;
}; };
}//namespace }//namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef LINE_FINDER_ITERATIVE_H_
#define LINE_FINDER_ITERATIVE_H_
//laserscanutils //laserscanutils
#include "line_finder.h" #include "line_finder.h"
...@@ -195,4 +193,3 @@ class LineFinderIterative : public LineFinder ...@@ -195,4 +193,3 @@ class LineFinderIterative : public LineFinder
}; };
} //namespace } //namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef LINE_FINDER_JUMP_FIT_H_
#define LINE_FINDER_JUMP_FIT_H_
//laserscanutils //laserscanutils
#include "line_finder.h" #include "line_finder.h"
...@@ -106,4 +104,3 @@ class LineFinderJumpFit : public LineFinder ...@@ -106,4 +104,3 @@ class LineFinderJumpFit : public LineFinder
void print() const override; void print() const override;
}; };
}//namespace }//namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,10 +17,8 @@ ...@@ -17,10 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END-------- #pragma once
#ifndef LINE_SEGMENT_H_
#define LINE_SEGMENT_H_
//laserscanutils //laserscanutils
#include "laser_scan_utils.h" #include "laser_scan_utils.h"
...@@ -107,4 +105,3 @@ class LineSegment ...@@ -107,4 +105,3 @@ class LineSegment
void print() const; void print() const;
}; };
}//namespace }//namespace
#endif
//--------LICENSE_START-------- // LaserScanUtils - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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),
// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // Andreu Corominas Murtra (acorominas@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of laser_scan_utils // This file is part of gnss_utils
// laser_scan_utils is free software: you can redistribute it and/or modify // 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 // 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 // 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, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -17,17 +17,8 @@ ...@@ -17,17 +17,8 @@
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // 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_ #pragma once
#define LOOP_CLOSURE_BASE_2D_H_
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
...@@ -40,50 +31,50 @@ ...@@ -40,50 +31,50 @@
#include "match_loop_closure_scene.h" #include "match_loop_closure_scene.h"
#include "scene_base.h" #include "scene_base.h"
namespace laserscanutils { namespace laserscanutils
/** \brief A 2base class for loop closure using falko library
*/
class LoopClosureBase2d
{ {
private: class LoopClosureBase2d;
public: typedef std::shared_ptr<LoopClosureBase2d> LoopClosureBase2dPtr;
/** \brief Constructor
**/ /** \brief A 2base class for loop closure using falko library
LoopClosureBase2d(){}; */
class LoopClosureBase2d
{
public:
/** \brief Constructor
**/
LoopClosureBase2d(){};
/** \brief Destructor /** \brief Destructor
**/ **/
virtual ~LoopClosureBase2d(){}; virtual ~LoopClosureBase2d(){};
/** \brief update the scene struct with keypoints and descriptors /** \brief update the scene struct with keypoints and descriptors
**/ **/
virtual sceneBasePtr extractScene(const LaserScan &scan, const LaserScanParams &scanParams) = 0; virtual sceneBasePtr extractScene(const LaserScan &scan, const LaserScanParams &scanParams) = 0;
/** \brief Create and update a matchLoopClosure struct with the info that is /** \brief Create and update a matchLoopClosure struct with the info that is
*produced when matching two given scenes *produced when matching two given scenes
**/ **/
virtual MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene1, sceneBasePtr _scene2) = 0; 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 /** \brief It matches a target scene against a list of references scenes in order to find loop
* closures * closures
**/ **/
virtual std::map<double, MatchLoopClosureScenePtr> findLoopClosure(std::list<std::shared_ptr<SceneBase>> _l_scenes, virtual std::map<double, MatchLoopClosureScenePtr> findLoopClosure(std::list<std::shared_ptr<SceneBase>> _l_scenes,
const sceneBasePtr _new_scene) const sceneBasePtr _new_scene)
{ {
std::map<double, MatchLoopClosureScenePtr> matchings; std::map<double, MatchLoopClosureScenePtr> matchings;
for (auto ref_scene : _l_scenes) for (auto ref_scene : _l_scenes)
{ {
auto match = matchScene(ref_scene, _new_scene); auto match = matchScene(ref_scene, _new_scene);
while (matchings.find(match->score)!=matchings.end()) while (matchings.find(match->score) != matchings.end())
match->score+=0.001; match->score += 0.001;
matchings.emplace(match->score, match); matchings.emplace(match->score, match);
} }
return matchings; return matchings;
} }
}; };
} /* namespace laserscanutils */ } /* namespace laserscanutils */
#endif /* LOOP_CLOSURE_BASE_2D_H_ */