//==============================================================================
// Copyright 2018-2020 Kitware, Inc., Kitware SAS
// Author: Guilbert Pierre (Kitware SAS)
//         Cadart Nicolas (Kitware SAS)
// Creation date: 2018-03-27
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//==============================================================================

// This slam algorithm is inspired by the LOAM algorithm:
// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.

// The algorithm is composed of three sequential steps:
//
// - Keypoints extraction: this step consists of extracting keypoints over
// the points clouds. To do that, the laser lines / scans are treated independently.
// The laser lines are projected onto the XY plane and are rescaled depending on
// their vertical angle. Then we compute their curvature and create two classes of
// keypoints. The edges keypoints which correspond to points with a high curvature
// and planar points which correspond to points with a low curvature.
//
// - Ego-Motion: this step consists of recovering the motion of the lidar
// sensor between two frames (two sweeps). The motion is modelized by a constant
// velocity and angular velocity between two frames (i.e null acceleration).
// Hence, we can parameterize the motion by a rotation and translation per sweep / frame
// and interpolate the transformation inside a frame using the timestamp of the points.
// Since the points clouds generated by a lidar are sparse we can't design a
// pairwise match between keypoints of two successive frames. Hence, we decided to use
// a closest-point matching between the keypoints of the current frame
// and the geometric features derived from the keypoints of the previous frame.
// The geometric features are lines or planes and are computed using the edges
// and planar keypoints of the previous frame. Once the matching is done, a keypoint
// of the current frame is matched with a plane / line (depending of the
// nature of the keypoint) from the previous frame. Then, we recover R and T by
// minimizing the function f(R, T) = sum(d(point, line)^2) + sum(d(point, plane)^2).
// Which can be writen f(R, T) = sum((R*X+T-P).t*A*(R*X+T-P)) where:
// - X is a keypoint of the current frame
// - P is a point of the corresponding line / plane
// - A = (n*n.t) with n being the normal of the plane
// - A = (I - n*n.t).t * (I - n*n.t) with n being a director vector of the line
// Since the function f(R, T) is a non-linear mean square error function
// we decided to use the Levenberg-Marquardt algorithm to recover its argmin.
//
// - Localization: This step consists of refining the motion recovered in the Ego-Motion
// step and to add the new frame in the environment map. Thanks to the ego-motion
// recovered at the previous step it is now possible to estimate the new position of
// the sensor in the map. We use this estimation as an initial point (R0, T0) and we
// perform an optimization again using the keypoints of the current frame and the matched
// keypoints of the map (and not only the previous frame this time!). Once the position in the
// map has been refined from the first estimation it is then possible to update the map by
// adding the keypoints of the current frame into the map.
//
// In the following programs, three 3D coordinates system are used :
// - LIDAR {L} : attached to the geometric center of the LiDAR sensor. The
//   coordinates of the received pointclouds are expressed in this system.
//   LIDAR is rigidly linked (static transform) to BASE.
// - BASE  {B} : attached to the origin of the moving body (e.g. vehicle). We
//   are generally interested in tracking an other point of the moving body than
//   the LiDAR's (for example, we prefer to track the GPS antenna pose).
// - WORLD {W} : The world coordinate system {W} coincides with BASE at the
//   initial position. The output trajectory describes BASE origin in WORLD.

#pragma once

#include "LidarSlam/Utilities.h"
#include "LidarSlam/LidarPoint.h"
#include "LidarSlam/Enums.h"
#include "LidarSlam/SpinningSensorKeypointExtractor.h"
#include "LidarSlam/KeypointsMatcher.h"
#include "LidarSlam/LocalOptimizer.h"
#include "LidarSlam/RollingGrid.h"
#include "LidarSlam/PointCloudStorage.h"
#include "LidarSlam/ExternalSensorManagers.h"
#include "LidarSlam/State.h"
#include "LidarSlam/ConfidenceEstimators.h"

#include <Eigen/Geometry>

#include <list>

#ifdef USE_G2O
#include "LidarSlam/PoseGraphOptimizer.h"
#endif  // USE_G2O

#ifdef USE_TEASERPP
#include <teaser/registration.h>
#include <pcl/features/fpfh_omp.h>
#endif

#define SetMacro(name,type) void Set##name (type _arg) { name = _arg; }
#define GetMacro(name,type) type Get##name () const { return name; }

// The following macros are used to set/get a parameter in a struct
// !Attention the name of variable needs to be ended with "Params"
// to avoid a collision with the existed variable
#define SetStructParamsMacro(name, param, type)                              \
void Set##name##param (type _arg) { name##Params.param = _arg; }
// Macro to get a parameter value in a struct
#define GetStructParamsMacro(name, param, type)                             \
type Get##name##param () const { return name##Params.param; }

// The following macros are used to get/set a parameters in a nested structure
// !Attention the name of variable needs to be ended with "Params"
// to avoid a collision with the existed variable
#define SetNestedStructParamsMacro(name, nestedName, param, type)            \
void Set##name##param (type _arg) { name##Params.nestedName.param = _arg; }
#define GetNestedStructParamsMacro(name, nestedName, param, type)            \
type Get##name##param () const { return name##Params.nestedName.param; }

// The following macros are used to get/set a parameters in a nested structure
// !Attention the name of variable needs to be ended with "Params"
// to avoid a collision with the existed variable
#define SetDeepNestedStructParamsMacro(name, nested1, nested2, param, type)                             \
void Set##name##param (type _arg) { name##Params.nested1.nested2.param = _arg; }
#define GetDeepNestedStructParamsMacro(name, nested1, nested2, param, type)                            \
type Get##name##param () const { return name##Params.nested1.nested2.param; }

namespace LidarSlam
{
// Parameters for pose estimation by ICP-LM optimization
// Useful for EgoMotion, Localization and LoopClosureRegistration
namespace Optimization
{
struct Parameters
{
  // Point-to-neighborhood matching parameters.
  // The goal will be to loop over all keypoints, and to build the corresponding
  // point-to-neighborhood residuals that will be optimized later.
  // For each source keypoint, the steps will be:
  // - To extract the N nearest neighbors from the target cloud.
  //   These neighbors should not be too far from the source keypoint.
  // - Assess the neighborhood shape by checking its PCA eigenvalues.
  // - Fit a line/plane/blob model on the neighborhood using PCA.
  // - Assess the model quality by checking its error relatively to the neighborhood.
  // - Build the corresponding point-to-model distance operator
  // If any of this step fails, the matching procedure of the current keypoint aborts.
  // See KeypointsMatcher::Parameters for more details on each parameter.
  KeypointsMatcher::Parameters MatchingParams;

  // ICP-LM optimization parameters
  // Number of outer ICP-optim loop iterations to perform.
  // Each iteration will consist of building ICP matches, then optimizing them.
  unsigned int ICPMaxIter = 3;

  // Maximum number of iterations of the Levenberg-Marquardt optimizer to solve
  // the ICP problem composed of the built point-to-neighborhood residuals
  unsigned int LMMaxIter = 15;

  // Maximum distance (in meters) beyond which the residual errors are
  // saturated to robustify the optimization against outlier constraints.
  // The residuals will be robustified by Tukey loss at scale sqrt(SatDist),
  // leading to ~90% of saturation at SatDist/2, fully saturated at SatDist.
  double InitSaturationDistance = 2.;
  double FinalSaturationDistance = 0.5;

  UndistortionMode Undistortion = UndistortionMode::NONE;
  bool EnableExternalConstraints = false;
};
} // end of Optimization namespace

// Parameters for recovery mode
namespace Recovery
{
struct Parameters
{
  LidarSlam::MappingMode MapUpdate         = LidarSlam::MappingMode::NONE;
  LidarSlam::EgoMotionMode EgoMotion       = LidarSlam::EgoMotionMode::NONE;
  LidarSlam::UndistortionMode Undistortion = LidarSlam::UndistortionMode::NONE;
  unsigned int ICPMaxIter                  = 20;
  double MaxNeighborsDistance              = 5;
  double InitSaturationDistance            = 4;
};
} // end of Recovery namespace

// Parameters for loop closure
namespace LoopClosure
{
struct Parameters
{
  // Parameters for the loop closure detection

  // Which method to use to detect loop closure
  // EXTERNAL detector: users need to indicate the query frame index and the revisited frame index for loop closure.
  // TEASERPP detector: automatic detection of loop closure by teaser registration
  LoopClosureDetector Detector = LoopClosureDetector::EXTERNAL;

  // When a query frame searches its revisited frame,
  // there is very little possibility that the loop is in the last travel distance.
  // The GapLength is the travel distance before the query frame where is considered no loop formed
  double GapLength = 10.; // 10 meters

  // For automatic detection, the candidate frames are sampled with a step of LoopSampleStep (in meters)
  // By default SampleStep is set to -1, the only candidate submap is the full map
  double SampleStep = -1.; // meter

  // The threshold to decide whether a candidate frame is the revisited frame of loop closure or not
  double EvaluationThreshold = 0.6;

  #ifdef USE_TEASERPP
  teaser::RobustRegistrationSolver::Params TeaserParams
  {
    // Other parameters are kept as default values. See more details in
    // https://github.com/MIT-SPARK/TEASER-plusplus/blob/master/teaser/include/teaser/registration.h
    .noise_bound = 0.05,              // Noise bound in meters
    .estimate_scaling = false,        // no scaling in the pointcloud
    .rotation_cost_threshold = 0.005 // The cost threshold compares with the difference between costs of consecutive iterations.
  };
  #endif

  // Parameters for the loop closure registration

  // The submaps size in meters around the query frame or the revisited frame for loop closure.
  // To build sub maps around query frame, use frames in the range [QueryIdx - 5m, QueryIdx + 5m].
  // To build sub maps around the revisited frame, use frames in the range [RevisitedIdx - 5m, RevisitedIdx + 5m].
  double QueryMapStartRange = -5;     // 5 meters before query frame
  double QueryMapEndRange = 5;        // 5 meters after query frame
  double RevisitedMapStartRange = -5; // 5 meters before revisited frame
  double RevisitedMapEndRange = 5;    // 5 meters after revisited frame

  // Boolean to add an offset to loop closure pose prior when the frames are too far from each other.
  bool EnableOffset = false;

  // Boolean to enable the registration between two sub maps instead of registering a single frame on a sub map.
  bool ICPWithSubmap = true;

  Optimization::Parameters OptParams
  {
    // KeypointsMatcher::Parameters MatchingParams
    {
      // unsigned int NbThreads, bool SingleEdgePerRing, double MaxNeighborsDistance,
      1, false, 5.,
      // unsigned int EdgeNbNeighbors, unsigned int EdgeMinNbNeighbors, double EdgeMaxModelError,
      10, 4, 0.2,
      // unsigned int PlaneNbNeighbors, double PlanarityThreshold, double PlaneMaxModelError
      5, 0.04, 0.2,
      // unsigned int BlobNbNeighbors, double SaturationDistance
      10, 1.
    },
    // unsigned int ICPMaxIter, unsigned int LMMaxIter, double InitSaturationDistance, double FinalSaturationDistance
    3, 15, 2, 0.5,
    // UndistortionMode Undistortion, bool enableExternalConstraints
    UndistortionMode::NONE, false
  };
};

struct LoopIndices
{
  // Frame indices to indicate where the loop closure is formed.
  unsigned int QueryIdx = 0;
  unsigned int RevisitedIdx = 0;
  // The detection time
  double Time = -1;
  LoopIndices(unsigned int queryIdx, unsigned int revisitedIdx, double time):
    QueryIdx(queryIdx), RevisitedIdx(revisitedIdx), Time(time) {};
};
} // end of LoopClosure namespace

class Slam
{
public:
  // Needed as Slam has fixed size Eigen vectors as members
  // http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // Usefull types
  using Point = LidarPoint;
  using PointCloud = pcl::PointCloud<Point>;
  using KeypointExtractorPtr = std::shared_ptr<SpinningSensorKeypointExtractor>;
  using PCStorage = PointCloudStorage<LidarPoint>;
  using Maps = std::map<Keypoint, std::shared_ptr<RollingGrid>>;

  // Initialization
  Slam();
  // Reset internal state : maps and trajectory are cleared,
  // current pose is set back to origin and the external sensor data are emptied.
  // This keeps parameters unchanged.
  void Reset(bool resetLog = true);

  // Init map with default values
  // This function is useful to keep a set of initial map parameters which are not SLAM members
  // Maps can only be removed when reset is called but they don't depend on UseKeypoint
  void InitMap(Keypoint k);

  // ---------------------------------------------------------------------------
  //   Main SLAM use
  // ---------------------------------------------------------------------------

  // Add a new frame to SLAM process.
  // This will trigger the following sequential steps:
  // - keypoints extraction: extract interesting keypoints to lower problem dimensionality
  // - ego-motion: estimate motion since last pose to init localization step
  // - localization: estimate global pose of current frame in map
  // - maps update: update maps using current registered frame
  void AddFrame(const PointCloud::Ptr& pc) { this->AddFrames({pc}); }

  // Add a set of frames to SLAM process.
  // This will trigger the following sequential steps:
  // - keypoints extraction: extract interesting keypoints from each frame to
  //   lower problem dimensionality, then aggregate them.
  // - ego-motion: estimate motion since last pose to init localization step
  // - localization: estimate global pose of current frame in map
  // - maps update: update maps using current registered frame
  // This first frame will be considered as 'main': its timestamp will be the
  // current pose time, its frame id will be used if no other is specified, ...
  void AddFrames(const std::vector<PointCloud::Ptr>& frames);

  // Get keypoints maps
  // If clean is true, the moving objects are removed from map
  PointCloud::Ptr GetMap(Keypoint k, bool clean = false) const;

  // Get target keypoints for current scan
  PointCloud::Ptr GetTargetSubMap(Keypoint k) const;

  // Get extracted and optionally undistorted keypoints from current frame.
  // If worldCoordinates=false, it returns keypoints in BASE coordinates,
  // If worldCoordinates=true, it returns keypoints in WORLD coordinates.
  // NOTE: The requested keypoints are lazy-transformed: if the requested WORLD
  // keypoints are not directly available in case they have not already been
  // internally transformed, this will be done on first call of this method.
  PointCloud::Ptr GetKeypoints(Keypoint k, bool worldCoordinates = false);

  // Get current registered (and optionally undistorted) input points.
  // All frames from all devices are aggregated.
  PointCloud::Ptr GetRegisteredFrame();

  // Get current number of frames already processed
  GetMacro(NbrFrameProcessed, unsigned int)

  // Get general information about ICP and optimization
  std::unordered_map<std::string, double> GetDebugInformation() const;
  // Get information for each keypoint of the current frame (used/rejected keypoints, ...)
  std::unordered_map<std::string, std::vector<double>> GetDebugArray() const;

  // Update LocalMaps from the beginning of the LogStates
  // By default, clear points in maps after the first timestamp in the LogStates
  // and replace them by the keypoints stored in the LogStates
  // If enable resetMaps, reset LocalMaps and rebuild it by the keypoints stored in the LogStates
  void UpdateMaps(bool resetMaps = false);

  // Optimize graph containing lidar states with
  // landmarks' constraints as a postprocess
  bool OptimizeGraph();

  // Use IMU measurements and optimized poses (using IMU + Lidar SLAM)
  // to update LogStates, maps and the current pose
  bool UpdateTrajectoryAndMapsWithIMU();

  // Get the pose of the last state in the LogStates by default
  // trackTime enables the time tracking in the external sensor and thus reduces
  // the computation time if this function is to be called on successive timestamps.
  Eigen::Isometry3d GetTworld(double time = -1., bool trackTime = false);

  // Set current pose and notify a discontinuity in the trajectory
  // For interpolations/extrapolations and IMU preintegration
  // WARNING : this function may break the map, a reset might be needed
  // before calling this function
  void SetTworld(const Eigen::Isometry3d& pose);

  // Change the reference frame of the Lidar trajectory and maps
  // odom <- odom * transform
  // This can allow to recenter the trajectory to origin and to limit
  // errors due to numbers precision
  // It can also be used to place the Lidar in a map initially
  void TransformOdom(const Eigen::Isometry3d& transform);

  // Make TworldInit the first logged pose
  // This is useful to keep a consistent map
  // even after the logged poses have been updated
  // and allow to keep odom frame consistent after PGO
  // WARNING : the maps are not updated with the new trajectory
  void ResetTrajWithTworldInit();

  // Move odom to reference frame of external pose measurements:
  // the whole trajectory is moved rigidly.
  // As the trajectory may have drifted, a time is required to choose
  // which poses to superimpose to derive the reference frames offset.
  // If no time is input, the last logged state is used.
  // If no Lidar frames are logged, the external pose
  // at time is used as initial SLAM pose
  bool MoveOdomToExtPosesRefFrame(double time = -1);

  // Save keypoints maps to disk for later use
  // Keypoints maps are rebuilt to recover removed points if the time threshold (DecayingThreshold) is set
  void SaveMapsToPCD(const std::string& filePrefix, PCDFormat pcdFormat = PCDFormat::BINARY_COMPRESSED, bool submap = true);

  // Load keypoints maps from disk (and reset SLAM maps)
  void LoadMapsFromPCD(const std::string& filePrefix, bool resetMaps = true);

  // Reset trajectory pose in LogStates
  void ResetStatePoses(ExternalSensors::PoseManager& newTrajectoryManager);

  // Notify the IMU and the ego-motion that
  // there has been a discontinuity in the trajectory
  void NotifyDiscontinuity();

  // ---------------------------------------------------------------------------
  //   General parameters
  // ---------------------------------------------------------------------------

  GetMacro(NbThreads, int)
  void SetNbThreads(int n);

  void SetVerbosity(int verbosity);
  GetMacro(Verbosity, int)

  SetMacro(EgoMotion, EgoMotionMode)
  GetMacro(EgoMotion, EgoMotionMode)

  void SetUndistortion(UndistortionMode undistMode);
  GetMacro(Undistortion, UndistortionMode)

  void SetInterpolation(Interpolation::Model model);
  GetMacro(Interpolation, Interpolation::Model);

  void SetLoggingTimeout(double lMax);
  GetMacro(LoggingTimeout, double)

  SetMacro(LoggingStorage, PointCloudStorageType)
  GetMacro(LoggingStorage, PointCloudStorageType)

  SetMacro(LogOnlyKeyframes, bool)
  GetMacro(LogOnlyKeyframes, bool)

  GetMacro(LogStates, const std::list<LidarState>&)
  // Get the last states since last input frame timestamp
  // at a specified frequency
  // This will use external sensor measurements and/or poses interpolation
  std::vector<LidarState> GetLastStates(double freq = -1);
  LidarState GetLastState() {return this->GetLastStates().front();};

  // ---------------------------------------------------------------------------
  //   Graph parameters
  // ---------------------------------------------------------------------------

  GetMacro(G2oFileName, std::string)
  SetMacro(G2oFileName, std::string)

  GetMacro(CovarianceScale, float)
  SetMacro(CovarianceScale, float)

  GetMacro(NbGraphIterations, int)
  SetMacro(NbGraphIterations, int)

  // Set the pose graph constraint type to use
  void EnablePGOConstraint(PGOConstraint constraint, bool enabled = true);
  // Get the pose graph constraint types used
  bool IsPGOConstraintEnabled(PGOConstraint constraint) const;

  // ---------------------------------------------------------------------------
  //   Coordinates systems parameters
  // ---------------------------------------------------------------------------

  SetMacro(BaseFrameId, std::string const&)
  GetMacro(BaseFrameId, std::string)

  SetMacro(WorldFrameId, std::string const&)
  GetMacro(WorldFrameId, std::string)

  // ---------------------------------------------------------------------------
  //   Keypoints extraction
  // ---------------------------------------------------------------------------

  // Get/Set all keypoints extractors
  std::map<std::string, KeypointExtractorPtr> GetKeyPointsExtractors() const;
  void SetKeyPointsExtractors(const std::map<std::string, KeypointExtractorPtr>& extractors);

  // Get/Set a specific keypoints extractor
  // NOTE: If no keypoint extractor exists for the requested deviceId, the returned pointer is null.
  KeypointExtractorPtr GetKeyPointsExtractor(const std::string& deviceId = "mainLidar" ) const;
  void SetKeyPointsExtractor(KeypointExtractorPtr extractor, const std::string& deviceId = "mainLidar");

  // Get/Set a specific base to Lidar offset
  // NOTE: If no base to lidar offset exists for the requested deviceId, the returned transform is identity.
  Eigen::Isometry3d GetBaseToLidarOffset(const std::string& deviceId = "mainLidar") const;
  void SetBaseToLidarOffset(const Eigen::Isometry3d& transform, const std::string& deviceId = "mainLidar");

  // Set the keypoint types to use
  void EnableKeypointType(Keypoint k, bool enabled = true);
  // Get the keypoint types used
  bool KeypointTypeEnabled(Keypoint k) const;

  // ---------------------------------------------------------------------------
  //   Optimization parameters
  // ---------------------------------------------------------------------------

  GetMacro(TwoDMode, bool)
  SetMacro(TwoDMode, bool)

  // Get/Set EgoMotion
  GetStructParamsMacro(EgoMotion, LMMaxIter, unsigned int)
  SetStructParamsMacro(EgoMotion, LMMaxIter, unsigned int)

  GetStructParamsMacro(EgoMotion, ICPMaxIter, unsigned int)
  SetStructParamsMacro(EgoMotion, ICPMaxIter, unsigned int)

  GetNestedStructParamsMacro(EgoMotion, MatchingParams, MaxNeighborsDistance, double)
  SetNestedStructParamsMacro(EgoMotion, MatchingParams, MaxNeighborsDistance, double)

  GetNestedStructParamsMacro(EgoMotion, MatchingParams, EdgeNbNeighbors, unsigned int)
  SetNestedStructParamsMacro(EgoMotion, MatchingParams, EdgeNbNeighbors, unsigned int)

  GetNestedStructParamsMacro(EgoMotion, MatchingParams, EdgeMinNbNeighbors, unsigned int)
  SetNestedStructParamsMacro(EgoMotion, MatchingParams, EdgeMinNbNeighbors, unsigned int)

  GetNestedStructParamsMacro(EgoMotion, MatchingParams, PlaneNbNeighbors, unsigned int)
  SetNestedStructParamsMacro(EgoMotion, MatchingParams, PlaneNbNeighbors, unsigned int)

  GetNestedStructParamsMacro(EgoMotion, MatchingParams, PlanarityThreshold, double)
  SetNestedStructParamsMacro(EgoMotion, MatchingParams, PlanarityThreshold, double)

  GetNestedStructParamsMacro(EgoMotion, MatchingParams, EdgeMaxModelError, double)
  SetNestedStructParamsMacro(EgoMotion, MatchingParams, EdgeMaxModelError, double)

  GetNestedStructParamsMacro(EgoMotion, MatchingParams, PlaneMaxModelError, double)
  SetNestedStructParamsMacro(EgoMotion, MatchingParams, PlaneMaxModelError, double)

  GetStructParamsMacro(EgoMotion, InitSaturationDistance, double)
  SetStructParamsMacro(EgoMotion, InitSaturationDistance, double)

  GetStructParamsMacro(EgoMotion, FinalSaturationDistance, double)
  SetStructParamsMacro(EgoMotion, FinalSaturationDistance, double)

  // Get/Set Localization
  GetStructParamsMacro(Localization, LMMaxIter, unsigned int)
  SetStructParamsMacro(Localization, LMMaxIter, unsigned int)

  GetStructParamsMacro(Localization, ICPMaxIter, unsigned int)
  SetStructParamsMacro(Localization, ICPMaxIter, unsigned int)

  GetNestedStructParamsMacro(Localization, MatchingParams, MaxNeighborsDistance, double)
  SetNestedStructParamsMacro(Localization, MatchingParams, MaxNeighborsDistance, double)

  GetNestedStructParamsMacro(Localization, MatchingParams, EdgeNbNeighbors, unsigned int)
  SetNestedStructParamsMacro(Localization, MatchingParams, EdgeNbNeighbors, unsigned int)

  GetNestedStructParamsMacro(Localization, MatchingParams, EdgeMinNbNeighbors, unsigned int)
  SetNestedStructParamsMacro(Localization, MatchingParams, EdgeMinNbNeighbors, unsigned int)

  GetNestedStructParamsMacro(Localization, MatchingParams, PlaneNbNeighbors, unsigned int)
  SetNestedStructParamsMacro(Localization, MatchingParams, PlaneNbNeighbors, unsigned int)

  GetNestedStructParamsMacro(Localization, MatchingParams, PlanarityThreshold, double)
  SetNestedStructParamsMacro(Localization, MatchingParams, PlanarityThreshold, double)

  GetNestedStructParamsMacro(Localization, MatchingParams, EdgeMaxModelError, double)
  SetNestedStructParamsMacro(Localization, MatchingParams, EdgeMaxModelError, double)

  GetNestedStructParamsMacro(Localization, MatchingParams, PlaneMaxModelError, double)
  SetNestedStructParamsMacro(Localization, MatchingParams, PlaneMaxModelError, double)

  GetNestedStructParamsMacro(Localization, MatchingParams, BlobNbNeighbors, unsigned int)
  SetNestedStructParamsMacro(Localization, MatchingParams, BlobNbNeighbors, unsigned int)

  GetStructParamsMacro(Localization, InitSaturationDistance, double)
  SetStructParamsMacro(Localization, InitSaturationDistance, double)

  GetStructParamsMacro(Localization, FinalSaturationDistance, double)
  SetStructParamsMacro(Localization, FinalSaturationDistance, double)

  // External Sensor parameters

  // General
  GetMacro(SensorTimeOffset, double)
  void SetSensorTimeOffset(double timeOffset);

  GetMacro(SensorTimeThreshold, double)
  void SetSensorTimeThreshold(double timeOffset);

  GetMacro(SensorMaxMeasures, unsigned int)
  void SetSensorMaxMeasures(unsigned int max);

  void ResetSensors(bool emptyMeasurements = false);
  void ResetSensor(bool emptyMeasurements, ExternalSensor sensor);

  // Check if there are external sensor data
  // available for local optimization
  bool IsExtSensorForLocalOpt();

  // Odometer
  double GetWheelOdomWeight() const;
  void SetWheelOdomWeight(double weight);

  float GetWheelOdomSaturationDistance() const;
  void SetWheelOdomSaturationDistance(float dist);

  SetMacro(WheelOdomRelative, bool)
  GetMacro(WheelOdomRelative, bool)

  void SetWheelOdomCalibration(const Eigen::Isometry3d& calib);
  Eigen::Isometry3d GetWheelOdomCalibration() const;

  void SetWheelOdomReference(const Eigen::Vector3d& ref);
  Eigen::Vector3d GetWheelOdomReference() const;

  void AddWheelOdomMeasurement(const ExternalSensors::WheelOdomMeasurement& om);

  bool WheelOdomHasData() const {return this->WheelOdomManager && this->WheelOdomManager->HasData();}

  bool WheelOdomCanBeUsedLocally() const;

  // Gravity from IMU
  double GetGravityWeight() const;
  void SetGravityWeight(double weight);

  void AddGravityMeasurement(const ExternalSensors::GravityMeasurement& gm);

  bool GravityHasData() const {return this->GravityManager && this->GravityManager->HasData();}

  double GetImuWeight() const;
  void SetImuWeight(double weight);

  void SetImuCalibration(const Eigen::Isometry3d& calib);

  Eigen::Vector3d GetImuGravity() const;
  void SetImuGravity(const Eigen::Vector3d& gravity);

  float GetImuFrequency() const;
  void SetImuFrequency(float freq);

  unsigned int GetImuResetThreshold() const;
  void SetImuResetThreshold(unsigned int);

  GetMacro(ImuUpdate, bool)
  SetMacro(ImuUpdate, bool)

  void AddImuMeasurement(const ExternalSensors::ImuMeasurement& gm);

  bool ImuHasData() const {return this->ImuManager && this->ImuManager->HasData();}

  // Landmark detector
  GetMacro(LandmarkWeight, double)
  void SetLandmarkWeight(double weight);

  GetMacro(LandmarkSaturationDistance, float)
  void SetLandmarkSaturationDistance(float dist);

  GetMacro(LandmarkPositionOnly, bool)
  void SetLandmarkPositionOnly(bool positionOnly);

  GetMacro(LandmarkCovarianceRotation, bool)
  void SetLandmarkCovarianceRotation(bool rotate);

  GetMacro(LandmarkConstraintLocal, bool)
  SetMacro(LandmarkConstraintLocal, bool)

  void SetLmDetectorCalibration(const Eigen::Isometry3d& calib);

  void AddLandmarkManager(int id, const Eigen::Vector6d& absolutePose, const Eigen::Matrix6d& absolutePoseCovariance);

  void AddLandmarkMeasurement(const ExternalSensors::LandmarkMeasurement& lm, int id);

  bool LmCanBeUsedLocally() const;
  bool LmHasData() const;

  // GPS
  void AddGpsMeasurement(const ExternalSensors::GpsMeasurement& gpsMeas);

  Eigen::Isometry3d GetGpsCalibration();
  void SetGpsCalibration(const Eigen::Isometry3d& calib);

  Eigen::Isometry3d GetGpsOffset();

  bool GpsHasData() const {return this->GpsManager && this->GpsManager->HasData();}

  // Pose
  double GetPoseWeight() const;
  void SetPoseWeight(double weight);

  void AddPoseMeasurement(const ExternalSensors::PoseMeasurement& pm);
  // Check if pose manager has been filled with poses
  // and if it has been filled by the IMU manager, check that it has been updated
  // at least twice to trust the measurements
  // This is mostly done because if the frame timestamp references to the frame last point
  // The update before this timestamp won't have been done.
  // Moreover, the bias may be still wrong after the first graph optimization.
  bool PoseHasData() const {return this->PoseManager && this->PoseManager->HasData() &&
                                   (!this->ImuHasData() || this->ImuHasBeenUpdated > 2);}

  // Find the calibration offset between the base frame and the frame tracked by the external poses
  // The two trajectories can be represented in different global frames
  bool CalibrateWithExtPoses(bool reset = false, bool planarTrajectory = false);

  Eigen::Isometry3d GetPoseCalibration() const;
  void SetPoseCalibration(const Eigen::Isometry3d& calib);

  // Camera
  void AddCameraImage(const ExternalSensors::Image& image);

  void SetCameraCalibration(const Eigen::Isometry3d& calib);

  void SetCameraIntrinsicCalibration(const Eigen::Matrix3f& k);

  bool CameraHasData() {return this->CameraManager && this->CameraManager->HasData();}
  bool CameraCanBeUsedLocally() {return this->CameraManager && this->CameraManager->CanBeUsedLocally();}

  double GetCameraWeight() const;
  void SetCameraWeight(double weight);

  float GetCameraSaturationDistance() const;
  void SetCameraSaturationDistance(float dist);

  // ---------------------------------------------------------------------------
  //   Key frames and Maps parameters
  // ---------------------------------------------------------------------------

  GetMacro(KfDistanceThreshold, double)
  SetMacro(KfDistanceThreshold, double)

  GetMacro(KfAngleThreshold, double)
  SetMacro(KfAngleThreshold, double)

  GetMacro(MapUpdate, MappingMode)
  SetMacro(MapUpdate, MappingMode)

  GetMacro(SubmapMode, PreSearchMode)
  SetMacro(SubmapMode, PreSearchMode)

  double GetVoxelGridDecayingThreshold() const;
  void SetVoxelGridDecayingThreshold(double decay);

  SamplingMode GetVoxelGridSamplingMode(Keypoint k) const;
  void SetVoxelGridSamplingMode(Keypoint k, SamplingMode sm);

  // Set RollingGrid Parameters
  void ClearMaps(Maps& maps);
  void ClearLocalMaps() {this->ClearMaps(this->LocalMaps);}
  void ClearLog();
  double GetVoxelGridLeafSize(Keypoint k) const;
  void SetVoxelGridLeafSize(Keypoint k, double size);
  void SetVoxelGridSize(int size);
  void SetVoxelGridResolution(double resolution);
  void SetVoxelGridMinFramesPerVoxel(unsigned int minFrames);

  // ---------------------------------------------------------------------------
  //   Loop Closure parameters
  // ---------------------------------------------------------------------------

  // Detect a loop for the current frame
  bool DetectLoopClosureIndices(LoopClosure::LoopIndices& loop);

  // Add indices of a loop into vector LoopDetections
  void AddLoopClosureIndices(LoopClosure::LoopIndices& loop);

  // Reset LoopDetections vector
  void ClearLoopDetections();

  GetStructParamsMacro(Loop, Detector, LoopClosureDetector)
  SetStructParamsMacro(Loop, Detector, LoopClosureDetector)

  GetStructParamsMacro(Loop, QueryMapStartRange, double)
  SetStructParamsMacro(Loop, QueryMapStartRange, double)

  GetStructParamsMacro(Loop, QueryMapEndRange, double)
  SetStructParamsMacro(Loop, QueryMapEndRange, double)

  GetStructParamsMacro(Loop, RevisitedMapStartRange, double)
  SetStructParamsMacro(Loop, RevisitedMapStartRange, double)

  GetStructParamsMacro(Loop, RevisitedMapEndRange, double)
  SetStructParamsMacro(Loop, RevisitedMapEndRange, double)

  // Parameters for automatic detection
  GetStructParamsMacro(Loop, GapLength, double)
  SetStructParamsMacro(Loop, GapLength, double)

  GetStructParamsMacro(Loop, SampleStep, double)
  SetStructParamsMacro(Loop, SampleStep, double)

  GetStructParamsMacro(Loop, EvaluationThreshold, double)
  SetStructParamsMacro(Loop, EvaluationThreshold, double)

  // Get/Set Loop Closure registration parameters
  GetStructParamsMacro(Loop, EnableOffset, bool)
  SetStructParamsMacro(Loop, EnableOffset, bool)

  GetStructParamsMacro(Loop, ICPWithSubmap, bool)
  SetStructParamsMacro(Loop, ICPWithSubmap, bool)

  GetNestedStructParamsMacro(Loop, OptParams, LMMaxIter, unsigned int)
  SetNestedStructParamsMacro(Loop, OptParams, LMMaxIter, unsigned int)

  GetNestedStructParamsMacro(Loop, OptParams, ICPMaxIter, unsigned int)
  SetNestedStructParamsMacro(Loop, OptParams, ICPMaxIter, unsigned int)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, MaxNeighborsDistance, double)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, MaxNeighborsDistance, double)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, EdgeNbNeighbors, unsigned int)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, EdgeNbNeighbors, unsigned int)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, EdgeMinNbNeighbors, unsigned int)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, EdgeMinNbNeighbors, unsigned int)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, PlaneNbNeighbors, unsigned int)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, PlaneNbNeighbors, unsigned int)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, PlanarityThreshold, double)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, PlanarityThreshold, double)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, EdgeMaxModelError, double)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, EdgeMaxModelError, double)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, PlaneMaxModelError, double)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, PlaneMaxModelError, double)

  GetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, BlobNbNeighbors, unsigned int)
  SetDeepNestedStructParamsMacro(Loop, OptParams, MatchingParams, BlobNbNeighbors, unsigned int)

  GetNestedStructParamsMacro(Loop, OptParams, InitSaturationDistance, double)
  SetNestedStructParamsMacro(Loop, OptParams, InitSaturationDistance, double)

  GetNestedStructParamsMacro(Loop, OptParams, FinalSaturationDistance, double)
  SetNestedStructParamsMacro(Loop, OptParams, FinalSaturationDistance, double)

  // ---------------------------------------------------------------------------
  //   Confidence estimation
  // ---------------------------------------------------------------------------

  // Matches
  GetMacro(TotalMatchedKeypoints, int)

  // Motion constraints
  Eigen::Array2f GetPoseLimits() const {return this->MotionCheck.GetAccelerationLimits();}
  void SetPoseLimits(Eigen::Array2f& lim) {this->MotionCheck.SetPoseLimits(lim);}

  Eigen::Array2f GetVelocityLimits() const {return this->MotionCheck.GetVelocityLimits();}
  void SetVelocityLimits(Eigen::Array2f& lim) {this->MotionCheck.SetVelocityLimits(lim);}

  Eigen::Array2f GetAccelerationLimits() const {return this->MotionCheck.GetAccelerationLimits();}
  void SetAccelerationLimits(Eigen::Array2f& lim) {this->MotionCheck.SetAccelerationLimits(lim);}

  GetMacro(ComplyMotionLimits, bool)

  unsigned int GetConfidenceWindow() const {return this->FailDetect.GetWindowSize();}
  void SetConfidenceWindow(int window) {this->MotionCheck.SetWindowSize(window);
                                        this->FailDetect.SetWindowSize(window);}

  // Overlap
  GetMacro(OverlapSamplingRatio, float)
  void SetOverlapSamplingRatio(float _arg);

  GetMacro(OverlapEstimation, float)

  float GetOverlapDerivativeThreshold() const {return this->FailDetect.GetOverlapDerivativeThreshold();}
  void SetOverlapDerivativeThreshold(float thresh) {this->FailDetect.SetOverlapDerivativeThreshold(thresh);}

  // Position error
  float GetPositionErrorThreshold() const {return this->FailDetect.GetPositionErrorThreshold();}
  void SetPositionErrorThreshold(float thresh) {this->FailDetect.SetPositionErrorThreshold(thresh);}

  float GetPositionErrorStd() const;

  // Fuse confidence estimators to
  // check if SLAM has failed
  bool HasFailed() {return this->FailDetect.HasFailed();}

  // In case of failure, the maps and trajectory
  // are restored to an old state (duration seconds away)
  // and the parameters are changed to be more robust to bigger motion
  void StartRecovery(double duration = 5.);

  // When the current frame is relocalized
  // The parameters are restored to go on with the mapping process
  void EndRecovery();

  // Check if the SLAM is in recovery mode
  bool IsRecovery() const {return this->RecoveryStorage != nullptr;}

  GetMacro(FailureDetectionEnabled, bool)
  SetMacro(FailureDetectionEnabled, bool)

private:

  // ---------------------------------------------------------------------------
  //   General stuff and flags
  // ---------------------------------------------------------------------------

  // Max number of threads to use for parallel processing
  int NbThreads = 1;

  // Booleans to decide whether to extract the keypoints of the relative type or not
  std::map<Keypoint, bool> UseKeypoints = {{EDGE, true}, {INTENSITY_EDGE, true}, {PLANE, true}, {BLOB, false}};
  std::vector<Keypoint> UsableKeypoints = {EDGE, INTENSITY_EDGE, PLANE};

  // How to estimate Ego-Motion (approximate relative motion since last frame).
  // The ego-motion step aims to give a fast and approximate initialization of
  // new frame world pose to ensure faster and more precise convergence in
  // Localization step.
  EgoMotionMode EgoMotion = EgoMotionMode::MOTION_EXTRAPOLATION;

  // How to correct the rolling shutter distortion during frame acquisition.
  // The undistortion should greatly improve the accuracy for smooth motions,
  // but might be unstable for high-frequency motions.
  UndistortionMode Undistortion = UndistortionMode::REFINED;

  // Indicate verbosity level to display more or less information :
  // 0: print errors, warnings or one time info
  // 1: 0 + frame number, total frame processing time
  // 2: 1 + extracted features, used keypoints, localization variance, ego-motion and localization summary
  // 3: 2 + sub-problems processing duration
  // 4: 3 + ceres optimization summary
  // 5: 4 + logging/maps memory usage
  int Verbosity = 0;

  // Maximum duration on which to keep states in memory in seconds
  // This duration must be increased if a pose graph optimization is planned
  // The minimum number of logged states is 2, to be able to handle ego-motion and undistortion,
  // independently of this timeout value
  double LoggingTimeout = 0;

  // Wether to use octree compression during keypoints logging.
  // This reduces about 5 times the memory consumption, but slows down logging (and PGO).
  PointCloudStorageType LoggingStorage = PointCloudStorageType::PCL_CLOUD;

  bool LogOnlyKeyframes = true;

  // Number of frames that have been processed by SLAM (number of poses in trajectory)
  unsigned int NbrFrameProcessed = 0;

  // Timestamp of the current input frame
  double CurrentTime = 0.;
  // ---------------------------------------------------------------------------
  //   Trajectory, transforms and undistortion
  // ---------------------------------------------------------------------------

  // **** COORDINATES SYSTEMS ****

  // Coordinates systems (CS) names to fill in pointclouds or poses headers
  std::string WorldFrameId = "odom";  // CS of trajectory and maps
  std::string BaseFrameId = "base_link";    // CS of current keypoints

  // **** LOCALIZATION ****

  // Global transformation to map the current pointcloud to the previous one
  Eigen::Isometry3d Trelative = Eigen::Isometry3d::Identity();

  // Transformation to map the current pointcloud in the world coordinates
  // This pose is the pose of BASE in WORLD coordinates, at the time
  // corresponding to the timestamp in the header of input Lidar scan.
  Eigen::Isometry3d Tworld = Eigen::Isometry3d::Identity();
  // Variable to store the oldest logged pose
  // before logging, it is set to the first pose that is found
  // (set from outside or set using the external poses)
  // It is used to reset the pose in case of failure
  // or to keep the odom frame link in case of PGO
  Eigen::Isometry3d TworldInit = Eigen::Isometry3d::Identity();

  // Reflect the success of the optimization for current input frames
  bool OptimizationValid = true;
  // Store the keyframe information
  bool IsKeyFrame = true;

  // Model used to interpolate between poses
  // This model can be linear, quadratic or cubic
  // The interpolator will use a different
  // number of data to perform the interpolation depending on the model type
  // Poses interpolation is used during ego-motion, undistortion
  // and external sensors synchronization
  Interpolation::Model Interpolation = Interpolation::LINEAR;

  // **** LOGGING ****

  // Log info on each pose
  // It contains :
  //     -The estimated Lidar isometry and its covariance
  //     -The time associated with the pose
  //     -The relative index in the pose graph
  //     -A boolean to store the keyframe info
  //     -The undistorted keypoints (expressed in BASE)
  // The oldest states are forgotten (cf. LoggingTimeout parameter)
  std::list<LidarState> LogStates;

  // Store last timestamps of recovery
  // to handle the logged states correctly
  std::vector<double> RecoveryTimes;

  // ---------------------------------------------------------------------------
  //   Keypoints extraction
  // ---------------------------------------------------------------------------

  // Sequence id of the previous processed frame, used to check frames dropping
  std::map<int, unsigned int> PreviousFramesSeq;

  // Keypoints extractors, 1 for each lidar device
  std::map<std::string, KeypointExtractorPtr> KeyPointsExtractors;

  // Static transform to link BASE and LIDAR coordinates systems for each device.
  // It corresponds to the pose of each LIDAR device origin in BASE coordinates.
  // If the transform is not available for a given device, identity will be used.
  std::map<std::string, Eigen::UnalignedIsometry3d> BaseToLidarOffsets = {{"frame", Eigen::UnalignedIsometry3d::Identity()}};

  // ---------------------------------------------------------------------------
  //   Keypoints from current frame
  // ---------------------------------------------------------------------------

  // Current frames (all raw input frames)
  std::vector<PointCloud::Ptr> CurrentFrames;

  // Current aggregated points from all input frames, in WORLD coordinates (with undistortion if enabled)
  PointCloud::Ptr RegisteredFrame;

  // Raw extracted keypoints, in BASE coordinates (no undistortion)
  std::map<Keypoint, PointCloud::Ptr> CurrentRawKeypoints;
  std::map<Keypoint, PointCloud::Ptr> PreviousRawKeypoints;

  // Extracted keypoints, in BASE coordinates (with undistortion if enabled)
  std::map<Keypoint, PointCloud::Ptr> CurrentUndistortedKeypoints;

  // Extracted keypoints, in WORLD coordinates (with undistortion if enabled)
  std::map<Keypoint, PointCloud::Ptr> CurrentWorldKeypoints;

  // ---------------------------------------------------------------------------
  //   Key frames and Maps
  // ---------------------------------------------------------------------------

  // Last keyframe pose
  Eigen::Isometry3d KfLastPose = Eigen::Isometry3d::Identity();

  // Min distance or angle to travel since last keyframe to add a new one
  double KfDistanceThreshold = 0.5;  ///< [m] Min distance to travel since last KF to add a new one
  double KfAngleThreshold = 5.;      ///< [°] Min angle to rotate since last KF to add a new one

  // Number of keyrames
  int KfCounter = 0;

  // How to update the map
  // The map can be updated more or less with new input keypoints
  // from current scanned points depending on the initial map reliability.
  MappingMode MapUpdate = MappingMode::UPDATE;

  // How to extract the submap
  // The submap can be extracted using a bounding box (extraction fast but kdtree heavy)
  // or using a profile of the current frame (extraction heavy but kdtree fast)
  // The use of one mode or the other depends on the environment and the needs
  PreSearchMode SubmapMode = PreSearchMode::BOUNDING_BOX;

  // How to downsample the points in the keypoints' maps
  // This mode parameter allows to choose how to select the remaining point in each voxel.
  // It can be taking the first/last acquired point, taking the max intensity point,
  // considering the closest point to the voxel center or averaging the points.
  SamplingMode DownSampling = SamplingMode::MAX_INTENSITY;

  // Keypoints local map
  Maps LocalMaps;

  // ---------------------------------------------------------------------------
  //   Loop closure
  // ---------------------------------------------------------------------------

  // Loop closure parameters for the detection and the registration
  LoopClosure::Parameters LoopParams;

  // Store the frame indices of detected loops
  std::vector<LoopClosure::LoopIndices> LoopDetections;

  // Transform between the query frame and the revisited frame that has been found by the automatic loop detector
  // This pose can be used as a pose prior in LoopClosureRegistration step
  Eigen::Isometry3d LoopDetectionTransform = Eigen::Isometry3d::Identity();

  // ---------------------------------------------------------------------------
  //   Optimization data
  // ---------------------------------------------------------------------------

  //! Matching results
  std::map<Keypoint, KeypointsMatcher::MatchingResults> EgoMotionMatchingResults;
  std::map<Keypoint, KeypointsMatcher::MatchingResults> LocalizationMatchingResults;

  // Optimization results
  // Variance-Covariance matrix that estimates the localization error about the
  // 6-DoF parameters (DoF order : X, Y, Z, rX, rY, rZ)
  LocalOptimizer::RegistrationError LocalizationUncertainty;

  // Odometry manager
  // It computes the residual with a weight, a measurements list and
  // taking account of the acquisition time correspondance
  // The odometry measurements must be filled from outside this lib
  // using External Sensors interface
  std::shared_ptr<ExternalSensors::WheelOdometryManager> WheelOdomManager;
  // Whether the wheel odometer constraint apply relatively to the first pose
  // or between successive poses
  bool WheelOdomRelative = false;

  // IMU managers
  // Gravity manager
  // It computes the gravity residual with a weight, a measurements list,
  // taking account of the acquisition time correspondance
  // The IMU measurements must be filled from outside this lib
  // using External Sensors interface
  std::shared_ptr<ExternalSensors::ImuGravityManager> GravityManager;
  // Raw IMU data manager
  // It performs the IMU data (accelerations + angle velocities) preintegration
  // using an external gravity vector, the SLAM output poses
  // taking into account the acquisition time correspondance.
  // This preintegration is used as an external pose (see the pose manager below)
  // The IMU measurements must be filled from outside this lib
  // using External Sensors interface
  std::shared_ptr<ExternalSensors::ImuManager> ImuManager;
  // Variable used internally to store the IMU update information
  // IMU should not be used before having been updated once with a SLAM state
  unsigned int ImuHasBeenUpdated = 0;

  // Landmarks manager
  // Each landmark has its own manager and is identified by its ID.
  // This map can be initialized from outside the lib if the absolute poses of the tags are known
  // If not, it will be filled at each new detection.
  // The managers compute the residuals with a weight, measurements lists and
  // taking account of the acquisition time correspondance
  // The tag measurements must be filled from outside this lib
  // using External Sensors interface
  std::map<int, ExternalSensors::LandmarkManager> LandmarksManagers;
  // Calibration
  Eigen::Isometry3d LmDetectorCalibration = Eigen::Isometry3d::Identity();
  // Variable to store the landmark weight to init correctly the landmark managers
  float LandmarkWeight = 0.f;
  // Boolean to choose whether to compute the reference pose of
  // the tag locally from observations when updating the model (true)
  // or to use the absolute tag pose supplied at init (false)
  bool LandmarkConstraintLocal = false;
  // Saturation distance beyond which the tags are not taken into account in the optimization
  float LandmarkSaturationDistance = 5.f;
  // Boolean to check whether to use the whole tag pose (position + orientation)
  // or only the position to create a constraint in the optimization
  bool LandmarkPositionOnly = true;
  // Boolean to decide whether to rotate the covariance
  // during measures interpolation or not. Covariances are only used
  // in pose graph optimization, not in local optimization
  bool LandmarkCovarianceRotation = true;

  // GPS manager
  // The GPS measurements must be filled from outside this lib
  // using External Sensors interface
  std::shared_ptr<ExternalSensors::GpsManager> GpsManager;
  // Calibration
  Eigen::Isometry3d GpsCalibration = Eigen::Isometry3d::Identity();

  // Camera Manager
  // Manager for RGB images
  // The Camera images must be filled and cleared from outside this lib
  // using External Sensors interface
  std::shared_ptr<ExternalSensors:: CameraManager> CameraManager;

  // Pose Manager
  // Manager for the acquisition of pose measurements (e.g. from GNNS system, pre-integrated
  // IMU or any other device able to give absolute pose.)
  // It computes a residual with a weight taking into account the timing at which it is captured
  // The Pose measurements must be filled from outside this lib
  // using External Sensors interface
  std::shared_ptr<ExternalSensors::PoseManager> PoseManager;

  // Weight for pose when integrating it to the SLAM optimization
  // This needs a specific variable storage to be able to switch between
  // IMU data and Poses data as they share the same pointer
  double PoseWeight = 0.;

  // Time difference between Lidar's measurements and external sensors'
  // not null if they are not expressed relatively to the same time reference
  double SensorTimeOffset = 0.;

  // Maximum time difference (s) between two measurements to
  // allow the measures interpolation and the integration
  // of an external sensor constraint in the optimization
  double SensorTimeThreshold = 1.;

  // Maximum number of sensor measurements stored
  // Above this number, the oldest measurements are forgotten
  unsigned int SensorMaxMeasures = 1e6;

  // To update the IMU bias or not depending on the accuracy
  bool ImuUpdate = true;

  // ---------------------------------------------------------------------------
  //   Optimization parameters
  // ---------------------------------------------------------------------------

  // Optimize only 2D pose in BASE coordinates.
  // This will only optimize X, Y (ground coordinates) and yaw (rZ).
  // This will hold Z (elevation), rX (roll) and rY (pitch) constant.
  bool TwoDMode = false;

  Optimization::Parameters EgoMotionParams
  {
    // KeypointsMatcher::Parameters MatchingParams
    {
      // unsigned int NbThreads, bool SingleEdgePerRing, double MaxNeighborsDistance,
      1, true, 5.,
      // unsigned int EdgeNbNeighbors, unsigned int EdgeMinNbNeighbors, double EdgeMaxModelError,
      8, 3, 0.2,
      // unsigned int PlaneNbNeighbors, double PlanarityThreshold, double PlaneMaxModelError
      5, 0.04, 0.2,
      // unsigned int BlobNbNeighbors, double SaturationDistance
      10, 1.
    },
    // unsigned int ICPMaxIter, unsigned int LMMaxIter, double InitSaturationDistance, double FinalSaturationDistance
    4, 15, 2, 0.5,
    // UndistortionMode Undistortion, bool enableExternalConstraints
    UndistortionMode::NONE, false
  };

  Optimization::Parameters LocalizationParams
  {
    // KeypointsMatcher::Parameters MatchingParams
    {
      // unsigned int NbThreads, bool SingleEdgePerRing, double MaxNeighborsDistance,
      1, false, 5.,
      // unsigned int EdgeNbNeighbors, unsigned int EdgeMinNbNeighbors, double EdgeMaxModelError,
      10, 4, 0.2,
      // unsigned int PlaneNbNeighbors, double PlanarityThreshold, double PlaneMaxModelError
      5, 0.04, 0.2,
      // unsigned int BlobNbNeighbors, double SaturationDistance
      10, 1.
    },
    // unsigned int ICPMaxIter, unsigned int LMMaxIter, double InitSaturationDistance, double FinalSaturationDistance
    3, 15, 2, 0.5,
    // UndistortionMode Undistortion, bool enableExternalConstraints
    this->Undistortion, true
  };

  // ---------------------------------------------------------------------------
  //   Graph parameters
  // ---------------------------------------------------------------------------
  // Log info from g2o, if empty, log is not stored
  std::string G2oFileName;

  // Scale to increase or decrease SLAM pose covariances
  float CovarianceScale = 1.f;
  int NbGraphIterations = 100;

  // Booleans to decide whether to use a pose graph constraint for the optimization
  std::map<PGOConstraint, bool> UsePGOConstraints = {{PGOConstraint::LOOP_CLOSURE, true},
                                                     {PGOConstraint::LANDMARK,     true},
                                                     {PGOConstraint::GPS,          true},
                                                     {PGOConstraint::EXT_POSE,     true}};

  // ---------------------------------------------------------------------------
  //   Confidence estimation
  // ---------------------------------------------------------------------------

  // Data

  // Overlap estimation of the current registered scan on the keypoints map
  // A valid value lies in range [0-1].
  // It is set to -1 if overlap has not been evaluated (disabled or not enough points).
  float OverlapEstimation = -1.f;

  // Number of matches for processed frame
  unsigned int TotalMatchedKeypoints = 0;

  // Helper to check the motion and compare it
  // to input limits
  Confidence::MotionChecker MotionCheck;
  // Check motion limitations compliance
  bool ComplyMotionLimits = true;

  // Failure detector
  Confidence::FailDetector FailDetect;

  // Store current parameters in case of recovery mode
  std::shared_ptr<Recovery::Parameters> RecoveryStorage;

  // Parameters

  // Extrapolating a pose farther from this time ratio is forbidden and will abort.
  // i.e, if using 2 frames with timestamps t1 and t2, extrapolating at t3 is
  // allowed only if abs((t3 - t2)/(t2 - t1)) < MaxExtrapolationRatio.
  // Otherwise, extrapolation will return the pose at t2.
  double MaxExtrapolationRatio = 3.;

  // Min number of matches to consider the optimization problem usable.
  // Below this threshold, we consider that there are not enough matches to
  // provide good enough optimization results, and registration is aborted.
  unsigned int MinNbMatchedKeypoints = 20;

  // [0-1] Ratio of points from the input cloud to compute overlap on.
  // Downsampling accelerates the overlap computation, but may be less precise.
  // If 0, overlap won't be computed.
  float OverlapSamplingRatio = 0.f;

  // Enable/Disable failure detection, if enabled,
  // and a failure is detected, the frame will be skipped
  // i.e. points are not added to the map and the trajectory is not updated
  bool FailureDetectionEnabled = false;

  // ---------------------------------------------------------------------------
  //   Main sub-problems and methods
  // ---------------------------------------------------------------------------

  // Check that input frames are correct
  // (empty frame, same timestamp, frame dropping, ...)
  bool CheckFrames(const std::vector<PointCloud::Ptr>& frames);

  // Extract keypoints from input pointclouds,
  // and transform them from LIDAR to BASE coordinate system.
  void ExtractKeypoints();

  // Compute constraints provided by external sensors
  void ComputeSensorConstraints();

  // Estimate the ego motion since last frame.
  // Extrapolate new pose with a constant velocity model and/or
  // refine estimation by registering current frame keypoints on previous frame keypoints.
  void ComputeEgoMotion();

  // Compute the pose of the current frame in world referential by registering
  // current frame keypoints on keypoints from maps
  void Localization();

  // Transform current keypoints to WORLD coordinates,
  // and add points to the maps if we are dealing with a new keyframe.
  void UpdateMapsUsingTworld();

  // Check if a new keyframe is needed or not
  // This is based on the motion that has been performed
  // and if a tag manager requires one
  bool NeedNewKeyFrame();

  // Log current frame processing results : pose, covariance and keypoints.
  void LogCurrentFrameState();

  // ---------------------------------------------------------------------------
  //   Loop Closure usage
  // ---------------------------------------------------------------------------

  // Get the iterator of LogStates for an input frame index
  // It is possible that the input frame index does not correspond to a keyframe
  // In this case, output the nearest neighbor keyframe iterator
  std::list<LidarState>::iterator GetKeyStateIterator(const unsigned int& frameIdx);

  // Return true if a loop closure has been found and update itRevisitedState iterator, if not return false.
  bool DetectLoopWithTeaser(std::list<LidarState>::iterator& itQueryState, std::list<LidarState>::iterator& itRevisitedState);

  #ifdef USE_TEASERPP
  // Compute FPFH features for a input pointcloud
  // The input arguments are: input cloud, search radius for estimating normals and
  // search radius for calculating FPFH (needs to be at least normalSearchRadius)
  pcl::PointCloud<pcl::FPFHSignature33>::Ptr ComputeFPFHFeatures(const PointCloud::Ptr inputCloud,
                                                                 double normalSearchRadius,
                                                                 double fpfhSearchRadius);

  // Compute correspondences between two pointclouds by searching nearest FPFH features in kdtree
  std::vector<std::pair<int, int>> CalculateCorrespondences(pcl::PointCloud<pcl::FPFHSignature33>::Ptr sourceFeatures,
                                                          pcl::PointCloud<pcl::FPFHSignature33>::Ptr targetFeatures);
  #endif

  // Compute the transform between a query frame and the revisited frame
  // by registering query frame keypoints onto keypoints of the submap around the revisited frame.
  // revisitedFrameIdx is the frame index where the query frame meets a loop.
  bool LoopClosureRegistration(std::list<LidarState>::iterator& itQueryState,
                               std::list<LidarState>::iterator& itRevisitedState,
                               Eigen::Isometry3d& loopClosureTransform,
                               Eigen::Matrix6d& loopClosureCovariance);

  // ---------------------------------------------------------------------------
  //   Map helpers
  // ---------------------------------------------------------------------------

  // Init sub maps with same parameters of LocalMaps
  void InitSubMaps(Maps& maps);

  // Aggregate logged keypoints of frames between # [windowStartIdx, windowStartEndIdx]
  // Default arguments lead to the aggregation of all available logged keypoints of keyframes.
  // It is used to recompute the current SLAM maps from the beginning using the new trajectory (after PGO)
  // and to build sub maps in the loop closure context
  // Keypoints are aggregated in world coordinates by default
  // or in base coordinates of frame #idxFrame when idxFrame is not negative
  void BuildMaps(Maps& maps, int windowStartIdx = -1, int windowEndIdx = -1, int idxFrame = -1);

  // Fetch the index of the frame stored in LogStates, acquired before startIt
  // and at a distance at least equal to minDistance
  // minDistance can be positive or negative to choose
  // in which chronological direction to perform the search
  std::list<LidarState>::iterator FetchStateIndex(std::list<LidarState>::iterator startIt, double minDistance);

  // ICP-LM Optimization process to estimate pose
  // Compute the pose of the sourceKeypoints by registering
  // sourcekeypoints on targetkeypoints
  LocalOptimizer::RegistrationError EstimatePose(const std::map<Keypoint, PointCloud::Ptr>& sourceKeypoints,
                                                 const Maps& targetKeypoints,
                                                 Optimization::Parameters& params,
                                                 Eigen::Isometry3d& posePrior,
                                                 std::map<Keypoint, KeypointsMatcher::MatchingResults>& matchingResults);

  // ---------------------------------------------------------------------------
  //   Undistortion helpers
  // ---------------------------------------------------------------------------

  // All points of the current frame have been acquired at different timestamps.
  // The goal is to express them in the same referential, at the timestamp in
  // input scan header. This can be done using estimated egomotion and assuming
  // a constant velocity during a sweep or using external measurements.

  // TODO LogStates should be a sensor in External Sensor to get all useful synchronization functions

  // Get the trajectory section ([0, N]) between failures and recoveries
  // from a timestamp. This allows to notify the discontinuities
  // in the trajectory when interpolating
  int GetTrajSection(double time) const;

  // Get the state logged which is the closest to the input time
  std::list<LidarState>::const_iterator GetClosestState(double time) const;

  // Get a window of states around an iterator
  // This function checks time consistency and logging bounds
  std::vector<PoseStamped> GetStatesWindow(std::list<LidarState>::const_iterator queryIt, unsigned int windowWidth) const;

  // Undistort the points using previous logged states
  // the input points must be represented in base frame or in lidar frame
  // if baseToPointsRef is specified, each at their own timestamp
  // All output points will be represented in the last frame of LogStates or in current frame (Tworld)
  // If addTworld is true, Tworld is the reference to undistort the points
  // Best performances can be achieved if pcIn is sorted
  void UndistortWithLogStates(PointCloud::Ptr pcIn, PointCloud::Ptr pcOut,
                              bool addTworld,
                              int startIdx = 0, int endIdx = -1,
                              Eigen::Isometry3d baseToPointsRef = Eigen::Isometry3d::Identity(),
                              double timeOffset = 0) const;

  // Undistort the points using external pose measurement information
  // the input points must be represented in base frame or in lidar frame
  // if baseToPointsRef is specified, each at their own timestamp
  // All output points will be represented in the frame at refTime
  // Best performances can be achieved if pc is sorted
  void UndistortWithPoseMeasurement(PointCloud::Ptr pc, double refTime,
                                    int startIdx = 0, int endIdx = -1,
                                    Eigen::Isometry3d baseToPointsRef = Eigen::Isometry3d::Identity(),
                                    double timeOffset = 0) const;

  // ---------------------------------------------------------------------------
  //   Confidence estimator helpers
  // ---------------------------------------------------------------------------

  // Estimate the overlap of the current scan onto the keypoints submaps
  void EstimateOverlap();

  // Test if the pose complies with motion limitations
  void CheckMotionLimits();

  // ---------------------------------------------------------------------------
  //   Transformation helpers
  // ---------------------------------------------------------------------------

  // Rigidly transform a pointcloud in a multi-threaded way.
  PointCloud::Ptr TransformPointCloud(PointCloud::ConstPtr cloud,
                                      const Eigen::Isometry3d& tf,
                                      const std::string& frameId = "") const;

  // Aggregate a set of frames from LIDAR to BASE or WORLD coordinates.
  // If worldCoordinates=false, it returns points in BASE coordinates (no undistortion).
  // If worldCoordinates=true, it returns points in WORLD coordinates (optionally undistorted).
  // The LIDAR to BASE offsets specific to each sensor are properly added.
  // The output aggregated points timestamps are corrected to be relative to the 1st frame timestamp.
  // NOTE: If transforming to WORLD coordinates, be sure that Tworld has been updated
  //       (e.g. during Localization step).
  // This function is parallelized internally, do not put it in a parallelized loop
  PointCloud::Ptr AggregateFrames(const std::vector<PointCloud::Ptr>& frames, bool worldCoordinates = false, bool undistort = true) const;

  // ---------------------------------------------------------------------------
  //   External sensor helpers
  // ---------------------------------------------------------------------------
  // All external sensors are shared ptr.
  // The init function creates the objects with known parameters
  void InitWheelOdom();
  void InitGravity();
  void InitImu();
  // WARNING : If the calibration has not been set for the landmarks detector (cf SetLmDetectorCalibration),
  // default identity calibration is set to the current landmark.
  // This way, data can be stored before receiving the calibration.
  void InitLandmarkManager(int id);
  void InitGps();
  void InitPoseSensor();
  void InitCamera();
};

} // end of LidarSlam namespace