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

// LOCAL
#include "LidarSlam/Slam.h"
#include "LidarSlam/Utilities.h"
#include "LidarSlam/KDTreePCLAdaptor.h"
#include "LidarSlam/ConfidenceEstimators.h"

#ifdef USE_G2O
#include "LidarSlam/PoseGraphOptimization.h"
#endif  // USE_G2O
// CERES
#include <ceres/solver.h>
// PCL
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
// EIGEN
#include <Eigen/Dense>

#define PRINT_VERBOSE(minVerbosityLevel, stream) if (this->Verbosity >= (minVerbosityLevel)) {std::cout << stream << std::endl;}
#define IF_VERBOSE(minVerbosityLevel, command) if (this->Verbosity >= (minVerbosityLevel)) { command; }

namespace LidarSlam
{

using KDTree = KDTreePCLAdaptor<Slam::Point>;

namespace Utils
{
namespace
{
//-----------------------------------------------------------------------------
std::array<double, 36> Matrix6dToStdArray36(const Eigen::Matrix6d& covar)
{
  std::array<double, 36> cov;
  std::copy_n(covar.data(), 36, cov.begin());
  return cov;
}

//-----------------------------------------------------------------------------
//! Approximate pointcloud memory size
inline size_t PointCloudMemorySize(const Slam::PointCloud& cloud)
{
  return (sizeof(cloud) + (sizeof(Slam::PointCloud::PointType) * cloud.size()));
}

} // end of anonymous namespace
} // end of Utils namespace

//==============================================================================
//   Main SLAM use
//==============================================================================

//-----------------------------------------------------------------------------
Slam::Slam()
{
  PRINT_COLOR(GREEN, "*************Starting Slam process*************");
  // By default, the waiting frames list contains 10 elements at max
  // 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...)
  this->InputScans.SetSizeMax(10);
  // By default, the log buffer contains 2 states at max,
  // to be able to handle undistortion and ego motion modes
  // It can be increased with the SetLoggingMax function.
  // It must be increased if global optimization is set.
  this->LogStates.SetSizeMax(2);
  // In a normal working pipeline, the size of this list must not 
  // increase, if it reaches its max size, some output issues are encountered
  this->OutputResults.SetSizeMax(10);

  // Allocate a default Keypoint Extractor for device 0
  this->KeyPointsExtractors[0] = std::make_shared<SpinningSensorKeypointExtractor>();

  // Init output fields' names for debug info
  for (auto k : {EDGE, PLANE})
    this->DebugInfoFields.push_back("EgoMotion: " + Utils::Plural(KeypointTypeNames.at(k)) + " used");
  for (auto k : KeypointTypes)
    this->DebugInfoFields.push_back("Localization: " + Utils::Plural(KeypointTypeNames.at(k)) + " used");

  this->DebugInfoFields.push_back("Localization: position error");
  this->DebugInfoFields.push_back("Localization: orientation error");
  this->DebugInfoFields.push_back("Confidence: overlap");
  this->DebugInfoFields.push_back("Confidence: comply motion limits");

  // Allocate maps
  for (auto k : KeypointTypes)
    this->LocalMaps[k] = std::make_shared<RollingGrid>();

  // Set default maps parameters
  this->SetVoxelGridResolution(10.);
  this->SetVoxelGridSize(50);
  this->SetVoxelGridLeafSize(EDGE, 0.30);
  this->SetVoxelGridLeafSize(PLANE, 0.60);
  this->SetVoxelGridLeafSize(BLOB, 0.30);

  // Initialize output requirement
  // No output is required by default
  for (int outIdx = POSE_ODOM; outIdx < nOutputs; ++outIdx)
  {
    Output out = static_cast<Output>(outIdx);
    this->OutputRequirement[out] = false;
  }

  // Reset SLAM internal state
  this->Reset();
}

Slam::~Slam()
{
  this->Stop();
  PRINT_COLOR(GREEN, "*************Ending Slam process*************");
}

//-----------------------------------------------------------------------------
void Slam::Stop(bool stopPublishing)
{
  // Free front end thread
  this->InputScans.Stop();
  // Free back end thread
  this->LogStates.Stop();
  // Free publisher thread if required
  if (stopPublishing)
    this->OutputResults.Stop();

  // Wait for internal local slam thread which was just freed
  if (this->LocalSlam.joinable())
  {
    PRINT_COLOR(GREEN, "*************Ending front end*************");
    this->LocalSlam.join();
  }

  // Wait for internal global slam thread which was just freed
  if (this->GlobalSlam.joinable())
  {
    PRINT_COLOR(GREEN, "*************Ending back end**************");
    this->GlobalSlam.join();
  }
}

//-----------------------------------------------------------------------------
void Slam::Reset(bool resetLog)
{
  // Free front end and back end
  this->Stop(false);

  // Reset keypoints maps
  this->ClearMaps();

  // Clear waiting scans
  this->InputScans.Reset();
  // Clear output waiting results
  this->OutputResults.Reset();

  // Reset keyframes
  this->KfLastPose = Eigen::Isometry3d::Identity();
  this->KfCounter = 0;

  // n-DoF parameters
  this->Tworld = Eigen::Isometry3d::Identity();
  this->WithinFrameMotion.SetTransforms(Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity());

  // Reset pose uncertainty
  this->LocalizationUncertainty = LocalOptimizer::RegistrationError();

  // Reset point clouds
  this->CurrentFrames.clear();
  this->RegisteredFrame.reset(new PointCloud);
  this->CurrentFrames.emplace_back(new PointCloud);
  for (auto k : KeypointTypes)
  {
    this->CurrentRawKeypoints[k].reset(new PointCloud);
    this->CurrentUndistortedKeypoints[k].reset(new PointCloud);
    this->CurrentWorldKeypoints[k].reset(new PointCloud);
  }

  // Reset keypoints matching results
  for (auto k : {EDGE, PLANE})
    this->EgoMotionMatchingResults[k] = KeypointsMatcher::MatchingResults();
  for (auto k : KeypointTypes)
    this->LocalizationMatchingResults[k] = KeypointsMatcher::MatchingResults();

  // Reset log history
  if (resetLog)
  {
    // Clear log states
    this->NbrFrameProcessed = 0;
    this->LogStates.Reset();

    // Reset processing duration timers
    Utils::Timer::Reset();
  }

  PRINT_COLOR(GREEN, "*************Starting front end*************");
  // Launch front end in another thread
  this->LocalSlam = std::thread(&Slam::GetAndProcessFrames, this);

  PRINT_COLOR(GREEN, "*************Starting back end*************");
  // Launch back end in another thread
  // this->GlobalSlam = std::thread(&Slam::GetAndProcessExternalAbsolutePose, this);
}

//-----------------------------------------------------------------------------
void Slam::SetNbThreads(int n)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  // Set number of threads for main processes
  this->NbThreads = n;
  // Set number of threads for keypoints extraction
  for (const auto& kv : this->KeyPointsExtractors)
    kv.second->SetNbThreads(n); 
}

//-----------------------------------------------------------------------------
void Slam::GetAndProcessExternalAbsolutePose()
{
  // std::vector<PointCloud::Ptr> PoseReceived;
  // while (this->InputScans.WaitAndPopFront(lastFramesReceived))
  //   this->ProcessFrames(lastFramesReceived);
}

//-----------------------------------------------------------------------------
void Slam::GetAndProcessFrames()
{
  std::vector<PointCloud::Ptr> FramesReceived;
  while (this->InputScans.WaitAndPopFront(FramesReceived))
    this->ProcessFrames(FramesReceived);
}

//-----------------------------------------------------------------------------
void Slam::ProcessFrames(const std::vector<PointCloud::Ptr>& frames)
{
  // Lock the parameters mutex
  // These values can be changed with setters in external main thread
  std::shared_lock<std::shared_timed_mutex> lockParams(this->ParamsMutex);
  // Lock the local variables mutex (Tworld / maps / logStates)
  // These structures can be changed from the external main thread in LoadMapsFromPCD()
  // and SetWorldTransformFromGuess() and from front end / back end processes
  std::unique_lock<std::shared_timed_mutex> lockLocalVariables(this->LocalVariablesMutex);
  Utils::Timer::Init("SLAM frame processing");

  // Check that input frames are correct and can be processed
  if (!this->CheckFrames(frames))
  { 
    Publishable toPublish;
    toPublish.State.Time = -1;
    this->OutputResults.EmplaceBack(toPublish);
    return;
  }

  this->CurrentFrames = frames;
  auto time = this->CurrentFrames[0]->header.stamp;

  // Set init pose (can have been modified by global optimization / reset)
  // 1) To insure a smooth local SLAM, the global optimization must refine
  // poses relatively to last pose, i.e, last pose must be fixed.
  // 2) To insure a fix reference point, the global optimization must refine
  // poses relatively to starting point, i.e, last pose can have changed.
  // New local frames can have been computed while global optimization was performed
  // This implies a "new transforms rebasing step" in global optimization with local optimization disabled.
  LidarState lastState;
  // If LogStates empty, do not modify Tworld (must be identity after Reset)
  if (this->LogStates.Back(lastState))
    this->Tworld = lastState.Isometry;

  PRINT_VERBOSE(2, "\n#########################################################");
  PRINT_VERBOSE(1, "Processing frame " << this->NbrFrameProcessed <<
                   " (at time " << time / 1000000 << "." << time % 1000000 << ")");
  PRINT_VERBOSE(2, "#########################################################\n");

  // Compute the edge and planar keypoints
  IF_VERBOSE(3, Utils::Timer::Init("Keypoints extraction"));
  this->ExtractKeypoints();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Keypoints extraction"));

  // Estimate an init Tworld by extrapolating new pose with a constant velocity model
  // and/or registering current frame on previous one
  IF_VERBOSE(3, Utils::Timer::Init("Ego-Motion"));
  this->ComputeEgoMotion();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Ego-Motion"));

  bool externalSensorsEnabled = this->GetWheelOdomWeight() > 1e-6 || this->GetGravityWeight() > 1e-6;
  if (this->TimeDependentProcessesEnabled && externalSensorsEnabled)
  {
    IF_VERBOSE(3, Utils::Timer::Init("Sensor constraints computation"));
    this->ComputeSensorConstraints();
    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Sensor constraints computation"));
  }

  // Perform Localization : update Tworld from map and current frame keypoints
  // and optionally undistort keypoints clouds based on ego-motion
  IF_VERBOSE(3, Utils::Timer::Init("Localization"));
  this->Localization();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization"));

  // Compute and check pose confidence estimators
  if (this->OverlapSamplingRatio > 0 || this->WindowWidth > 0)
  {
    IF_VERBOSE(3, Utils::Timer::Init("Confidence estimators computation"));
    if (this->OverlapSamplingRatio > 0)
      this->EstimateOverlap();
    if (this->WindowWidth > 0 && this->TimeDependentProcessesEnabled)
      this->CheckMotionLimits();
    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Confidence estimators computation"));
  }

  // Update keypoints maps : add current keypoints to map using Tworld
  if (this->UpdateMap)
  {
    IF_VERBOSE(3, Utils::Timer::Init("Maps update"));
    this->UpdateMapsUsingTworld();
    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Maps update"));
  }

  // Log current frame processing results : pose, covariance and keypoints.
  IF_VERBOSE(3, Utils::Timer::Init("Logging"));
  this->LogCurrentFrameState(Utils::PclStampToSec(time));
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Logging"));

  // Motion and localization parameters estimation information display
  if (this->Verbosity >= 2)
  {
    SET_COUT_FIXED_PRECISION(3);
    std::cout << "========== SLAM results ==========\n";
    if (this->Undistortion && this->TimeDependentProcessesEnabled)
    {
      Eigen::Isometry3d motion = this->WithinFrameMotion.GetTransformRange();
      std::cout << "Within frame motion:\n"
                   " translation = [" << motion.translation().transpose()                                        << "] m\n"
                   " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(motion.linear())).transpose() << "] °\n";
    }

    Eigen::Isometry3d Trelative = this->Tworld;
    LidarState lastState;
    if (this->LogStates.Back(lastState))
      Trelative = lastState.Isometry.inverse() * this->Tworld;
    std::cout << "Local Motion:\n"
                 " translation = [" << Trelative.translation().transpose()                                        << "] m\n"
                 " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(Trelative.linear())).transpose() << "] °\n"
                 "Global pose:\n"
                 " position    = [" << this->Tworld.translation().transpose()                                        << "] m\n"
                 " orientation = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(this->Tworld.linear())).transpose() << "] °" << std::endl;
    RESET_COUT_FIXED_PRECISION;
  }

  if (this->Verbosity >= 5)
  {
    SET_COUT_FIXED_PRECISION(3);
    std::cout << "========== Memory usage ==========\n";
    for (auto k : KeypointTypes)
    {
      PointCloud::Ptr map = this->GetMap(k);
      std::cout << Utils::Capitalize(Utils::Plural(KeypointTypeNames.at(k))) << " map: "
                << map->size() << " points, " << Utils::PointCloudMemorySize(*map) * 1e-6 << " MB\n";
    }

    // Logged keypoints
    std::deque<LidarState> LogCopy = this->LogStates.Copy();
    std::map<Keypoint, size_t> memory, points;
    // Initialize number of points per keypoint type
    for (auto k : KeypointTypes)
    {
      points[k] = 0;
      memory[k] = 0;
    }
    // Sum points and memory allocated of each keypoints cloud
    for (auto const& st: LogCopy)
    {
      for (auto k : KeypointTypes)
      {
        points[k] += st.Keypoints.at(k)->PointsSize();
        memory[k] += st.Keypoints.at(k)->MemorySize();
      }
    }

    // Print keypoints memory usage
    for (auto k : KeypointTypes)
    {
      std::cout << Utils::Capitalize(Utils::Plural(KeypointTypeNames.at(k)))<< " log  : "
                << LogCopy.size() << " frames, "
                << memory[k] << " points, "
                << points[k] * 1e-6 << " MB\n";
    }
    RESET_COUT_FIXED_PRECISION;
  }

  // Frame processing duration
  this->Latency = Utils::Timer::Stop("SLAM frame processing");
  this->NbrFrameProcessed++;
  IF_VERBOSE(1, Utils::Timer::StopAndDisplay("SLAM frame processing"));
  this->TimeDependentProcessesEnabled = true;
}

//-----------------------------------------------------------------------------
void Slam::ComputeSensorConstraints()
{
  double currLidarTime = Utils::PclStampToSec(this->CurrentFrames[0]->header.stamp);
  this->WheelOdomManager.ComputeWheelAbsoluteConstraint(currLidarTime);
  this->ImuManager.ComputeGravityConstraint(currLidarTime);
}

//-----------------------------------------------------------------------------
void Slam::RunPoseGraphOptimization(const std::vector<Transform>& gpsPositions,
                                    const std::vector<std::array<double, 9>>& gpsCovariances,
                                    Eigen::Isometry3d& gpsToSensorOffset,
                                    const std::string& g2oFileName)
{
  #if 0
  #ifdef USE_G2O
  IF_VERBOSE(1, Utils::Timer::Init("Pose graph optimization"));
  IF_VERBOSE(3, Utils::Timer::Init("PGO : optimization"));

  // Copy all states
  std::vector<LidarState> states = this->LogStates.LastElements();

  const unsigned int nbSlamPoses = states.size();

  if (this->LoggingMax == 2)
  {
    PRINT_WARNING("SLAM logging is not enabled : covariances will be "
                  "arbitrarly set and maps will not be optimized during pose "
                  "graph optimization.");

    // Set all poses covariances equal to twice the last one if we did not log it
    std::array<double, 36> fakeSlamCovariance = Utils::Matrix6dToStdArray36(this->LocalizationUncertainty.Covariance * 2);
    for (unsigned int i = 0; i < nbSlamPoses; i++)
      slamCovariances.emplace_back(fakeSlamCovariance);
  }

  // Init pose graph optimizer
  PoseGraphOptimization poseGraphOptimization;
  poseGraphOptimization.SetNbIteration(500);
  poseGraphOptimization.SetVerbose(this->Verbosity >= 2);
  poseGraphOptimization.SetSaveG2OFile(!g2oFileName.empty());
  poseGraphOptimization.SetG2OFileName(g2oFileName);
  poseGraphOptimization.SetGpsToSensorCalibration(gpsToSensorOffset);

  // Run pose graph optimization
  // TODO : templatize poseGraphOptimization to accept any STL container and avoid deque <-> vector copies
  std::vector<Transform> optimizedSlamPoses;
  if (!poseGraphOptimization.Process(slamPoses, gpsPositions,
                                     slamCovariances, gpsCovariances,
                                     optimizedSlamPoses))
  {
    PRINT_ERROR("Pose graph optimization failed.");
    return;
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("PGO : optimization"));

  // Update GPS/LiDAR calibration
  gpsToSensorOffset = optimizedSlamPoses.front().GetIsometry();

  // Update SLAM trajectory and maps
  IF_VERBOSE(3, Utils::Timer::Init("PGO : SLAM reset"));
  this->Reset(false);
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("PGO : SLAM reset"));
  IF_VERBOSE(3, Utils::Timer::Init("PGO : frames keypoints aggregation"));
  std::map<Keypoint, PointCloud> keypoints;
  std::map<Keypoint, PointCloud::Ptr> aggregatedKeypointsMap;
  for (auto k : KeypointTypes)
    aggregatedKeypointsMap[k].reset(new PointCloud);

  for (unsigned int i = 0; i < nbSlamPoses; i++)
  {
    // Update SLAM pose
    this->LogTrajectory[i].SetIsometry(gpsToSensorOffset.inverse() * optimizedSlamPoses[i].GetIsometry());

    // Transform frame keypoints to world coordinates
    std::map<Keypoint, PointCloud::Ptr> logKeypoints;
    for (auto k : KeypointTypes)
      logKeypoints[k] = this->UseKeypoints[k] ? this->LogKeypoints[k][i].GetCloud() : PointCloud::Ptr(new PointCloud);

    if (this->Undistortion && this->TimeDependentProcessesEnabled && i >= 1)
    {
      // Init the undistortion interpolator
      LinearTransformInterpolator<double> interpolator;
      interpolator.SetTransforms(this->LogTrajectory[i - 1].GetIsometry(), this->LogTrajectory[i].GetIsometry());
      interpolator.SetTimes(this->LogTrajectory[i].time - this->LogTrajectory[i - 1].time, 0.);

      // Perform undistortion of keypoint clouds
      for (auto k : KeypointTypes)
      {
        keypoints[k].clear();
        keypoints[k].reserve(logKeypoints[k]->size());
        for (const Point& p : *logKeypoints[k])
          keypoints[k].push_back(Utils::TransformPoint(p, interpolator(p.time)));
      }
    }
    else
    {
      Eigen::Matrix4d currentTransform = this->LogTrajectory[i].GetMatrix();
      for (auto k : KeypointTypes)
        pcl::transformPointCloud(*logKeypoints[k],  keypoints[k],  currentTransform);
    }

    // Aggregate new keypoints to maps
    for (auto k : KeypointTypes)
      *aggregatedKeypointsMap[k] += keypoints[k];
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("PGO : frames keypoints aggregation"));
  IF_VERBOSE(3, Utils::Timer::Init("PGO : final SLAM map update"));

  // Set final pose
  this->Tworld         = this->LogTrajectory[nbSlamPoses - 1].GetIsometry();
  this->PreviousTworld = this->LogTrajectory[nbSlamPoses - 2].GetIsometry();

  // Update SLAM maps
  for (auto k : KeypointTypes)
  {
    // We do not do map.Add(aggregatedPoints) as this would try to roll the map
    // so that the entire aggregatedPoints can best fit into map. But if this
    // entire point cloud does not fit into map, the rolling grid will be
    // centered on the aggregatedPoints bounding box center.
    // Instead, we prefer to roll so that the last frame keypoints can fit,
    // ensuring that next frame will be matched efficiently to rolled map.
    if (this->UseKeypoints[k])
    {
      Eigen::Vector4f minPoint, maxPoint;
      pcl::getMinMax3D(keypoints[k], minPoint, maxPoint);
      this->LocalMaps[k]->Roll(minPoint.head<3>().cast<double>().array(), maxPoint.head<3>().cast<double>().array());
      this->LocalMaps[k]->Add(aggregatedKeypointsMap[k], false);
    }
  }

  // Processing duration
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("PGO : final SLAM map update"));
  IF_VERBOSE(1, Utils::Timer::StopAndDisplay("Pose graph optimization"));
  #else
  #define UNUSED(var) (void)(var)
  UNUSED(gpsPositions); UNUSED(gpsCovariances); UNUSED(gpsToSensorOffset); UNUSED(g2oFileName);
  PRINT_ERROR("SLAM PoseGraphOptimization requires G2O, but it was not found.");
  #endif  // USE_G2O
  #endif // Compile
}

//-----------------------------------------------------------------------------
void Slam::SetWorldTransformFromGuess(const Transform& poseGuess)
{
  // Lock mutex to set pose outside the front end thread
  std::unique_lock<std::shared_timed_mutex> lock(this->LocalVariablesMutex);
  // Set current pose
  this->Tworld = poseGuess.GetIsometry();

  // Time dependent processes are not valid anymore since we imposed a discontinuity.
  // (i.e undistortion + ego motion with motion model + local acceleration / velocity)
  this->TimeDependentProcessesEnabled = false;
  // Current frame keypoints are reset so that ego-motion registration will be skipped for next frame.
  for (auto k : KeypointTypes)
    this->CurrentRawKeypoints[k].reset(new PointCloud);
}

//-----------------------------------------------------------------------------
void Slam::SaveMapsToPCD(const std::string& filePrefix, PCDFormat pcdFormat) const
{
  IF_VERBOSE(3, Utils::Timer::Init("Keypoints maps saving to PCD"));

  // Save keypoints maps
  for (auto k : KeypointTypes)
  {
    if (this->UseKeypoints.at(k))
      savePointCloudToPCD(filePrefix + Utils::Plural(KeypointTypeNames.at(k)) + ".pcd",  *this->GetMap(k),  pcdFormat, true);
  }

  // TODO : save map origin (in which coordinates?) in title or VIEWPOINT field

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Keypoints maps saving to PCD"));
}

//-----------------------------------------------------------------------------
void Slam::LoadMapsFromPCD(const std::string& filePrefix, bool resetMaps)
{
  // Lock mutex to reset maps outside the front end thread
  std::unique_lock<std::shared_timed_mutex> lock(this->LocalVariablesMutex);
  IF_VERBOSE(3, Utils::Timer::Init("Keypoints maps loading from PCD"));

  // In most of the cases, we would like to reset SLAM internal maps before
  // loading new maps to avoid conflicts.
  if (resetMaps)
    this->ClearMaps();

  for (auto k : KeypointTypes)
  {
    std::string path = filePrefix + Utils::Plural(KeypointTypeNames.at(k)) + ".pcd";
    PointCloud::Ptr keypoints(new PointCloud);
    if (pcl::io::loadPCDFile(path, *keypoints) == 0)
    {
      std::cout << "SLAM keypoints map successfully loaded from " << path << std::endl;
      this->LocalMaps[k]->Add(keypoints);
    }
  }
  // TODO : load/use map origin (in which coordinates?) in title or VIEWPOINT field
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Keypoints maps loading from PCD"));
}

//==============================================================================
//   Shared lists for multithreaded processes
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::AddScans(const std::vector<PointCloud::Ptr>& scans)
{
  this->InputScans.EmplaceBack(scans);
}

//-----------------------------------------------------------------------------
bool Slam::GetResult(Publishable& toPublish)
{
  return this->OutputResults.WaitAndPopFront(toPublish);
}

//==============================================================================
//   SLAM results getters
//==============================================================================

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::GetLatencyCompensatedWorldTransform() const
{
  // Get 2 last transforms
  std::vector<LidarState> lastStates = this->LogStates.LastElements(2);
  if (lastStates.empty())
    return Eigen::Isometry3d::Identity();
  else if (lastStates.size() == 1)
    return lastStates.back().Isometry;
  const LidarState& previous = lastStates.front();
  const LidarState& current = lastStates.back();
  const Eigen::Isometry3d& H0 = previous.Isometry;
  const Eigen::Isometry3d& H1 = current.Isometry;

  // Linearly compute normalized timestamp of Hpred.
  // We expect H0 and H1 to match with time 0 and 1.
  // If timestamps are not defined or too close, extrapolation is impossible.
  if (std::abs(current.Time - previous.Time) < 1e-6)
  {
    PRINT_WARNING("Unable to compute latency-compensated transform : timestamps undefined or too close.");
    return current.Isometry;
  }
  // If requested extrapolation timestamp is too far from previous frames timestamps, extrapolation is impossible.
  if (std::abs(this->Latency / (current.Time - previous.Time)) > this->MaxExtrapolationRatio)
  {
    PRINT_WARNING("Unable to compute latency-compensated transform : extrapolation time is too far.");
    return current.Isometry;
  }

  // Extrapolate H0 and H1 to get expected Hpred at current time
  Eigen::Isometry3d Hpred = LinearInterpolation(H0, H1, current.Time + this->Latency, previous.Time, current.Time);
  return Hpred;
}

//-----------------------------------------------------------------------------
std::array<double, 36> Slam::GetTransformCovariance() const
{
  return Utils::Matrix6dToStdArray36(this->LocalizationUncertainty.Covariance);
}

//-----------------------------------------------------------------------------
std::unordered_map<std::string, double> Slam::GetDebugInformation() const
{
  std::unordered_map<std::string, double> map;
  for (auto k : {EDGE, PLANE})
  {
    std::string name = "EgoMotion: " + Utils::Plural(KeypointTypeNames.at(k)) + " used";
    map[name] = this->EgoMotionMatchingResults.at(k).NbMatches();
  }

  for (auto k : KeypointTypes)
  {
    std::string name = "Localization: " + Utils::Plural(KeypointTypeNames.at(k)) + " used";
    map[name] = this->LocalizationMatchingResults.at(k).NbMatches();
  }

  map["Localization: position error"]      = this->LocalizationUncertainty.PositionError;
  map["Localization: orientation error"]   = this->LocalizationUncertainty.OrientationError;
  map["Confidence: overlap"]               = this->OverlapEstimation;
  map["Confidence: comply motion limits"]  = this->ComplyMotionLimits;

  return map;
}

//-----------------------------------------------------------------------------
std::unordered_map<std::string, std::vector<double>> Slam::GetDebugArray() const
{
  auto toDoubleVector = [](auto const& scalars) { return std::vector<double>(scalars.begin(), scalars.end()); };

  std::unordered_map<std::string, std::vector<double>> map;
  
  if (this->EgoMotion == EgoMotionMode::REGISTRATION ||
      this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION_AND_REGISTRATION)
  {
    for (auto k : {EDGE, PLANE})
    {
      std::string name = "EgoMotion: " + KeypointTypeNames.at(k) + " matches";
      map[name]  = toDoubleVector(this->EgoMotionMatchingResults.at(k).Rejections);
      name = "EgoMotion: " + KeypointTypeNames.at(k) + " weights";
      map[name]  = this->EgoMotionMatchingResults.at(k).Weights;
    }
  }

  for (auto k : KeypointTypes)
  {
    std::string name = "Localization: " + KeypointTypeNames.at(k) + " matches";
    map[name]  = toDoubleVector(this->LocalizationMatchingResults.at(k).Rejections);
    name = "Localization: " + KeypointTypeNames.at(k) + " weights";
    map[name]  = this->LocalizationMatchingResults.at(k).Weights;
  }

  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetRegisteredFrame()
{
  // If the input points have not been aggregated to WORLD coordinates yet,
  // transform and aggregate them
  if (this->RegisteredFrame->header.stamp != this->CurrentFrames[0]->header.stamp)
    this->RegisteredFrame = this->AggregateFrames(this->CurrentFrames, true);
  return this->RegisteredFrame;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetMap(Keypoint k) const
{
  PointCloud::Ptr map = this->LocalMaps.at(k)->Get();
  map->header = Utils::BuildPclHeader(this->CurrentFrames[0]->header.stamp,
                                      this->WorldFrameId,
                                      this->NbrFrameProcessed);
  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetKeypoints(Keypoint k, bool worldCoordinates)
{
  // Return keypoints in BASE coordinates
  if (!worldCoordinates)
    return this->CurrentUndistortedKeypoints.at(k);

  // Return keypoints in WORLD coordinates
  // If the keypoints have not been transformed yet to WORLD coordinates, perform transformation
  if (this->CurrentWorldKeypoints.at(k)->header.stamp != this->CurrentUndistortedKeypoints.at(k)->header.stamp)
    this->CurrentWorldKeypoints[k] = this->TransformPointCloud(this->CurrentUndistortedKeypoints[k],
                                                               this->Tworld, this->WorldFrameId);
  return this->CurrentWorldKeypoints.at(k);
}

//==============================================================================
//   Main SLAM steps
//==============================================================================

//-----------------------------------------------------------------------------
bool Slam::CheckFrames(const std::vector<PointCloud::Ptr>& frames)
{
  // Check input frames and return if they are all empty
  bool allFramesEmpty = true;
  for (unsigned int i = 0; i < frames.size(); ++i)
  {
    if (!frames[i] || !frames[i]->empty())
      allFramesEmpty = false;
    else
      PRINT_WARNING("SLAM input frame " << i << " is an empty pointcloud : frame ignored.");
  }
  if (allFramesEmpty)
  {
    PRINT_ERROR("SLAM input only contains empty pointclouds : exiting.");
    return false;
  }

  // Check time

  // Skip frames if it has the same timestamp as previous ones (will induce problems in extrapolation)
  if (frames[0]->header.stamp == this->CurrentFrames[0]->header.stamp)
  {
    PRINT_ERROR("SLAM frames have the same timestamp (" << frames[0]->header.stamp << ") as previous ones : frames ignored.");
    return false;
  }

  // Disable time dependent processes if timestamp is not consistent with previous ones
  this->TimeDependentProcessesEnabled = true;
  const double t = Utils::PclStampToSec(frames[0]->header.stamp);
  std::vector<LidarState> lastStates = this->LogStates.LastElements(2);
  if (lastStates.size() == 2)
  {
    const double t0 = lastStates[0].Time;
    const double t1 = lastStates[1].Time;
    if (std::abs((t - t1) / (t1 - t0)) > this->MaxExtrapolationRatio)
    {
      PRINT_WARNING("New timestamp is too far from previous one, all time dependent processes are ignored.")
      this->TimeDependentProcessesEnabled = false;
    }
  }

  if (lastStates.size() > 0)
  {
    const double tLast = lastStates.back().Time;
    if (std::abs(t - tLast) < 1.f / this->MaxLidarFrequency)
    {
      PRINT_WARNING("New timestamp is too close from previous one, all time dependent processes are ignored.")
      this->TimeDependentProcessesEnabled = false;
    }
  }

  // Check frame dropping
  for (unsigned int i = 0; i < frames.size(); ++i)
  {
    unsigned int droppedFrames = frames[i]->header.seq - this->PreviousFramesSeq[i] - 1;
    if ((this->PreviousFramesSeq[i] > 0) && (droppedFrames > 0))
      PRINT_WARNING(droppedFrames << " frame(s)" << (frames.size() > 1 ? " from LiDAR device " + std::to_string(i) : "") << " were dropped by SLAM\n");
    this->PreviousFramesSeq[i] = frames[i]->header.seq;
  }

  return true;
}

//-----------------------------------------------------------------------------
void Slam::ExtractKeypoints()
{
  PRINT_VERBOSE(2, "========== Keypoints extraction ==========");

  // Current keypoints become previous ones
  this->PreviousRawKeypoints = this->CurrentRawKeypoints;

  // Extract keypoints from each input cloud
  std::map<Keypoint, std::vector<PointCloud::Ptr>> keypoints;
  for (const auto& frame: this->CurrentFrames)
  {
    // If the frame is empty, ignore it
    if (frame->empty())
      continue;

    // Get keypoints extractor to use for this LiDAR device
    int lidarDevice = frame->front().device_id;
    // Check if KE exists
    if (!this->KeyPointsExtractors.count(lidarDevice))
    {
      // If KE does not exist but we are only using a single KE, use default one
      if (this->KeyPointsExtractors.size() == 1)
      {
        PRINT_WARNING("Input frame comes from LiDAR device " << lidarDevice
                    << " but no keypoints extractor has been set for this device : using default extractor for device 0.");
        lidarDevice = 0;
      }
      // Otherwise ignore frame
      else
      {
        PRINT_ERROR("Input frame comes from LiDAR device " << lidarDevice
                    << " but no keypoints extractor has been set for this device : ignoring frame.");
        continue;
      }
    }
    KeypointExtractorPtr& ke = this->KeyPointsExtractors[lidarDevice];

    // Extract keypoints from this frame
    ke->ComputeKeyPoints(frame);
    for (auto k : KeypointTypes)
      keypoints[k].push_back(ke->GetKeypoints(k));
  }

  // Merge all keypoints extracted from different frames together
  for (auto k : KeypointTypes)
  {
    if (this->UseKeypoints[k])
      this->CurrentRawKeypoints[k] = this->AggregateFrames(keypoints[k], false);
    else
    {
      this->CurrentRawKeypoints[k].reset(new PointCloud);
      this->CurrentRawKeypoints[k]->header = Utils::BuildPclHeader(this->CurrentFrames[0]->header.stamp,
                                                                   this->BaseFrameId,
                                                                   this->NbrFrameProcessed);
    }
  }

  if (this->Verbosity >= 2)
  {
    std::cout << "Extracted features : ";
    for (auto k : KeypointTypes)
      std::cout << this->CurrentRawKeypoints[k]->size() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
    std::cout << std::endl;
  }
}

//-----------------------------------------------------------------------------
void Slam::ComputeEgoMotion()
{
  PRINT_VERBOSE(2, "========== Ego-Motion ==========");

  // Reset ego-motion
  Eigen::Isometry3d Trelative = Eigen::Isometry3d::Identity();

  // Linearly extrapolate previous motion to estimate new pose
  std::vector<LidarState> lastStates = this->LogStates.LastElements(2);
  if (lastStates.size() >= 2 &&
      (this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION ||
       this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION_AND_REGISTRATION) &&
      this->TimeDependentProcessesEnabled)
  {
    // Estimate relative transform with a constant velocity model
    const double t = Utils::PclStampToSec(this->CurrentFrames[0]->header.stamp);
    const double t0 = lastStates[0].Time;
    const double t1 = lastStates[1].Time;
    Eigen::Isometry3d nextTworldEstimation = LinearInterpolation(lastStates[0].Isometry, lastStates[1].Isometry, t, t0, t1);
    Trelative = this->Tworld.inverse() * nextTworldEstimation;
  }

  // Refine Trelative estimation by registering current frame on previous one
  if (this->EgoMotion == EgoMotionMode::REGISTRATION ||
      this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION_AND_REGISTRATION)
  {
    // kd-tree to process fast nearest neighbor
    // among the keypoints of the previous pointcloud
    IF_VERBOSE(3, Utils::Timer::Init("EgoMotion : build KD tree"));
    std::map<Keypoint, KDTree> kdtreePrevious;
    // Kdtrees map initialization to parallelize their 
    // construction using OMP and avoid concurrency issues
    for (auto k : {EDGE, PLANE})
      kdtreePrevious[k] = KDTree();

    // The iteration is not directly on Keypoint types
    // because of openMP behaviour which needs int iteration on MSVC
    int nbKeypointTypes = static_cast<int>(KeypointTypes.size());
    #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
    for (int i = 0; i < nbKeypointTypes; ++i)
    {
      Keypoint k = static_cast<Keypoint>(KeypointTypes[i]);
      if (kdtreePrevious.count(k))
        kdtreePrevious[k].Reset(this->PreviousRawKeypoints[k]);
    }

    if (this->Verbosity >= 2)
    {
      std::cout << "Keypoints extracted from previous frame : ";
      for (auto k : {EDGE, PLANE})
        std::cout << this->PreviousRawKeypoints[k]->size() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
      std::cout << std::endl;
    }

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("EgoMotion : build KD tree"));
    IF_VERBOSE(3, Utils::Timer::Init("Ego-Motion : whole ICP-LM loop"));

    // Reset ICP results
    this->TotalMatchedKeypoints = 0;

    // Init matching parameters
    KeypointsMatcher::Parameters matchingParams;
    matchingParams.NbThreads = this->NbThreads;
    matchingParams.SingleEdgePerRing = true;
    matchingParams.MaxNeighborsDistance = this->EgoMotionMaxNeighborsDistance;
    matchingParams.EdgeNbNeighbors = this->EgoMotionEdgeNbNeighbors;
    matchingParams.EdgeMinNbNeighbors = this->EgoMotionEdgeMinNbNeighbors;
    matchingParams.EdgePcaFactor = this->EgoMotionEdgePcaFactor;
    matchingParams.EdgeMaxModelError = this->EgoMotionEdgeMaxModelError;
    matchingParams.PlaneNbNeighbors = this->EgoMotionPlaneNbNeighbors;
    matchingParams.PlanePcaFactor1 = this->EgoMotionPlanePcaFactor1;
    matchingParams.PlanePcaFactor2 = this->EgoMotionPlanePcaFactor2;
    matchingParams.PlaneMaxModelError = this->EgoMotionPlaneMaxModelError;

    // ICP - Levenberg-Marquardt loop
    // At each step of this loop an ICP matching is performed. Once the keypoints
    // are matched, we estimate the the 6-DOF parameters by minimizing the
    // non-linear least square cost function using Levenberg-Marquardt algorithm.
    for (unsigned int icpIter = 0; icpIter < this->EgoMotionICPMaxIter; ++icpIter)
    {
      IF_VERBOSE(3, Utils::Timer::Init("  Ego-Motion : ICP"));

      // We want to estimate our 6-DOF parameters using a non linear least square
      // minimization. The non linear part comes from the parametrization of the
      // rotation endomorphism SO(3).
      // First, we need to build the point-to-line, point-to-plane and
      // point-to-blob ICP matches that will be optimized.
      // Then, we use CERES Levenberg-Marquardt optimization to minimize the problem.

      // Create a keypoints matcher
      // At each ICP iteration, the outliers removal is refined to be stricter
      double iterRatio = icpIter / static_cast<double>(this->EgoMotionICPMaxIter - 1);
      matchingParams.SaturationDistance = (1 - iterRatio) * this->EgoMotionInitSaturationDistance + iterRatio * this->EgoMotionFinalSaturationDistance;
      KeypointsMatcher matcher(matchingParams, Trelative);

      // Loop over keypoints to build the residuals
      for (auto k : {EDGE, PLANE})
        this->EgoMotionMatchingResults[k] = matcher.BuildMatchResiduals(this->CurrentRawKeypoints[k], kdtreePrevious[k], k);

      // Count matches and skip this frame
      // if there are too few geometric keypoints matched
      this->TotalMatchedKeypoints = 0;
      for (auto k : {EDGE, PLANE})
        this->TotalMatchedKeypoints += this->EgoMotionMatchingResults[k].NbMatches();

      if (this->TotalMatchedKeypoints < this->MinNbMatchedKeypoints)
      {
        PRINT_WARNING("Not enough keypoints, EgoMotion skipped for this frame.");
        break;
      }

      IF_VERBOSE(3, Utils::Timer::StopAndDisplay("  Ego-Motion : ICP"));
      IF_VERBOSE(3, Utils::Timer::Init("  Ego-Motion : LM optim"));

      // Init the optimizer with initial pose and parameters
      LocalOptimizer optimizer;
      optimizer.SetTwoDMode(this->TwoDMode);
      optimizer.SetPosePrior(Trelative);

      optimizer.SetLMMaxIter(this->EgoMotionLMMaxIter);
      optimizer.SetNbThreads(this->NbThreads);

      // Add LiDAR ICP matches
      for (auto k : {EDGE, PLANE})
        optimizer.AddResiduals(this->EgoMotionMatchingResults[k].Residuals);

      // Run LM optimization
      ceres::Solver::Summary summary = optimizer.Solve();
      PRINT_VERBOSE(4, summary.BriefReport());

      // Get back optimized Trelative
      Trelative = optimizer.GetOptimizedPose();

      IF_VERBOSE(3, Utils::Timer::StopAndDisplay("  Ego-Motion : LM optim"));

      // If no L-M iteration has been made since the last ICP matching, it means
      // that we reached a local minimum for the ICP-LM algorithm.
      if (summary.num_successful_steps == 1)
      {
        break;
      }
    }

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Ego-Motion : whole ICP-LM loop"));
    if (this->Verbosity >= 2)
    {
      std::cout << "Matched keypoints: " << this->TotalMatchedKeypoints << " (";
      for (auto k : {EDGE, PLANE})
        std::cout << this->EgoMotionMatchingResults[k].NbMatches() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
      std::cout << ")" << std::endl;
    }
  }

  // Init Tworld with ego motion result for Localization step
  this->Tworld = this->Tworld * Trelative;

  // Print EgoMotion results
  SET_COUT_FIXED_PRECISION(3);
  PRINT_VERBOSE(2, "Estimated Ego-Motion (motion since last frame):\n"
                   " translation = [" << Trelative.translation().transpose() << "] m\n"
                   " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(Trelative.linear())).transpose() << "] °");
  RESET_COUT_FIXED_PRECISION;
}

//-----------------------------------------------------------------------------
void Slam::Localization()
{
  PRINT_VERBOSE(2, "========== Localization ==========");

  // Init undistorted keypoints clouds from raw points
  this->CurrentUndistortedKeypoints = this->CurrentRawKeypoints;

  // Init and run undistortion if required
  if (this->Undistortion && this->TimeDependentProcessesEnabled)
  {
    IF_VERBOSE(3, Utils::Timer::Init("Localization : initial undistortion"));
    // Init the within frame motion interpolator time bounds
    this->InitUndistortion();
    // Undistort keypoints clouds
    this->RefineUndistortion();
    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization : initial undistortion"));
  }

  // Get keypoints from maps and build kd-trees for fast nearest neighbors search
  IF_VERBOSE(3, Utils::Timer::Init("Localization : keypoints extraction"));

  // The iteration is not directly on Keypoint types
  // because of openMP behaviour which needs int iteration on MSVC
  int nbKeypointTypes = static_cast<int>(KeypointTypes.size());
  #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
  for (int i = 0; i < nbKeypointTypes; ++i)
  {
    // If the map has been updated, the KD-tree needs to be updated
    Keypoint k = static_cast<Keypoint>(KeypointTypes[i]);
    if (this->UseKeypoints[k] && !this->LocalMaps[k]->IsSubMapKdTreeValid())
    {
      // If maps are fixed, we can build a single KD-tree of the entire map
      // to avoid rebuilding it again
      if (!this->UpdateMap)
        this->LocalMaps[k]->BuildSubMapKdTree();

      // Otherwise, we only extract the local sub maps to build a local and
      // smaller KD-tree
      else
      {
        // Estimate current keypoints bounding box
        PointCloud currWordKeypoints;
        pcl::transformPointCloud(*this->CurrentUndistortedKeypoints[k], currWordKeypoints, this->Tworld.matrix());
        Eigen::Vector4f minPoint, maxPoint;
        pcl::getMinMax3D(currWordKeypoints, minPoint, maxPoint);

        // Build submap of all points lying in this bounding box
        this->LocalMaps[k]->BuildSubMapKdTree(minPoint.head<3>().cast<double>().array(), maxPoint.head<3>().cast<double>().array());
      }
    }
  }

  if (this->Verbosity >= 2)
  {
    std::cout << "Keypoints extracted from map : ";
    for (auto k : KeypointTypes)
      std::cout << this->LocalMaps[k]->GetSubMapKdTree().GetInputCloud()->size()
                << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
    std::cout << std::endl;
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization : keypoints extraction"));
  IF_VERBOSE(3, Utils::Timer::Init("Localization : whole ICP-LM loop"));

  // Reset ICP results
  this->TotalMatchedKeypoints = 0;

  // Init matching parameters
  KeypointsMatcher::Parameters matchingParams;
  matchingParams.NbThreads = this->NbThreads;
  matchingParams.SingleEdgePerRing = false;
  matchingParams.MaxNeighborsDistance = this->LocalizationMaxNeighborsDistance;
  matchingParams.EdgeNbNeighbors = this->LocalizationEdgeNbNeighbors;
  matchingParams.EdgeMinNbNeighbors = this->LocalizationEdgeMinNbNeighbors;
  matchingParams.EdgePcaFactor = this->LocalizationEdgePcaFactor;
  matchingParams.EdgeMaxModelError = this->LocalizationEdgeMaxModelError;
  matchingParams.PlaneNbNeighbors = this->LocalizationPlaneNbNeighbors;
  matchingParams.PlanePcaFactor1 = this->LocalizationPlanePcaFactor1;
  matchingParams.PlanePcaFactor2 = this->LocalizationPlanePcaFactor2;
  matchingParams.PlaneMaxModelError = this->LocalizationPlaneMaxModelError;
  matchingParams.BlobNbNeighbors = this->LocalizationBlobNbNeighbors;

  // ICP - Levenberg-Marquardt loop
  // At each step of this loop an ICP matching is performed. Once the keypoints
  // are matched, we estimate the the 6-DOF parameters by minimizing the
  // non-linear least square cost function using Levenberg-Marquardt algorithm.
  for (unsigned int icpIter = 0; icpIter < this->LocalizationICPMaxIter; ++icpIter)
  {
    IF_VERBOSE(3, Utils::Timer::Init("  Localization : ICP"));

    // We want to estimate our 6-DOF parameters using a non linear least square
    // minimization. The non linear part comes from the parametrization of the
    // rotation endomorphism SO(3).
    // First, we need to build the point-to-line, point-to-plane and
    // point-to-blob ICP matches that will be optimized.
    // Then, we use CERES Levenberg-Marquardt optimization to minimize problem.

    // Create a keypoints matcher
    // At each ICP iteration, the outliers removal is refined to be stricter
    double iterRatio = icpIter / static_cast<double>(this->LocalizationICPMaxIter - 1);
    matchingParams.SaturationDistance = (1 - iterRatio) * this->LocalizationInitSaturationDistance + iterRatio * this->LocalizationFinalSaturationDistance;
    KeypointsMatcher matcher(matchingParams, this->Tworld);

    // Loop over keypoints to build the point to line residuals
    for (auto k : KeypointTypes)
      this->LocalizationMatchingResults[k] = matcher.BuildMatchResiduals(this->CurrentUndistortedKeypoints[k], this->LocalMaps[k]->GetSubMapKdTree(), k);

    // Count matches and skip this frame
    // if there is too few geometric keypoints matched
    this->TotalMatchedKeypoints = 0;
    for (auto k : KeypointTypes)
      this->TotalMatchedKeypoints += this->LocalizationMatchingResults[k].NbMatches();

    if (this->TotalMatchedKeypoints < this->MinNbMatchedKeypoints)
    {
      this->Tworld = Eigen::Isometry3d::Identity();
      LidarState lastState;
      if (this->LogStates.Back(lastState))
        this->Tworld = lastState.Isometry;

      if (this->Undistortion && this->TimeDependentProcessesEnabled)
        this->WithinFrameMotion.SetTransforms(Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity());
      PRINT_ERROR("Not enough keypoints matched, Localization skipped for this frame.");
      break;
    }

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("  Localization : ICP"));
    IF_VERBOSE(3, Utils::Timer::Init("  Localization : LM optim"));

    // Init the optimizer with initial pose and parameters
    LocalOptimizer optimizer;
    optimizer.SetTwoDMode(this->TwoDMode);
    optimizer.SetPosePrior(this->Tworld);
    optimizer.SetLMMaxIter(this->LocalizationLMMaxIter);
    optimizer.SetNbThreads(this->NbThreads);

    // Add LiDAR ICP matches
    for (auto k : KeypointTypes)
      optimizer.AddResiduals(this->LocalizationMatchingResults[k].Residuals);

    // Add odometry constraint
    // if constraint has been successfully created
    if (this->WheelOdomManager.GetResidual().Cost)
      optimizer.AddResidual(this->WheelOdomManager.GetResidual());

    // Add gravity alignment constraint
    // if constraint has been successfully created
    if (this->ImuManager.GetResidual().Cost)
      optimizer.AddResidual(this->ImuManager.GetResidual());

    // Run LM optimization
    ceres::Solver::Summary summary = optimizer.Solve();
    PRINT_VERBOSE(4, summary.BriefReport());

    // Update Tworld from optimization results
    this->Tworld = optimizer.GetOptimizedPose();

    // Optionally refine undistortion
    if (this->Undistortion == UndistortionMode::REFINED && this->TimeDependentProcessesEnabled)
      this->RefineUndistortion();

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("  Localization : LM optim"));

    // If no L-M iteration has been made since the last ICP matching, it means
    // that we reached a local minimum for the ICP-LM algorithm.
    // We evaluate the quality of the Tworld optimization using an approximate
    // computation of the variance covariance matrix.
    if ((summary.num_successful_steps == 1) || (icpIter == this->LocalizationICPMaxIter - 1))
    {
      this->LocalizationUncertainty = optimizer.EstimateRegistrationError();
      break;
    }
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization : whole ICP-LM loop"));

  // Optionally print localization optimization summary
  if (this->Verbosity >= 2)
  {
    SET_COUT_FIXED_PRECISION(3);
    std::cout << "Matched keypoints: " << this->TotalMatchedKeypoints << " (";
    for (auto k : KeypointTypes)
      std::cout << this->LocalizationMatchingResults[k].NbMatches() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
    std::cout << ")"
              << "\nPosition uncertainty    = " << this->LocalizationUncertainty.PositionError    << " m"
              << " (along [" << this->LocalizationUncertainty.PositionErrorDirection.transpose()    << "])"
              << "\nOrientation uncertainty = " << this->LocalizationUncertainty.OrientationError << " °"
              << " (along [" << this->LocalizationUncertainty.OrientationErrorDirection.transpose() << "])"
              << std::endl;
    RESET_COUT_FIXED_PRECISION;
  }
}

//-----------------------------------------------------------------------------
void Slam::UpdateMapsUsingTworld()
{
  // Compute motion since last keyframe
  Eigen::Isometry3d motionSinceLastKf = this->KfLastPose.inverse() * this->Tworld;
  double transSinceLastKf = motionSinceLastKf.translation().norm();
  double rotSinceLastKf = Eigen::AngleAxisd(motionSinceLastKf.linear()).angle();
  PRINT_VERBOSE(3, "Motion since last keyframe " << this->KfCounter << ": "
                                                 << transSinceLastKf << " m, "
                                                 << Utils::Rad2Deg(rotSinceLastKf) << " °");

  // Check if current frame is a new keyframe
  // If we don't have enough keyframes yet, the threshold is linearly lowered
  constexpr double MIN_KF_NB = 10.;
  double thresholdCoef = std::min(this->KfCounter / MIN_KF_NB, 1.);
  unsigned int nbMapKpts = 0;
  for (const auto& mapKptsCloud : this->LocalMaps)
    nbMapKpts += mapKptsCloud.second->Size();
  bool isNewKeyFrame = nbMapKpts < this->MinNbMatchedKeypoints * 10 ||
                       transSinceLastKf >= thresholdCoef * this->KfDistanceThreshold ||
                       rotSinceLastKf >= Utils::Deg2Rad(thresholdCoef * this->KfAngleThreshold);
  if (!isNewKeyFrame)
    return;

  // Notify current frame to be a new keyframe
  this->KfCounter++;
  this->KfLastPose = this->Tworld;
  PRINT_VERBOSE(3, "Adding new keyframe " << this->KfCounter);

  // Transform keypoints to WORLD coordinates
  for (auto k : KeypointTypes)
    this->CurrentWorldKeypoints[k] = this->GetKeypoints(k, true);

  // Add registered points to map
  // The iteration is not directly on Keypoint types
  // because of openMP behaviour which needs int iteration on MSVC
  int nbKeypointTypes = static_cast<int>(KeypointTypes.size());
  #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
  for (int i = 0; i < nbKeypointTypes; ++i)
  {
    Keypoint k = static_cast<Keypoint>(KeypointTypes[i]);
    if (this->UseKeypoints[k])
      this->LocalMaps[k]->Add(this->CurrentWorldKeypoints[k]);
  }
}

//-----------------------------------------------------------------------------
void Slam::LogCurrentFrameState(double time)
{
  // Save current state to log buffer.
  // This buffer will be processed by a global optimization thread
  LidarState state;
  state.Isometry = this->Tworld;
  state.Covariance = Utils::Matrix6dToStdArray36(this->LocalizationUncertainty.Covariance);
  state.Time = time;
  state.ValidTime = this->TimeDependentProcessesEnabled;
  for (auto k : KeypointTypes)
    state.Keypoints[k] = std::make_shared<PCStorage>(this->CurrentUndistortedKeypoints[k], this->LoggingStorage);
  this->LogStates.EmplaceBack(state);

  // Save current data to publishable queue.
  // This buffer should be processed by an external publisher thread
  Publishable toPublish;
  // Light data
  toPublish.State = state;
  toPublish.IdxFrame = this->NbrFrameProcessed;
  if (OutputRequirement[EDGES_MAP])
    toPublish.Maps[EDGE]  =  std::make_shared<PCStorage>(this->LocalMaps[EDGE]->Get(), this->LoggingStorage);
  if (OutputRequirement[PLANES_MAP])
    toPublish.Maps[PLANE] =  std::make_shared<PCStorage>(this->LocalMaps[PLANE]->Get(), this->LoggingStorage);
  if (OutputRequirement[BLOBS_MAP])
    toPublish.Maps[BLOB]  =  std::make_shared<PCStorage>(this->LocalMaps[BLOB]->Get(), this->LoggingStorage);
  
  if (OutputRequirement[EDGE_KEYPOINTS])
    toPublish.KeypointsWorld[EDGE]  =  std::make_shared<PCStorage>(this->GetKeypoints(EDGE, true), this->LoggingStorage);
  if (OutputRequirement[PLANE_KEYPOINTS])
    toPublish.KeypointsWorld[PLANE] =  std::make_shared<PCStorage>(this->GetKeypoints(PLANE, true), this->LoggingStorage);
  if (OutputRequirement[BLOB_KEYPOINTS])
    toPublish.KeypointsWorld[BLOB]  =  std::make_shared<PCStorage>(this->GetKeypoints(BLOB, true), this->LoggingStorage);

  // Heavier data
  if (OutputRequirement[SLAM_REGISTERED_POINTS])
    toPublish.RegisteredFrame = std::make_shared<PCStorage>(this->GetRegisteredFrame(), this->LoggingStorage);
  if (OutputRequirement[DEBUG_ARRAYS])
    toPublish.DebugArray = this->GetDebugArray();
  if (OutputRequirement[KE_DEBUG_ARRAYS])
    toPublish.KEDebugArray = this->GetKeyPointsExtractor()->GetDebugArray();
  if (OutputRequirement[POSE_PREDICTION_ODOM] || OutputRequirement[POSE_PREDICTION_TF])
    toPublish.LatencyCorrectedIsometry = this->GetLatencyCompensatedWorldTransform();
  if (OutputRequirement[CONFIDENCE])
  {
    toPublish.DebugInformation = this->GetDebugInformation();
    toPublish.Overlap = this->OverlapEstimation;
    toPublish.ComplyMotionLimits = this->ComplyMotionLimits;
    toPublish.NbMatchedKeypoints = this->TotalMatchedKeypoints;
  }

  this->OutputResults.EmplaceBack(toPublish);
}

//==============================================================================
//   Undistortion helpers
//==============================================================================

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::InterpolateScanPose(double time)
{
  LidarState lastState;
  if (!this->LogStates.Back(lastState))
    return this->Tworld;

  const double prevPoseTime = lastState.Time;
  const double currPoseTime = Utils::PclStampToSec(this->CurrentFrames[0]->header.stamp);
  if (std::abs(time / (currPoseTime - prevPoseTime)) > this->MaxExtrapolationRatio)
  {
    PRINT_WARNING("Unable to interpolate scan pose from motion : extrapolation time is too far.");
    return this->Tworld;
  }

  return LinearInterpolation(lastState.Isometry, this->Tworld, currPoseTime + time, prevPoseTime, currPoseTime);
}

//-----------------------------------------------------------------------------
void Slam::InitUndistortion()
{
  // Get 'time' field range
  double frameFirstTime = std::numeric_limits<double>::max();
  double frameLastTime  = std::numeric_limits<double>::lowest();
  for (auto k : KeypointTypes)
  {
    for (const auto& point : *this->CurrentUndistortedKeypoints[k])
    {
      frameFirstTime = std::min(frameFirstTime, point.time);
      frameLastTime  = std::max(frameLastTime, point.time);
    }
  }

  // Update interpolator timestamps and reset transforms
  this->WithinFrameMotion.SetTimes(frameFirstTime, frameLastTime);
  this->WithinFrameMotion.SetTransforms(Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity());

  // Check time values
  if (this->WithinFrameMotion.GetTimeRange() < 1e-6)
  {
    // If frame duration is 0, it means that the time field is constant and cannot be used.
    // We reset timestamps to 0, to ensure no time offset will be used.
    PRINT_WARNING("'time' field is not properly set (constant value) and cannot be used for undistortion.");
    this->WithinFrameMotion.SetTimes(0., 0.);
  }
  else if (this->WithinFrameMotion.GetTimeRange() > 10.)
  {
    // If frame duration is bigger than 10 seconds, it is probably wrongly set
    PRINT_WARNING("'time' field looks not properly set (frame duration > 10 s) and can lead to faulty undistortion.");
  }
}

//-----------------------------------------------------------------------------
void Slam::RefineUndistortion()
{
  // Get previously applied undistortion
  Eigen::Isometry3d previousBaseBegin = this->WithinFrameMotion.GetH0();
  Eigen::Isometry3d previousBaseEnd = this->WithinFrameMotion.GetH1();

  // Extrapolate first and last poses to update within frame motion interpolator
  Eigen::Isometry3d worldToBaseBegin = this->InterpolateScanPose(this->WithinFrameMotion.GetTime0());
  Eigen::Isometry3d worldToBaseEnd = this->InterpolateScanPose(this->WithinFrameMotion.GetTime1());
  Eigen::Isometry3d baseToWorld = this->Tworld.inverse();
  Eigen::Isometry3d newBaseBegin = baseToWorld * worldToBaseBegin;
  Eigen::Isometry3d newBaseEnd = baseToWorld * worldToBaseEnd;
  this->WithinFrameMotion.SetTransforms(newBaseBegin, newBaseEnd);

  // Init the interpolator to use to remove previous undistortion and apply updated one
  auto transformInterpolator = this->WithinFrameMotion;
  transformInterpolator.SetTransforms(newBaseBegin * previousBaseBegin.inverse(),
                                      newBaseEnd   * previousBaseEnd.inverse());

  // Refine undistortion of keypoints clouds
  for (auto k : KeypointTypes)
  {
    int nbPoints = this->CurrentUndistortedKeypoints[k]->size();
    #pragma omp parallel for num_threads(this->NbThreads)
    for (int i = 0; i < nbPoints; ++i)
    {
      auto& point = this->CurrentUndistortedKeypoints[k]->at(i);
      Utils::TransformPoint(point, transformInterpolator(point.time));
    }
  }
}

//==============================================================================
//   Confidence estimators
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::SetOverlapSamplingRatio (float _arg)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  // Clamp ratio beteen 0 and 1
  this->OverlapSamplingRatio = Utils::Clamp(_arg, 0.f, 1.f);

  // Reset overlap value if overlap computation is disabled
  if (this->OverlapSamplingRatio == 0.)
    this->OverlapEstimation = -1.f;
}

//-----------------------------------------------------------------------------
void Slam::EstimateOverlap()
{
  // Aggregate all input points into WORLD coordinates
  PointCloud::Ptr aggregatedPoints = this->GetRegisteredFrame();

  // Keep only the maps to use
  std::map<Keypoint, std::shared_ptr<RollingGrid>> mapsToUse;
  for (auto k : KeypointTypes)
  {
    if (this->UseKeypoints[k] && this->LocalMaps[k]->IsSubMapKdTreeValid())
      mapsToUse[k] = this->LocalMaps[k];
  }

  // Compute LCP like estimator
  // (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info)
  this->OverlapEstimation = Confidence::LCPEstimator(aggregatedPoints, mapsToUse, this->OverlapSamplingRatio, this->NbThreads);
  PRINT_VERBOSE(3, "Overlap : " << this->OverlapEstimation << ", estimated on : "
                                << static_cast<int>(aggregatedPoints->size() * this->OverlapSamplingRatio) << " points.");
}

//-----------------------------------------------------------------------------
void Slam::CheckMotionLimits()
{
  std::vector<LidarState> lastStates = this->LogStates.LastElements(this->WindowWidth - 1);
  // If the window's width required cannot be reached, take the last available pose
  int windowWidth = lastStates.size();

  if (windowWidth == 0)
    return;

  for (int stateIdx = 0; stateIdx < windowWidth; ++stateIdx)
  {
    if (!lastStates[stateIdx].ValidTime)
    {
      PRINT_WARNING("Times not valid in window, skipping the motion checks.")
      return;
    }
  }

  // If the number of logged poses is lower than required,
  // A warning is raised because it can come from a bad parameterization.
  // This warning will always be raised for first poses.
  if (windowWidth + 1 != this->WindowWidth)
    PRINT_WARNING("Not enough logged poses, using " << windowWidth + 1 << " poses to compute velocity.")

  // Get the starting bound state
  LidarState windowStartBound = this->LogStates[0];
  // Compute duration time between the two pose bounds of the window
  double currentTimeStamp = Utils::PclStampToSec(this->CurrentFrames[0]->header.stamp);
  double deltaTime = currentTimeStamp - windowStartBound.Time;
  // Compute transform between the two pose bounds of the window
  Eigen::Isometry3d TWindow = windowStartBound.Isometry.inverse() * this->Tworld;

  // Initialize confidence estimator
  this->ComplyMotionLimits = true;

  // Compute angular part
  // NOTE : It is not possible to detect an angle greater than PI,
  // the detectable velocity and acceleration are limited on deltaTime
  // Rotation angle in [0, 2pi]
  float angle = Eigen::AngleAxisd(TWindow.linear()).angle();
  // Rotation angle in [0, pi]
  if (angle > M_PI)
    angle = 2 * M_PI - angle;
  angle = Utils::Rad2Deg(angle);
  // Compute linear part
  float distance = TWindow.translation().norm();

  // Compute velocity
  Eigen::Array2f velocity = {distance / deltaTime, angle / deltaTime};
  SET_COUT_FIXED_PRECISION(3);
  // Print local velocity
  PRINT_VERBOSE(3, "Velocity     = " << std::setw(6) << velocity[0] << " m/s,  "
                                     << std::setw(6) << velocity[1] << " °/s")
  RESET_COUT_FIXED_PRECISION;

  if (this->NbrFrameProcessed >= 2)
  {
    // Compute local acceleration in BASE
    Eigen::Array2f acceleration = (velocity - this->PreviousVelocity) / deltaTime;
    // Print local acceleration
    SET_COUT_FIXED_PRECISION(3);
    PRINT_VERBOSE(3, "Acceleration = " << std::setw(6) << acceleration[0] << " m/s2, "
                                       << std::setw(6) << acceleration[1] << " °/s2")
    RESET_COUT_FIXED_PRECISION;

    // Check velocity compliance
    bool complyVelocityLimits = (velocity < this->VelocityLimits).all();
    // Check acceleration compliance
    bool complyAccelerationLimits = (acceleration.abs() < this->AccelerationLimits).all();

    // Set ComplyMotionLimits
    this->ComplyMotionLimits = complyVelocityLimits && complyAccelerationLimits;
  }

  this->PreviousVelocity = velocity;

  if (!this->ComplyMotionLimits)
    PRINT_WARNING("The pose does not comply with the motion limitations. Lidar SLAM may have failed.")
}

//==============================================================================
//   Transformation helpers
//==============================================================================

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::TransformPointCloud(PointCloud::ConstPtr cloud,
                                                const Eigen::Isometry3d& tf,
                                                const std::string& frameId) const
{
  // Copy all fields and set frame ID
  PointCloud::Ptr transformedCloud(new PointCloud);
  pcl::copyPointCloud(*cloud, *transformedCloud);
  transformedCloud->header.frame_id = frameId;

  // Transform each point inplace in parallel
  int nbPoints = transformedCloud->size();
  #pragma omp parallel for num_threads(this->NbThreads)
  for (int i = 0; i < nbPoints; ++i)
  {
    auto& point = transformedCloud->at(i);
    Utils::TransformPoint(point, tf);
  }
  return transformedCloud;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::AggregateFrames(const std::vector<PointCloud::Ptr>& frames, bool worldCoordinates) const
{
  PointCloud::Ptr aggregatedFrames(new PointCloud);
  aggregatedFrames->header = Utils::BuildPclHeader(this->CurrentFrames[0]->header.stamp,
                                                   worldCoordinates ? this->WorldFrameId : this->BaseFrameId,
                                                   this->NbrFrameProcessed);

  // Loop over frames of input
  for (const auto& frame: frames)
  {
    // If the frame is empty, ignore it
    if (frame->empty())
      continue;

    // Add frame to aggregated output
    int startIdx = aggregatedFrames->size();
    int endIdx = startIdx + frame->size();
    *aggregatedFrames += *frame;

    // Modify point-wise time offsets to match header.stamp
    // And transform points from LIDAR to BASE or WORLD coordinate system
    double timeOffset = Utils::PclStampToSec(frame->header.stamp) - Utils::PclStampToSec(aggregatedFrames->header.stamp);
    Eigen::Isometry3d baseToLidar = this->GetBaseToLidarOffset(frame->front().device_id);

    // Rigid transform from LIDAR to BASE then undistortion from BASE to WORLD
    if (worldCoordinates && this->Undistortion && this->TimeDependentProcessesEnabled)
    {
      auto transformInterpolator = this->WithinFrameMotion;
      transformInterpolator.SetTransforms(this->Tworld * this->WithinFrameMotion.GetH0() * baseToLidar,
                                          this->Tworld * this->WithinFrameMotion.GetH1() * baseToLidar);
      #pragma omp parallel for num_threads(this->NbThreads)
      for (int i = startIdx; i < endIdx; ++i)
      {
        auto& point = aggregatedFrames->at(i);
        point.time += timeOffset;
        Utils::TransformPoint(point, transformInterpolator(point.time));
      }
    }

    // Rigid transform from LIDAR to BASE or WORLD coordinate system
    else
    {
      // Get rigid transform to apply
      Eigen::Isometry3d tf = worldCoordinates ? this->Tworld * baseToLidar : baseToLidar;
      // If transform to apply is identity, avoid much work
      if (tf.isApprox(Eigen::Isometry3d::Identity()))
      {
        #pragma omp parallel for num_threads(this->NbThreads)
        for (int i = startIdx; i < endIdx; ++i)
          aggregatedFrames->at(i).time += timeOffset;
      }
      // If transform is non trivial, run transformation
      else
      {
        #pragma omp parallel for num_threads(this->NbThreads)
        for (int i = startIdx; i < endIdx; ++i)
        {
          auto& point = aggregatedFrames->at(i);
          point.time += timeOffset;
          Utils::TransformPoint(point, tf);
        }
      }
    }
  }

  return aggregatedFrames;
}

//==============================================================================
//   Sensor managers setting
//==============================================================================

// Sensor parameters

//-----------------------------------------------------------------------------
void Slam::SetWheelOdomWeight(double weight)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->WheelOdomManager.SetWeight(weight);
} 

//-----------------------------------------------------------------------------
double Slam::GetWheelOdomWeight() const
{
  std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  return this->WheelOdomManager.GetWeight();
}

//-----------------------------------------------------------------------------
void Slam::SetGravityWeight(double weight)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->ImuManager.SetWeight(weight);
}

//-----------------------------------------------------------------------------
double Slam::GetGravityWeight() const
{
  std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  return this->ImuManager.GetWeight();
}

//-----------------------------------------------------------------------------
void Slam::SetSensorTimeOffset(double timeOffset) 
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->WheelOdomManager.SetTimeOffset(timeOffset);
  this->ImuManager.SetTimeOffset(timeOffset);
}

//-----------------------------------------------------------------------------
double Slam::GetSensorTimeOffset() const
{
  std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  return this->ImuManager.GetTimeOffset();
}

// Sensor data

//-----------------------------------------------------------------------------
void Slam::AddGravityMeasurement(const SensorConstraints::GravityMeasurement& gm)
{
  // This function will lock the measurements
  // and block the sensor constraints computation in front end
  this->ImuManager.AddMeasurement(gm);
}

void Slam::AddWheelOdomMeasurement(const SensorConstraints::WheelOdomMeasurement& om)
{
  // This function will lock the measurements
  // and block the sensor constraints computation in front end
  this->WheelOdomManager.AddMeasurement(om);
}

void Slam::ClearSensorMeasurements()
{
  // This function will lock the measurements
  // and block the sensor constraints computation in front end
  this->WheelOdomManager.Reset();
  this->ImuManager.Reset();
}

//==============================================================================
//   Keypoints extraction parameters setting
//==============================================================================

//-----------------------------------------------------------------------------
std::map<uint8_t, Slam::KeypointExtractorPtr> Slam::GetKeyPointsExtractors() const
{
  std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  return this->KeyPointsExtractors;
}
void Slam::SetKeyPointsExtractors(const std::map<uint8_t, KeypointExtractorPtr>& extractors)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->KeyPointsExtractors = extractors;
}

//-----------------------------------------------------------------------------
Slam::KeypointExtractorPtr Slam::GetKeyPointsExtractor(uint8_t deviceId) const
{
  std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  return this->KeyPointsExtractors.count(deviceId) ? this->KeyPointsExtractors.at(deviceId) : KeypointExtractorPtr();
}
void Slam::SetKeyPointsExtractor(KeypointExtractorPtr extractor, uint8_t deviceId)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->KeyPointsExtractors[deviceId] = extractor;
}

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::GetBaseToLidarOffset(uint8_t deviceId) const
{
  std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  return this->BaseToLidarOffsets.count(deviceId) ? this->BaseToLidarOffsets.at(deviceId) : Eigen::UnalignedIsometry3d::Identity();
}
void Slam::SetBaseToLidarOffset(const Eigen::Isometry3d& transform, uint8_t deviceId)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->BaseToLidarOffsets[deviceId] = transform;
}

//==============================================================================
//   Rolling grids parameters setting
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::ClearMaps()
{
  for (auto k : KeypointTypes)
    this->LocalMaps[k]->Reset();
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridLeafSize(Keypoint k, double size)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->LocalMaps[k]->SetLeafSize(size);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridSize(int size)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  for (auto k : KeypointTypes)
    this->LocalMaps[k]->SetGridSize(size);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridResolution(double resolution)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  for (auto k : KeypointTypes)
    this->LocalMaps[k]->SetVoxelResolution(resolution);
}

//==============================================================================
//   Memory parameters setting
//==============================================================================

//-----------------------------------------------------------------------------
// The minimum logging size is 2, to be able to handle ego-motion and undistortion
void Slam::SetLoggingMax(int lMax)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->LogStates.SetSizeMax(std::max(2, lMax));
}

//-----------------------------------------------------------------------------
int Slam::GetLoggingMax() const
{
  return this->LogStates.GetSizeMax();
}

//==============================================================================
//   Outputs setting
//==============================================================================

void Slam::SetOutputRequirement(const std::unordered_map<Output, bool, std::hash<int>>& required)
{
  std::unique_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  this->OutputRequirement = required;
}

std::unordered_map<Output, bool, std::hash<int>> Slam::GetOutputRequirement() const
{
  std::shared_lock<std::shared_timed_mutex> lock(this->ParamsMutex);
  return this->OutputRequirement;
}


} // end of LidarSlam namespace