//==============================================================================
// 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 <Eigen/Geometry>

#include <list>

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

#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 get/set optimization parameters which include
// matching parameters for keypoints match and parameters for ICP-LM optimization
// See more details in struct Parameters
// Macro to set MatchingParameters in struct Parameters
#define OptMatchingParamsSetMacro(name, param, type)                             \
void Set##name##param (type _arg) { name##Params.MatchingParams.param = _arg; }
// Macro to get a parameter value in MatchingParameters
#define  OptMatchingParamsGetMacro(name, param, type)                            \
type Get##name##param () const { return name##Params.MatchingParams.param; }
// Macro to set parameters (except matching parameters) in struct Parameters
#define OptimizationParamsSetMacro(name, param, type)                            \
void Set##name##param (type _arg) { name##Params.param = _arg; }
// Macro to get a parameter value (except matching parameters) in struct Parameters
#define  OptimizationParamsGetMacro(name, param, type)                           \
type Get##name##param () const { return name##Params.param; }

// Macro to set/get which types of pose graph constraints to use
#define PGOConstraintSetMacro(name,type)                                         \
void SetPGOConstraint##name (type _arg) { UsePGOConstraints[name] = _arg; }
#define PGOConstraintGetMacro(name,type)                                         \
type GetPGOConstraint##name () const { return UsePGOConstraints.at(name); }

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

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 world transform with an initial guess (usually from GPS after calibration).
  void SetWorldTransformFromGuess(const Eigen::Isometry3d& poseGuess);

  // Initialize pose using pose measurements
  // This allows to represent the maps and the trajectory of Lidar
  // in an external frame (not first Lidar frame)
  // the time of the synchronized pose is input so no Lidar frame needs
  // to have been loaded to use this function
  bool InitTworldWithPoseMeasurement(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);

  // ---------------------------------------------------------------------------
  //   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, 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(FixFirstVertex, bool)
  SetMacro(FixFirstVertex, bool)

  GetMacro(FixLastVertex, bool)
  SetMacro(FixLastVertex, bool)

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

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

  PGOConstraintSetMacro(LOOP_CLOSURE, bool)
  PGOConstraintGetMacro(LOOP_CLOSURE, bool)

  PGOConstraintSetMacro(LANDMARK, bool)
  PGOConstraintGetMacro(LANDMARK, bool)

  PGOConstraintSetMacro(PGO_GPS, bool)
  PGOConstraintGetMacro(PGO_GPS, bool)

  // ---------------------------------------------------------------------------
  //   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<uint8_t, KeypointExtractorPtr> GetKeyPointsExtractors() const;
  void SetKeyPointsExtractors(const std::map<uint8_t, 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(uint8_t deviceId = 0) const;
  void SetKeyPointsExtractor(KeypointExtractorPtr extractor, uint8_t deviceId = 0);

  // 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(uint8_t deviceId = 0) const;
  void SetBaseToLidarOffset(const Eigen::Isometry3d& transform, uint8_t deviceId = 0);

  // 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
  OptimizationParamsGetMacro(EgoMotion, LMMaxIter, unsigned int)
  OptimizationParamsSetMacro(EgoMotion, LMMaxIter, unsigned int)

  OptimizationParamsGetMacro(EgoMotion, ICPMaxIter, unsigned int)
  OptimizationParamsSetMacro(EgoMotion, ICPMaxIter, unsigned int)

  OptMatchingParamsGetMacro(EgoMotion, MaxNeighborsDistance, double)
  OptMatchingParamsSetMacro(EgoMotion, MaxNeighborsDistance, double)

  OptMatchingParamsGetMacro(EgoMotion, EdgeNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(EgoMotion, EdgeNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(EgoMotion, EdgeMinNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(EgoMotion, EdgeMinNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(EgoMotion, PlaneNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(EgoMotion, PlaneNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(EgoMotion, PlanarityThreshold, double)
  OptMatchingParamsSetMacro(EgoMotion, PlanarityThreshold, double)

  OptMatchingParamsGetMacro(EgoMotion, EdgeMaxModelError, double)
  OptMatchingParamsSetMacro(EgoMotion, EdgeMaxModelError, double)

  OptMatchingParamsGetMacro(EgoMotion, PlaneMaxModelError, double)
  OptMatchingParamsSetMacro(EgoMotion, PlaneMaxModelError, double)

  OptimizationParamsGetMacro(EgoMotion, InitSaturationDistance, double)
  OptimizationParamsSetMacro(EgoMotion, InitSaturationDistance, double)

  OptimizationParamsGetMacro(EgoMotion, FinalSaturationDistance, double)
  OptimizationParamsSetMacro(EgoMotion, FinalSaturationDistance, double)

  // Get/Set Localization
  OptimizationParamsGetMacro(Localization, LMMaxIter, unsigned int)
  OptimizationParamsSetMacro(Localization, LMMaxIter, unsigned int)

  OptimizationParamsGetMacro(Localization, ICPMaxIter, unsigned int)
  OptimizationParamsSetMacro(Localization, ICPMaxIter, unsigned int)

  OptMatchingParamsGetMacro(Localization, MaxNeighborsDistance, double)
  OptMatchingParamsSetMacro(Localization, MaxNeighborsDistance, double)

  OptMatchingParamsGetMacro(Localization, EdgeNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(Localization, EdgeNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(Localization, EdgeMinNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(Localization, EdgeMinNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(Localization, PlaneNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(Localization, PlaneNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(Localization, PlanarityThreshold, double)
  OptMatchingParamsSetMacro(Localization, PlanarityThreshold, double)

  OptMatchingParamsGetMacro(Localization, EdgeMaxModelError, double)
  OptMatchingParamsSetMacro(Localization, EdgeMaxModelError, double)

  OptMatchingParamsGetMacro(Localization, PlaneMaxModelError, double)
  OptMatchingParamsSetMacro(Localization, PlaneMaxModelError, double)

  OptMatchingParamsGetMacro(Localization, BlobNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(Localization, BlobNbNeighbors, unsigned int)

  OptimizationParamsGetMacro(Localization, InitSaturationDistance, double)
  OptimizationParamsSetMacro(Localization, InitSaturationDistance, double)

  OptimizationParamsGetMacro(Localization, FinalSaturationDistance, double)
  OptimizationParamsSetMacro(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);

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

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

  bool GetWheelOdomRelative() const;
  void SetWheelOdomRelative(bool relative);

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

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

  // 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();}

  // Transform the whole trajectory (including current pose) to GPS reference frame (e.g. UTM)
  // Warning : in trajectory, the position is remained odometric i.e. only the orientation
  // is adapted for precision purposes but the offset position is stored in GPS manager
  bool CalibrateWithGps();

  // 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);}

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

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

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

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

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

  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
  // ---------------------------------------------------------------------------
  GetMacro(ExtDetectLoopClosure, bool)
  SetMacro(ExtDetectLoopClosure, bool)

  GetMacro(LoopClosureQueryIdx, unsigned int)
  SetMacro(LoopClosureQueryIdx, unsigned int)

  GetMacro(LoopClosureRevisitedIdx, unsigned int)
  SetMacro(LoopClosureRevisitedIdx, unsigned int)

  GetMacro(LCQueryWindowStartRange, int)
  SetMacro(LCQueryWindowStartRange, int)

  GetMacro(LCQueryWindowEndRange, int)
  SetMacro(LCQueryWindowEndRange, int)

  GetMacro(LCRevisitedWindowStartRange, int)
  SetMacro(LCRevisitedWindowStartRange, int)

  GetMacro(LCRevisitedWindowEndRange, int)
  SetMacro(LCRevisitedWindowEndRange, int)

  // Get/Set Loop Closure registration parameters
  GetMacro(EnableLoopClosureOffset, bool)
  SetMacro(EnableLoopClosureOffset, bool)

  GetMacro(LoopClosureICPWithSubmap, bool)
  SetMacro(LoopClosureICPWithSubmap, bool)

  OptimizationParamsGetMacro(LoopClosure, LMMaxIter, unsigned int)
  OptimizationParamsSetMacro(LoopClosure, LMMaxIter, unsigned int)

  OptimizationParamsGetMacro(LoopClosure, ICPMaxIter, unsigned int)
  OptimizationParamsSetMacro(LoopClosure, ICPMaxIter, unsigned int)

  OptMatchingParamsGetMacro(LoopClosure, MaxNeighborsDistance, double)
  OptMatchingParamsSetMacro(LoopClosure, MaxNeighborsDistance, double)

  OptMatchingParamsGetMacro(LoopClosure, EdgeNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(LoopClosure, EdgeNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(LoopClosure, EdgeMinNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(LoopClosure, EdgeMinNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(LoopClosure, PlaneNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(LoopClosure, PlaneNbNeighbors, unsigned int)

  OptMatchingParamsGetMacro(LoopClosure, PlanarityThreshold, double)
  OptMatchingParamsSetMacro(LoopClosure, PlanarityThreshold, double)

  OptMatchingParamsGetMacro(LoopClosure, EdgeMaxModelError, double)
  OptMatchingParamsSetMacro(LoopClosure, EdgeMaxModelError, double)

  OptMatchingParamsGetMacro(LoopClosure, PlaneMaxModelError, double)
  OptMatchingParamsSetMacro(LoopClosure, PlaneMaxModelError, double)

  OptMatchingParamsGetMacro(LoopClosure, BlobNbNeighbors, unsigned int)
  OptMatchingParamsSetMacro(LoopClosure, BlobNbNeighbors, unsigned int)

  OptimizationParamsGetMacro(LoopClosure, InitSaturationDistance, double)
  OptimizationParamsSetMacro(LoopClosure, InitSaturationDistance, double)

  OptimizationParamsGetMacro(LoopClosure, FinalSaturationDistance, double)
  OptimizationParamsSetMacro(LoopClosure, FinalSaturationDistance, double)
  // ---------------------------------------------------------------------------
  //   Confidence estimation
  // ---------------------------------------------------------------------------

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

  GetMacro(OverlapEstimation, float)

  // Matches
  GetMacro(TotalMatchedKeypoints, int)

  // Motion constraints
  GetMacro(AccelerationLimits, Eigen::Array2f)
  SetMacro(AccelerationLimits, const Eigen::Array2f&)

  GetMacro(VelocityLimits, Eigen::Array2f)
  SetMacro(VelocityLimits, const Eigen::Array2f&)

  GetMacro(TimeWindowDuration, float)
  SetMacro(TimeWindowDuration, float)

  GetMacro(ComplyMotionLimits, 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 = "world";  // CS of trajectory and maps
  std::string BaseFrameId = "base";    // 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 initial Tworld value (might be set by SetWorldTransformFromGuess)
  // It is used to reset the pose in case of failure
  Eigen::Isometry3d TworldInit = Eigen::Isometry3d::Identity();

  // Reflect the success of one SLAM iteration computation (used to log or not the state).
  bool Valid = 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;

  // ---------------------------------------------------------------------------
  //   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<uint8_t, 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<uint8_t, Eigen::UnalignedIsometry3d> BaseToLidarOffsets = {{0, 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 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
  // ---------------------------------------------------------------------------

  // Boolean to decide if the closure information can be provided externally by various sources (user or camera or landmark...)
  // If true, users need to indicate the query frame index and the revisited frame index for loop closure.
  bool ExtDetectLoopClosure = false;

  // Frame indices to indicate where the loop closure is.
  unsigned int LoopClosureQueryIdx = 0;
  unsigned int LoopClosureRevisitedIdx = 0;

  // Number of frames used to build the submaps around the query frame or the revisited frame for loop closure.
  // To build sub maps around query frame, use frames [LoopClosureQueryIdx + LCQueryWindowStartRange, LoopClosureQueryIdx + LCQueryWindowEndRange].
  // To build sub maps around the revisited frame, use frames [LoopClosureRevisitedIdx + LCRevisitedWindowStartRange, LoopClosureRevisitedIdx + LCRevisitedWindowEndRnage].
  int LCQueryWindowStartRange = -50;
  int LCQueryWindowEndRange = 50;
  int LCRevisitedWindowStartRange = -50;
  int LCRevisitedWindowEndRange = 50;

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

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

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

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

  // 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;

  // 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();

  // 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
  };

  Optimization::Parameters LoopClosureParams
  {
    // 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
  };

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

  // Boolean to decide if we want to some vertices of the graph
  bool FixFirstVertex = false;
  bool FixLastVertex = false;
  // 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 = {{LOOP_CLOSURE, true}, {LANDMARK, true}, {PGO_GPS, 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;

  // Check motion limitations compliance
  bool ComplyMotionLimits = true;

  // Previous computed velocity (for acceleration computation)
  Eigen::Array2f PreviousVelocity;

  // 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;

  // Motion limitations
  // Local velocity thresholds in BASE
  Eigen::Array2f VelocityLimits     = {FLT_MAX, FLT_MAX};
  // Local acceleration thresholds in BASE
  Eigen::Array2f AccelerationLimits = {FLT_MAX, FLT_MAX};

  // Duration on which to estimate the local velocity
  // This window is used to smooth values to get a more accurate velocity estimation
  // If 0, motion limits won't be checked.
  // WARNING : the logging time out must be greater
  // in order to comply with this required value.
  float TimeWindowDuration = 0.f;

  // ---------------------------------------------------------------------------
  //   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 the current frame is a keyframe or not
  bool CheckKeyFrame();

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

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

  // If external detection is enabled, check whether the inputs of loop closure frame indices are stored in the LogStates
  // if not, detect automatically a revisited frame idx for the current frame (TBA)
  bool DetectLoopClosureIndices(std::list<LidarState>::iterator& itQueryState, std::list<LidarState>::iterator& itRevisitedState);

  // 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);

  // 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 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.
  // WARNING : this function is parallelized internally, do not put it in a parallelized loop
  PointCloud::Ptr AggregateFrames(const std::vector<PointCloud::Ptr>& frames, bool worldCoordinates, 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();
};

} // end of LidarSlam namespace