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)
Hide whitespace changes
Inline Side-by-side
Showing
with 394 additions and 430 deletions
.gitignore
View file @ bda3314a
......@@ -2,6 +2,8 @@
bin/
build*/
lib/
doc/
cmake_modules/
.settings/language.settings.xml
.project
.cproject
......
.gitlab-ci.yml
View file @ bda3314a
......@@ -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 ..
......
CMakeLists.txt
View file @ bda3314a
......@@ -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)
......
examples/polyline_demo.cpp
View file @ bda3314a
//--------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"
......
include/laser_scan_utils/corner_falko_2d.h
View file @ bda3314a
//--------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_ */
include/laser_scan_utils/corner_finder.h
View file @ bda3314a
//--------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
include/laser_scan_utils/corner_finder_inscribed_angle.h
View file @ bda3314a
//--------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
include/laser_scan_utils/corner_point.h
View file @ bda3314a
//--------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
include/laser_scan_utils/grid_2d.h
View file @ bda3314a
//--------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
include/laser_scan_utils/grid_cluster.h
View file @ bda3314a
//--------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
include/laser_scan_utils/icp.h
View file @ bda3314a
//--------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 &params);
static void printLaserData(LDP &laser_data);
};
}
#endif
include/laser_scan_utils/laser_scan.h
View file @ bda3314a
//--------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
include/laser_scan_utils/laser_scan_utils.h
View file @ bda3314a
//--------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
include/laser_scan_utils/laser_scan_with_params.h
View file @ bda3314a
//--------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
include/laser_scan_utils/line_finder.h
View file @ bda3314a
//--------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
include/laser_scan_utils/line_finder_hough.h
View file @ bda3314a
//--------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
include/laser_scan_utils/line_finder_iterative.h
View file @ bda3314a
//--------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
include/laser_scan_utils/line_finder_jump_fit.h
View file @ bda3314a
//--------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
include/laser_scan_utils/line_segment.h
View file @ bda3314a
//--------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
include/laser_scan_utils/loop_closure_base.h
View file @ bda3314a
//--------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_ */