//==============================================================================
// 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/Transform.h"
#include "LidarSlam/LidarPoint.h"
#include "LidarSlam/Enums.h"
#include "LidarSlam/SpinningSensorKeypointExtractor.h"
#include "LidarSlam/KeypointsMatcher.h"
#include "LidarSlam/LocalOptimizer.h"
#include "LidarSlam/MotionModel.h"
#include "LidarSlam/RollingGrid.h"
#include "LidarSlam/PointCloudStorage.h"
#include "LidarSlam/SensorConstraints.h"
#include "LidarSlam/SharedList.h"

#include <Eigen/Geometry>

#include <deque>

#define SetProtectedMacro(name,type) void Set##name (type _arg) {std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex); name = _arg;}
#define GetProtectedMacro(name,type) type Get##name () const { std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex); return name;}

namespace LidarSlam
{

// Structure containing one state
// States are stored in a list for further global optimization processing
// They will be kept as long as they fit the buffer size
struct LidarState : public SensorState
{
  using Point = LidarPoint;
  using PCStoragePtr = std::shared_ptr<PointCloudStorage<Point>>;

  LidarState() = default;
  LidarState(const Eigen::UnalignedIsometry3d& isometry, const std::array<double, 36>& covariance = {}, double time = 0.)
    : SensorState(isometry, covariance, time) {};

  // boolean to reflect the time validity
  bool ValidTime = false;
  // Keypoints extracted at current pose, undistorted and expressed in BASE coordinates
  std::map<Keypoint, PCStoragePtr> Keypoints;
};

// Structure containing all publishable info
// These data are stored in a list for an external publishing
// They must be removed after being published
struct Publishable
{
  using Point = LidarPoint;
  using PCStoragePtr = std::shared_ptr<PointCloudStorage<Point>>;

  LidarState State;
  // Undistorted keypoints expressed in World coordinates
  std::map<Keypoint, PCStoragePtr> KeypointsWorld;
  // Local keypoint maps (edges/planes/blobs)
  std::map<Keypoint, PCStoragePtr> Maps;
  // Debug array
  std::unordered_map<std::string, std::vector<double>> DebugArray;
  // Debug array provided by the keypoints extractor
  std::unordered_map<std::string, std::vector<float>> KEDebugArray;
  // Debug information
  std::unordered_map<std::string, double> DebugInformation;
  // Current frames registered in map
  PCStoragePtr RegisteredFrame;
  // Pose relative to publish time
  // It corresponds to the computed pose corrected by latency time with ego motion model
  Eigen::Isometry3d LatencyCorrectedIsometry = Eigen::UnalignedIsometry3d::Identity();
  // Number of the frame processed since last reset
  int IdxFrame = 0;
  // Confidence estimators to evaluate pose output
  float Overlap = -1;
  bool ComplyMotionLimits = true;
  int NbMatchedKeypoints = 0;
};

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 PCStoragePtr = std::shared_ptr<PCStorage>;

  // Initialization
  Slam();

  // Destructor to free all threads depending on shared lists
  // and to stop local Slam thread
  ~Slam();
  
  // Ending of the SLAM,
  // It frees the front end and back end threads and join them
  // if stopPublishing is set, it frees the output depending threads too.
  void Stop(bool stopPublishing = true);

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

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

  // Process a new frame.
  // This triggers 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 ProcessFrame(const PointCloud::Ptr& pc) { this->ProcessFrames({pc}); }

  // Process a set of new frames.
  // This triggers 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 ProcessFrames(const std::vector<PointCloud::Ptr>& frames);

  // Run pose graph optimization using GPS trajectory to improve SLAM maps and trajectory.
  // Each GPS position must have an associated precision covariance.
  // TODO : run that in a separated thread.
  void RunPoseGraphOptimization(const std::vector<Transform>& gpsPositions,
                                const std::vector<std::array<double, 9>>& gpsCovariances,
                                Eigen::Isometry3d& gpsToSensorOffset,
                                const std::string& g2oFileName = "");

  // Set world transform with an initial guess (usually from GPS after calibration).
  void SetWorldTransformFromGuess(const Transform& poseGuess);

  // Save keypoints maps to disk for later use
  void SaveMapsToPCD(const std::string& filePrefix, PCDFormat pcdFormat = PCDFormat::BINARY_COMPRESSED) const;

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

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

  // ---------------------------------------------------------------------------
  //   Multithreading relative functions
  // ---------------------------------------------------------------------------

  // Add new scan to the waiting list
  // This must be performed by the external main thread receiving Lidar sensor data
  // It will be used by the front end thread
  void AddScans(const std::vector<PointCloud::Ptr>& scans);

  // Get the oldest waiting result
  // This must be performed by an external publisher thread
  // It is published by the front end thread
  // WARNING: this function is locking a mutex with a conditional variable,
  // Once called, it will block the thread until some result appear or until the end
  // of the waiting is required by the external main thread.
  bool GetResult(Publishable& toPublish);

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

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

  SetProtectedMacro(Verbosity, int)
  GetProtectedMacro(Verbosity, int)

  void SetUseBlobs(bool ub) { std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex); this->UseKeypoints[BLOB] = ub; }
  bool GetUseBlobs() const { std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex); return this->UseKeypoints.at(BLOB); }

  SetProtectedMacro(EgoMotion, EgoMotionMode)
  GetProtectedMacro(EgoMotion, EgoMotionMode)

  SetProtectedMacro(Undistortion, UndistortionMode)
  GetProtectedMacro(Undistortion, UndistortionMode)

  void SetLoggingMax(int lMax);
  int GetLoggingMax() const;

  SetProtectedMacro(LoggingStorage, PointCloudStorageType)
  GetProtectedMacro(LoggingStorage, PointCloudStorageType)

  SetProtectedMacro(UpdateMap, bool)
  GetProtectedMacro(UpdateMap, bool)

  void SetOutputRequirement(const std::unordered_map<Output, bool, std::hash<int>>& required);
  std::unordered_map<Output, bool, std::hash<int>> GetOutputRequirement() const;
  // ---------------------------------------------------------------------------
  //   Coordinates systems parameters
  // ---------------------------------------------------------------------------

  SetProtectedMacro(BaseFrameId, std::string const&)
  GetProtectedMacro(BaseFrameId, std::string)

  SetProtectedMacro(WorldFrameId, std::string const&)
  GetProtectedMacro(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);

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

  GetProtectedMacro(TwoDMode, bool)
  SetProtectedMacro(TwoDMode, bool)

  // Get/Set EgoMotion
  GetProtectedMacro(EgoMotionLMMaxIter, unsigned int)
  SetProtectedMacro(EgoMotionLMMaxIter, unsigned int)

  GetProtectedMacro(EgoMotionICPMaxIter, unsigned int)
  SetProtectedMacro(EgoMotionICPMaxIter, unsigned int)

  GetProtectedMacro(EgoMotionMaxNeighborsDistance, double)
  SetProtectedMacro(EgoMotionMaxNeighborsDistance, double)

  GetProtectedMacro(EgoMotionEdgeNbNeighbors, unsigned int)
  SetProtectedMacro(EgoMotionEdgeNbNeighbors, unsigned int)

  GetProtectedMacro(EgoMotionEdgeMinNbNeighbors, unsigned int)
  SetProtectedMacro(EgoMotionEdgeMinNbNeighbors, unsigned int)

  GetProtectedMacro(EgoMotionEdgePcaFactor, double)
  SetProtectedMacro(EgoMotionEdgePcaFactor, double)

  GetProtectedMacro(EgoMotionPlaneNbNeighbors, unsigned int)
  SetProtectedMacro(EgoMotionPlaneNbNeighbors, unsigned int)

  GetProtectedMacro(EgoMotionPlanePcaFactor1, double)
  SetProtectedMacro(EgoMotionPlanePcaFactor1, double)

  GetProtectedMacro(EgoMotionPlanePcaFactor2, double)
  SetProtectedMacro(EgoMotionPlanePcaFactor2, double)

  GetProtectedMacro(EgoMotionEdgeMaxModelError, double)
  SetProtectedMacro(EgoMotionEdgeMaxModelError, double)

  GetProtectedMacro(EgoMotionPlaneMaxModelError, double)
  SetProtectedMacro(EgoMotionPlaneMaxModelError, double)

  GetProtectedMacro(EgoMotionInitSaturationDistance, double)
  SetProtectedMacro(EgoMotionInitSaturationDistance, double)

  GetProtectedMacro(EgoMotionFinalSaturationDistance, double)
  SetProtectedMacro(EgoMotionFinalSaturationDistance, double)

  // Get/Set Localization
  GetProtectedMacro(LocalizationLMMaxIter, unsigned int)
  SetProtectedMacro(LocalizationLMMaxIter, unsigned int)

  GetProtectedMacro(LocalizationICPMaxIter, unsigned int)
  SetProtectedMacro(LocalizationICPMaxIter, unsigned int)

  GetProtectedMacro(LocalizationMaxNeighborsDistance, double)
  SetProtectedMacro(LocalizationMaxNeighborsDistance, double)

  GetProtectedMacro(LocalizationEdgeNbNeighbors, unsigned int)
  SetProtectedMacro(LocalizationEdgeNbNeighbors, unsigned int)

  GetProtectedMacro(LocalizationEdgeMinNbNeighbors, unsigned int)
  SetProtectedMacro(LocalizationEdgeMinNbNeighbors, unsigned int)

  GetProtectedMacro(LocalizationEdgePcaFactor, double)
  SetProtectedMacro(LocalizationEdgePcaFactor, double)

  GetProtectedMacro(LocalizationPlaneNbNeighbors, unsigned int)
  SetProtectedMacro(LocalizationPlaneNbNeighbors, unsigned int)

  GetProtectedMacro(LocalizationPlanePcaFactor1, double)
  SetProtectedMacro(LocalizationPlanePcaFactor1, double)

  GetProtectedMacro(LocalizationPlanePcaFactor2, double)
  SetProtectedMacro(LocalizationPlanePcaFactor2, double)

  GetProtectedMacro(LocalizationEdgeMaxModelError, double)
  SetProtectedMacro(LocalizationEdgeMaxModelError, double)

  GetProtectedMacro(LocalizationPlaneMaxModelError, double)
  SetProtectedMacro(LocalizationPlaneMaxModelError, double)

  GetProtectedMacro(LocalizationBlobNbNeighbors, unsigned int)
  SetProtectedMacro(LocalizationBlobNbNeighbors, unsigned int)

  GetProtectedMacro(LocalizationInitSaturationDistance, double)
  SetProtectedMacro(LocalizationInitSaturationDistance, double)

  GetProtectedMacro(LocalizationFinalSaturationDistance, double)
  SetProtectedMacro(LocalizationFinalSaturationDistance, double)

  // Sensor parameters
  void SetWheelOdomWeight(double weight);
  double GetWheelOdomWeight() const;

  void SetGravityWeight(double weight);
  double GetGravityWeight() const;

  // The time offset must be computed as FrameFirstPointTimestamp - FrameReceptionPOSIXTime
  void SetSensorTimeOffset(double timeOffset);
  double GetSensorTimeOffset() const;

  void AddGravityMeasurement(const SensorConstraints::GravityMeasurement& gm);
  void AddWheelOdomMeasurement(const SensorConstraints::WheelOdomMeasurement& om);
  void ClearSensorMeasurements();

  // ---------------------------------------------------------------------------
  //   Key frames and Maps parameters
  GetProtectedMacro(KfDistanceThreshold, double)
  SetProtectedMacro(KfDistanceThreshold, double)

  GetProtectedMacro(KfAngleThreshold, double)
  SetProtectedMacro(KfAngleThreshold, double)

  void ClearMaps();
  void SetVoxelGridLeafSize(Keypoint k, double size);
  void SetVoxelGridSize(int size);
  void SetVoxelGridResolution(double resolution);

  GetProtectedMacro(DebugInfoFields, std::vector<std::string>)
  SetProtectedMacro(DebugInfoFields, const std::vector<std::string>&)

  // ---------------------------------------------------------------------------
  //   Confidence estimation
  // ---------------------------------------------------------------------------
  // Overlap
  GetProtectedMacro(OverlapSamplingRatio, float)
  void SetOverlapSamplingRatio (float _arg);
  
  // Motion constraints
  GetProtectedMacro(AccelerationLimits, Eigen::Array2f)
  SetProtectedMacro(AccelerationLimits, const Eigen::Array2f&)

  GetProtectedMacro(VelocityLimits, Eigen::Array2f)
  SetProtectedMacro(VelocityLimits, const Eigen::Array2f&)

  GetProtectedMacro(WindowWidth, int)
  SetProtectedMacro(WindowWidth, int)

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}, {PLANE, true}, {BLOB, false}};

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

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

  // Should the keypoints features maps be updated at each step.
  // It is usually set to true, but forbiding maps update can be usefull in case
  // of post-SLAM optimization with GPS and then run localization only in fixed
  // optimized map.
  bool UpdateMap = true;

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

  // Boolean to enable or disable time dependent processes (motion confidence estimators,
  // motion model based ego-motion, undistortion)
  bool TimeDependentProcessesEnabled = true;

  // Output requirement
  std::unordered_map<Output, bool, std::hash<int>> OutputRequirement;

  // ---------------------------------------------------------------------------
  //   Multithreading relative variables
  // ---------------------------------------------------------------------------

  // ---------------------------------------------------------------------------
  // Shared lists

  // Log info on each pose
  // It may be shared by 2 threads : 
  // the local SLAM optimization and the global optimization.
  // It is filled by the local SLAM optimization until it reaches
  // a maximum size.
  // Default maximum size is fixed to 100 (cf. constructor),
  // and can be parameterized (cf. LoggingMax)
  SharedList<LidarState> LogStates;

  // List of publishable info (local Slam results)
  // It may be shared between the local SLAM optimization
  // and an external publisher.
  // If no publisher is popping on this list or if the publisher is long,
  // this list may increase.
  // Default maximum size is fixed to 10 (cf. constructor)
  SharedList<Publishable> OutputResults;

  // Current waiting frames
  // It is shared between the local Slam thread and a supplier thread.
  // This list may increase if an external event disturb local slam process
  // (e.g. the log list or the publishable objects list are locked,
  // the number of keypoints is very large for one frame...)
  // Default maximum size is fixed to 10 (cf. constructor)
  SharedList<std::vector<PointCloud::Ptr>> InputScans;

  // ---------------------------------------------------------------------------
  // Threads

  // Front end process of the SLAM
  // New scan processing and localization
  // relatively to the current map
  // Output a local pose
  std::thread LocalSlam;

  // Back end process of the SLAM
  // Recompute the states graph and the maps
  // when receiving a hint on absolute pose
  // (Translation or whole pose hint with relative covariances)
  // Update the map for front end thread (LocalSlam)
  // and last pose for next ego-motion step
  std::thread GlobalSlam;

  // ---------------------------------------------------------------------------
  // Mutexes

  // Mutex to protect local variables : Tworld, maps and logged States
  // from front end thread, back end thread and main thread (user) modifications
  // Used in front end (ProcessFrames() -> reading / writing in all steps),
  // in back end (while updating LogStates / Maps), 
  // LoadMapsFromPCD() and SetWorldTransformFromGuess()
  // NOTE: shared_timed_mutex should be replaced by shared_mutex
  // when upgrading to C++17
  mutable std::shared_timed_mutex LocalVariablesMutex;

  // Mutex to protect parameters
  // from main thread (user modifications)
  // while allowing front end / back end threads to use getters
  // Used in setters/getters and ProcessFrames()
  // NOTE: shared_timed_mutex should be replaced by shared_mutex
  // when upgrading to C++17
  mutable std::shared_timed_mutex ParamsMutex;

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

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

  // [s] SLAM computation duration of last processed frame (~Tworld delay)
  // used to compute latency compensated pose
  double Latency;

  // **** UNDISTORTION ****

  // Transform interpolator to estimate the pose of the sensor within a lidar
  // frame, using the BASE poses at the beginning and end of frame.
  // This will be used to undistort the pointcloud and express its points
  // relatively to the same BASE pose at frame header timestamp.
  // This will use the point-wise 'time' field, representing the time offset
  // in seconds to add to the frame header timestamp.
  LinearTransformInterpolator<double> WithinFrameMotion;

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

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

  // keypoints local map
  std::map<Keypoint, std::shared_ptr<RollingGrid>> LocalMaps;

  // ---------------------------------------------------------------------------
  //   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
  // Compute the residual with a weight, a measurements list and
  // taking account of the acquisition time correspondance
  // The sensor measurements must be filled and cleared from outside this lib
  SensorConstraints::WheelOdometryManager WheelOdomManager;

  // IMU manager
  // Compute the residual with a weight, a measurements list and
  // taking account of the acquisition time correspondance
  // The sensor measurements must be filled and cleared from outside this lib
  SensorConstraints::ImuManager ImuManager;

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

  // Number of outer ICP-optim loop iterations to perform.
  // Each iteration will consist of building ICP matches, then optimizing them.
  unsigned int EgoMotionICPMaxIter = 4;
  unsigned int LocalizationICPMaxIter = 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 EgoMotionLMMaxIter = 15;
  unsigned int LocalizationLMMaxIter = 15;

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

  // Max distance allowed between a source keypoint and its neighbors in target map.
  // If one of the neighbors is farther, the neighborhood will be rejected.
  double EgoMotionMaxNeighborsDistance = 5.;
  double LocalizationMaxNeighborsDistance = 5.;

  // Edge keypoints matching: point-to-line distance
  unsigned int EgoMotionEdgeNbNeighbors = 8;
  unsigned int EgoMotionEdgeMinNbNeighbors = 3;
  double EgoMotionEdgePcaFactor = 5.;
  double EgoMotionEdgeMaxModelError = 0.2;
  unsigned int LocalizationEdgeNbNeighbors = 10;
  unsigned int LocalizationEdgeMinNbNeighbors = 4;
  double LocalizationEdgePcaFactor = 5.0;
  double LocalizationEdgeMaxModelError = 0.2;

  // Plane keypoints matching: point-to-plane distance
  unsigned int EgoMotionPlaneNbNeighbors = 5;
  double EgoMotionPlanePcaFactor1 = 35.0;
  double EgoMotionPlanePcaFactor2 = 8.0;
  double EgoMotionPlaneMaxModelError = 0.2;
  unsigned int LocalizationPlaneNbNeighbors = 5;
  double LocalizationPlanePcaFactor1 = 35.0;
  double LocalizationPlanePcaFactor2 = 8.0;
  double LocalizationPlaneMaxModelError = 0.2;

  // Blob keypoints matching: point-to-ellipsoid distance
  unsigned int LocalizationBlobNbNeighbors = 10;

  // 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 EgoMotionInitSaturationDistance = 5. ;
  double EgoMotionFinalSaturationDistance = 1. ;
  double LocalizationInitSaturationDistance = 2.;
  double LocalizationFinalSaturationDistance = 0.5;

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

  // Number of poses 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.
  int WindowWidth = 0;

  // [Hz] Maximum LiDAR frequency to detect wrong timestamps
  float MaxLidarFrequency = 50.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();

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

  // ---------------------------------------------------------------------------
  //  Results
  // ---------------------------------------------------------------------------

  // Main front end function
  // Wait for a new frame in waiting list and process it
  // The loop stops when the scans waiting list closes
  void GetAndProcessFrames();

  // Main back end function
  // Wait for a new absolute pose in waiting list and use it to run pose graph optimization
  // The loop stops when the external poses waiting list closes
  void GetAndProcessExternalAbsolutePose();

  // Get the computed world transform so far, but compensating SLAM computation duration latency.
  Eigen::Isometry3d GetLatencyCompensatedWorldTransform() const;
  // Get the covariance of the last localization step (registering the current frame to the last map)
  // DoF order : X, Y, Z, rX, rY, rZ
  std::array<double, 36> GetTransformCovariance() const;

  // Get keypoints maps
  PointCloud::Ptr GetMap(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 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;
  // Names of the fields output in debugInfo
  std::vector<std::string> DebugInfoFields;

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

  // Extra/Interpolate scan pose using previous motion from PreviousTworld and Tworld.
  // 'time' arg is the time offset in seconds to current frame header.stamp.
  Eigen::Isometry3d InterpolateScanPose(double time);

  // Init undistortion interpolator time bounds based on point-wise time field.
  void InitUndistortion();

  // Update the undistortion interpolator poses bounds,
  // and refine the undistortion of the current keypoints clouds.
  void RefineUndistortion();

  // ---------------------------------------------------------------------------
  //   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/WithinFrameMotion have been updated
  //       (updated during the Localization step).
  PointCloud::Ptr AggregateFrames(const std::vector<PointCloud::Ptr>& frames, bool worldCoordinates) const;
};

} // end of LidarSlam namespace