//==============================================================================
// 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"
#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>
// 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
{

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

//-----------------------------------------------------------------------------
//! Approximate logged keypoints size
void LoggedKeypointsSize(const std::deque<PointCloudStorage<Slam::Point>>& log, size_t& totalMemory, size_t& totalPoints)
{
  totalMemory = 0;
  totalPoints = 0;
  for (auto const& storage: log)
  {
    totalPoints += storage.PointsSize();
    totalMemory += storage.MemorySize();
  }
}
} // end of anonymous namespace
} // end of Utils namespace

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

//-----------------------------------------------------------------------------
Slam::Slam()
{
  // Allocate Keypoints Extractor
  this->KeyPointsExtractor = std::make_shared<SpinningSensorKeypointExtractor>();

  // Allocate maps
  this->EdgesPointsLocalMap = std::make_shared<RollingGrid>();
  this->PlanarPointsLocalMap = std::make_shared<RollingGrid>();
  this->BlobsPointsLocalMap = std::make_shared<RollingGrid>();

  // Set default maps parameters
  this->SetVoxelGridResolution(10.);
  this->SetVoxelGridSize(50);
  this->SetVoxelGridLeafSizeEdges(0.30);
  this->SetVoxelGridLeafSizePlanes(0.60);
  this->SetVoxelGridLeafSizeBlobs(0.30);

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

//-----------------------------------------------------------------------------
void Slam::Reset(bool resetLog)
{
  // Reset keypoints maps
  this->ClearMaps();

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

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

  // Reset point clouds
  this->CurrentFrame.reset(new PointCloud);
  this->CurrentEdgesPoints.reset(new PointCloud);
  this->CurrentPlanarsPoints.reset(new PointCloud);
  this->CurrentBlobsPoints.reset(new PointCloud);
  this->CurrentWorldEdgesPoints.reset(new PointCloud);
  this->CurrentWorldPlanarsPoints.reset(new PointCloud);
  this->CurrentWorldBlobsPoints.reset(new PointCloud);

  // Reset keypoints matching results
  this->EgoMotionMatchingResults[EDGE] = KeypointsRegistration::MatchingResults();
  this->EgoMotionMatchingResults[PLANE] = KeypointsRegistration::MatchingResults();
  this->LocalizationMatchingResults[EDGE] = KeypointsRegistration::MatchingResults();
  this->LocalizationMatchingResults[PLANE] = KeypointsRegistration::MatchingResults();
  this->LocalizationMatchingResults[BLOB] = KeypointsRegistration::MatchingResults();

  // Reset log history
  if (resetLog)
  {
    this->NbrFrameProcessed = 0;
    this->LogTrajectory.clear();
    this->LogEdgesPoints.clear();
    this->LogPlanarsPoints.clear();
    this->LogBlobsPoints.clear();
  }

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

//-----------------------------------------------------------------------------
void Slam::AddFrame(const PointCloud::Ptr& pc, const std::vector<size_t>& laserIdMapping)
{
  Utils::Timer::Init("SLAM frame processing");

  // Skip frame if empty
  if (pc->empty())
  {
    PRINT_ERROR("SLAM entry is an empty pointcloud : frame ignored.");
    return;
  }

  // Skip frame if it has the same timestamp as previous one (will induce problems in extrapolation)
  if (pc->header.stamp == this->CurrentFrame->header.stamp)
  {
    PRINT_ERROR("SLAM entry has the same timestamp (" << pc->header.stamp << ") as previous pointcloud : frame ignored.");
    return;
  }

  PRINT_VERBOSE(2, "\n#########################################################");
  PRINT_VERBOSE(1, "Processing frame " << this->NbrFrameProcessed);
  PRINT_VERBOSE(2, "#########################################################\n");

  // Update current frame (check frame dropping, correct time field) and
  // estimate new state (extrapolate new pose with a constant velocity model)
  IF_VERBOSE(3, Utils::Timer::Init("Update frame and state"));
  this->UpdateFrameAndState(pc);
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Update frame and state"));

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

  // If the new frame is the first one we just add the extracted keypoints into
  // the map without running odometry and localization steps
  if (this->NbrFrameProcessed > 0)
  {
    // Compute Trelative by registering current frame on previous one
    if (this->EgoMotion == EgoMotionMode::REGISTRATION ||
        this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION_AND_REGISTRATION)
    {
      IF_VERBOSE(3, Utils::Timer::Init("Ego-Motion"));
      this->ComputeEgoMotion();
      IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Ego-Motion"));
    }

    // Integrate the relative motion to the world transformation
    this->Tworld = this->PreviousTworld * this->Trelative;
    if (this->Undistortion)
    {
      // Use extrapolated motion to initialize keypoints undistortion.
      // (All keypoints will be transformed to the BASE referential at the end
      // of frame acquisition)
      this->TworldFrameStart = this->InterpolateBeginScanPose();
      this->WithinFrameMotion.SetTransforms(this->TworldFrameStart, this->Tworld);
    }

    // Perform Localization : compute Tworld from map and current frame keypoints
    IF_VERBOSE(3, Utils::Timer::Init("Localization"));
    this->Localization();
    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization"));
  }

  // 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(this->CurrentFrame->header.stamp), this->WorldFrameId);
  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"
                 "Ego-Motion:\n"
                 " translation = [" << this->Trelative.translation().transpose()                                        << "]\n"
                 " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(this->Trelative.linear())).transpose() << "]\n";
    if (this->Undistortion)
    {
      Eigen::Isometry3d motion = this->TworldFrameStart.inverse() * this->Tworld;
      std::cout << "Within frame motion:\n"
                   " translation = [" << motion.translation().transpose()                                        << "]\n"
                   " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(motion.linear())).transpose() << "]\n";
    }
    std::cout << "Localization:\n"
                 " position    = [" << this->Tworld.translation().transpose()                                        << "]\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";
    // SLAM maps
    PointCloud::Ptr edgesMap   = this->GetEdgesMap(),
                    planarsMap = this->GetPlanarsMap(),
                    blobsMap   = this->GetBlobsMap();
    std::cout << "Edges map   : " << edgesMap->size()   << " points, " << Utils::PointCloudMemorySize(*edgesMap)   * 1e-6 << " MB\n";
    std::cout << "Planars map : " << planarsMap->size() << " points, " << Utils::PointCloudMemorySize(*planarsMap) * 1e-6 << " MB\n";
    std::cout << "Blobs map   : " << blobsMap->size()   << " points, " << Utils::PointCloudMemorySize(*blobsMap)   * 1e-6 << " MB\n";

    // Logged keypoints
    size_t memory, points;
    Utils::LoggedKeypointsSize(this->LogEdgesPoints, memory, points);
    std::cout << "Edges log   : " << this->LogEdgesPoints.size()   << " frames, " << points << " points, " << memory * 1e-6 << " MB\n";
    Utils::LoggedKeypointsSize(this->LogPlanarsPoints, memory, points);
    std::cout << "Planars log : " << this->LogPlanarsPoints.size() << " frames, " << points << " points, " << memory * 1e-6 << " MB\n";
    Utils::LoggedKeypointsSize(this->LogBlobsPoints, memory, points);
    std::cout << "Blobs log   : " << this->LogBlobsPoints.size()   << " frames, " << points << " points, " << memory * 1e-6 << " MB" << std::endl;
    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"));
}

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

  // Transform to modifiable vectors
  std::vector<Transform> slamPoses(this->LogTrajectory.begin(), this->LogTrajectory.end());
  std::vector<std::array<double, 36>> slamCovariances(this->LogCovariances.begin(), this->LogCovariances.end());

  const unsigned int nbSlamPoses = slamPoses.size();

  if (this->LoggingTimeout == 0.)
  {
    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"));
  PointCloud edgesKeypoints, planarsKeypoints, blobsKeypoints;
  PointCloud::Ptr aggregatedEdgesMap(new PointCloud),
                  aggregatedPlanarsMap(new PointCloud),
                  aggregatedBlobsMap(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
    Eigen::Matrix4d currentTransform = this->LogTrajectory[i].GetMatrix();

    pcl::transformPointCloud(*this->LogEdgesPoints[i].GetCloud(), edgesKeypoints, currentTransform);
    pcl::transformPointCloud(*this->LogPlanarsPoints[i].GetCloud(), planarsKeypoints, currentTransform);
    if (!this->FastSlam)
      pcl::transformPointCloud(*this->LogBlobsPoints[i].GetCloud(), blobsKeypoints, currentTransform);

    // TODO: Deal with undistortion case (properly transform pointclouds before aggreagtion)

    // Aggregate new keypoints to maps
    *aggregatedEdgesMap += edgesKeypoints;
    *aggregatedPlanarsMap += planarsKeypoints;
    if (!this->FastSlam)
      *aggregatedBlobsMap += blobsKeypoints;
  }

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

  // TODO : Deal with undistortion case (update motionParameters)

  // Update SLAM maps
  auto updateMap = [&](RollingGrid& map, const PointCloud& lastPoints, const PointCloud::Ptr& aggregatedPoints)
  {
    // 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.
    Eigen::Vector4f minPoint, maxPoint;
    pcl::getMinMax3D(lastPoints, minPoint, maxPoint);
    map.Roll(minPoint.head<3>().cast<double>().array(), maxPoint.head<3>().cast<double>().array());
    map.Add(aggregatedPoints, false);
  };
  updateMap(*this->EdgesPointsLocalMap, edgesKeypoints, aggregatedEdgesMap);
  updateMap(*this->PlanarPointsLocalMap, planarsKeypoints, aggregatedPlanarsMap);
  if (!this->FastSlam)
    updateMap(*this->BlobsPointsLocalMap, blobsKeypoints, aggregatedBlobsMap);

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

//-----------------------------------------------------------------------------
void Slam::SetWorldTransformFromGuess(const Transform& poseGuess)
{
  // Set current pose
  this->Tworld = poseGuess.GetIsometry();
  // TODO update motionParameters

  // Ego-Motion estimation is not valid anymore since we imposed a discontinuity.
  // We reset previous pose so that previous ego-motion extrapolation results in Identity matrix.
  // We reset current frame keypoints so that ego-motion registration will be skipped for next frame.
  this->PreviousTworld = this->Tworld;
  this->CurrentEdgesPoints.reset(new PointCloud);
  this->CurrentPlanarsPoints.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
  savePointCloudToPCD(filePrefix + "edges.pcd",   *this->GetEdgesMap(), pcdFormat, true);
  savePointCloudToPCD(filePrefix + "planars.pcd", *this->GetPlanarsMap(), pcdFormat, true);
  savePointCloudToPCD(filePrefix + "blobs.pcd",   *this->GetBlobsMap(), 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)
{
  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();

  auto loadMapFromPCD = [](const std::string& path, std::shared_ptr<RollingGrid>& map)
  {
    PointCloud::Ptr keypoints(new PointCloud);
    if (pcl::io::loadPCDFile(path, *keypoints) == 0)
    {
      std::cout << "SLAM keypoints map successfully loaded from " << path << std::endl;
      map->Add(keypoints);
    }
  };

  loadMapFromPCD(filePrefix + "edges.pcd",   this->EdgesPointsLocalMap);
  loadMapFromPCD(filePrefix + "planars.pcd", this->PlanarPointsLocalMap);
  loadMapFromPCD(filePrefix + "blobs.pcd",   this->BlobsPointsLocalMap);

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

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Keypoints maps loading from PCD"));
}

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

//-----------------------------------------------------------------------------
Transform Slam::GetWorldTransform() const
{
  return this->LogTrajectory.empty() ? Transform::Identity() : this->LogTrajectory.back();
}

//-----------------------------------------------------------------------------
Transform Slam::GetLatencyCompensatedWorldTransform() const
{
  constexpr double MAX_EXTRAPOLATION_RATIO = 3.;

  // Get 2 last transforms
  unsigned int trajectorySize = this->LogTrajectory.size();
  if (trajectorySize == 0)
    return Transform::Identity();
  else if (trajectorySize == 1)
    return this->LogTrajectory.back();
  const Transform& previous = this->LogTrajectory[trajectorySize - 2];
  const Transform& current = this->LogTrajectory[trajectorySize - 1];
  const Eigen::Isometry3d& H0 = previous.GetIsometry();
  const Eigen::Isometry3d& H1 = current.GetIsometry();

  // 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;
  }
  // If requested extrapolation timestamp is too far from previous frames timestamps, extrapolation is impossible.
  if (std::abs(this->Latency / (current.time - previous.time)) > MAX_EXTRAPOLATION_RATIO)
  {
    PRINT_WARNING("Unable to compute latency-compensated transform : extrapolation time is too far.");
    return current;
  }

  // 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 Transform(Hpred, current.time, current.frameid);
}

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

//-----------------------------------------------------------------------------
std::vector<Transform> Slam::GetTrajectory() const
{
  std::vector<Transform> slamPoses(this->LogTrajectory.begin(), this->LogTrajectory.end());
  return slamPoses;
}

//-----------------------------------------------------------------------------
std::vector<std::array<double, 36>> Slam::GetCovariances() const
{
  std::vector<std::array<double, 36>> slamCovariances(this->LogCovariances.begin(), this->LogCovariances.end());
  return slamCovariances;
}

//-----------------------------------------------------------------------------
std::unordered_map<std::string, double> Slam::GetDebugInformation() const
{
  std::unordered_map<std::string, double> map;
  map["EgoMotion: edges used"]           = this->EgoMotionMatchingResults.at(EDGE).NbMatches();
  map["EgoMotion: planes used"]          = this->EgoMotionMatchingResults.at(PLANE).NbMatches();
  map["Localization: edges used"]        = this->LocalizationMatchingResults.at(EDGE).NbMatches();
  map["Localization: planes used"]       = this->LocalizationMatchingResults.at(PLANE).NbMatches();
  map["Localization: blobs used"]        = this->LocalizationMatchingResults.at(BLOB).NbMatches();
  map["Localization: position error"]    = this->LocalizationUncertainty.PositionError;
  map["Localization: orientation error"] = this->LocalizationUncertainty.OrientationError;
  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;
  map["EgoMotion: edges matches"]  = toDoubleVector(this->EgoMotionMatchingResults.at(EDGE).Rejections);
  map["EgoMotion: planes matches"] = toDoubleVector(this->EgoMotionMatchingResults.at(PLANE).Rejections);
  map["Localization: edges matches"]  = toDoubleVector(this->LocalizationMatchingResults.at(EDGE).Rejections);
  map["Localization: planes matches"] = toDoubleVector(this->LocalizationMatchingResults.at(PLANE).Rejections);
  map["Localization: blobs matches"]  = toDoubleVector(this->LocalizationMatchingResults.at(BLOB).Rejections);
  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetOutputFrame()
{
  PointCloud::Ptr output(new PointCloud);

  // Transform from LiDAR sensor to BASE coordinate system,
  // followed by rigid transform or undistortion
  if (this->Undistortion)
  {
    const Eigen::Isometry3d beginPose = this->TworldFrameStart * this->BaseToLidarOffset;
    const Eigen::Isometry3d endPose = this->Tworld * this->BaseToLidarOffset;
    LinearTransformInterpolator<double> transformInterpolator(beginPose, endPose);

    output->header = this->CurrentFrame->header;
    output->points.reserve(this->CurrentFrame->size());
    for (const Slam::Point& p : *this->CurrentFrame)
      output->push_back(Utils::TransformPoint(p, transformInterpolator(p.time)));
  }
  else
  {
    const Eigen::Isometry3d endPose = this->Tworld * this->BaseToLidarOffset;
    pcl::transformPointCloud(*this->CurrentFrame, *output, endPose.matrix());
  }

  output->header.frame_id = this->WorldFrameId;
  return output;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetEdgesMap() const
{
  PointCloud::Ptr map = this->EdgesPointsLocalMap->Get();
  map->header.frame_id = this->WorldFrameId;
  map->header.stamp = Utils::SecToPclStamp(this->GetWorldTransform().time);
  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetPlanarsMap() const
{
  PointCloud::Ptr map = this->PlanarPointsLocalMap->Get();
  map->header.frame_id = this->WorldFrameId;
  map->header.stamp = Utils::SecToPclStamp(this->GetWorldTransform().time);
  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetBlobsMap() const
{
  PointCloud::Ptr map = this->BlobsPointsLocalMap->Get();
  map->header.frame_id = this->WorldFrameId;
  map->header.stamp = Utils::SecToPclStamp(this->GetWorldTransform().time);
  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetEdgesKeypoints(bool worldCoordinates) const
{
  return worldCoordinates ? this->CurrentWorldEdgesPoints : this->CurrentEdgesPoints;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetPlanarsKeypoints(bool worldCoordinates) const
{
  return worldCoordinates ? this->CurrentWorldPlanarsPoints : this->CurrentPlanarsPoints;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetBlobsKeypoints(bool worldCoordinates) const
{
  return worldCoordinates ? this->CurrentWorldBlobsPoints : this->CurrentBlobsPoints;
}

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

//-----------------------------------------------------------------------------
void Slam::UpdateFrameAndState(const PointCloud::Ptr& inputPc)
{
  // Check frame dropping
  unsigned int droppedFrames = inputPc->header.seq - this->PreviousFrameSeq - 1;
  if ((this->PreviousFrameSeq > 0) && (droppedFrames > 0))
    PRINT_WARNING("SLAM dropped " << droppedFrames << " frame" << (droppedFrames > 1 ? "s" : "") << ".\n");
  this->PreviousFrameSeq = inputPc->header.seq;

  // Estimate world pose at current time
  // Use previous pose as new pose estimation
  Eigen::Isometry3d TworldEstimation = this->Tworld;
  // Or linearly extrapolate previous motion to estimate new pose
  if (this->NbrFrameProcessed >= 2 &&
      (this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION ||
       this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION_AND_REGISTRATION))
  {
    // Estimate new Tworld with a constant velocity model
    const double t = Utils::PclStampToSec(inputPc->header.stamp);
    const double t1 = this->LogTrajectory[this->LogTrajectory.size() - 1].time;
    const double t0 = this->LogTrajectory[this->LogTrajectory.size() - 2].time;
    if (t0 < t1 && t1 < t)
      TworldEstimation = LinearInterpolation(this->PreviousTworld, this->Tworld, t, t0, t1);
    else
      PRINT_WARNING("Motion extrapolation skipped as time is not strictly increasing.");
  }
  this->PreviousTworld = this->Tworld;
  this->Tworld = TworldEstimation;
  this->Trelative = this->PreviousTworld.inverse() * this->Tworld;

  SET_COUT_FIXED_PRECISION(3);
  PRINT_VERBOSE(2, "========== Update SLAM State ==========\n"
                   "Estimated Ego-Motion:\n"
                   " translation = [" << this->Trelative.translation().transpose()                          << "]\n"
                   " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(this->Trelative.linear())).transpose() << "]");
  RESET_COUT_FIXED_PRECISION;

  // Current keypoints become previous ones
  this->PreviousEdgesPoints = this->CurrentEdgesPoints;
  this->PreviousPlanarsPoints = this->CurrentPlanarsPoints;

  // Copy the input cloud to avoid modifying input
  // CHECK : no other way to avoid this copy? Could be heavy with 64 or 128...
  this->CurrentFrame = inputPc->makeShared();

  // Get frame duration
  double frameStartTime = std::numeric_limits<double>::max(),
         frameEndTime   = std::numeric_limits<double>::min();
  for (const Point& point: *this->CurrentFrame)
  {
    frameStartTime = std::min(frameStartTime, point.time);
    frameEndTime = std::max(frameEndTime, point.time);
  }
  this->FrameDuration = frameEndTime - frameStartTime;

  // FrameDuration should be > 0 if the time field is properly set
  if (this->FrameDuration > 0)
  {
    // Modify the points so that time becomes a relative advancement (between 0 and 1)
    for (Point& point: *this->CurrentFrame)
      point.time = (point.time - frameStartTime) / this->FrameDuration;
  }
  else
  {
    // If time field is not usable, set it to 1 to match end frame timestamp
    PRINT_WARNING("'time' field is not properly set and cannot be used for undistortion.");
    for (Point& point: *this->CurrentFrame)
      point.time = 1.;
  }
}

//-----------------------------------------------------------------------------
void Slam::ExtractKeypoints(const std::vector<size_t>& laserIdMapping)
{
  auto transformToBase = [this](const Slam::PointCloud::Ptr& inputPc)
  {
    PointCloud::Ptr baseCloud;
    // If transform to apply is identity, avoid much work and just change frame id if it is defined
    if (this->BaseToLidarOffset.isApprox(Eigen::Isometry3d::Identity()))
    {
      baseCloud = inputPc;
      baseCloud->header.frame_id = this->BaseFrameId.empty() ? inputPc->header.frame_id : this->BaseFrameId;
    }
    // If transform is set and non trivial, run transformation and notify it by changing frame id
    else
    {
      baseCloud.reset(new PointCloud);
      pcl::transformPointCloud(*inputPc, *baseCloud, this->BaseToLidarOffset.matrix());
      baseCloud->header.frame_id = this->BaseFrameId.empty() ? this->BaseFrameIdDefault : this->BaseFrameId;
    }
    return baseCloud;
  };

  // Extract keypoints from input cloud,
  this->KeyPointsExtractor->ComputeKeyPoints(this->CurrentFrame, laserIdMapping);

  // Get keypoints and transform them from LIDAR to BASE coordinates if needed.
  this->CurrentEdgesPoints   = transformToBase(this->KeyPointsExtractor->GetEdgePoints());
  this->CurrentPlanarsPoints = transformToBase(this->KeyPointsExtractor->GetPlanarPoints());
  this->CurrentBlobsPoints   = transformToBase(this->KeyPointsExtractor->GetBlobPoints());

  PRINT_VERBOSE(2, "========== Keypoints extraction ==========\n"
                   "Extracted features : " << this->CurrentEdgesPoints->size()   << " edges, "
                                           << this->CurrentPlanarsPoints->size() << " planes, "
                                           << this->CurrentBlobsPoints->size()   << " blobs.");
}

//-----------------------------------------------------------------------------
void Slam::ComputeEgoMotion()
{
  // kd-tree to process fast nearest neighbor
  // among the keypoints of the previous pointcloud
  // CHECK : This step behaves strangely much slower when using OpenMP.
  // This section processing duration (arbitrary unit) :
  //  1. without OpenMP included nor used in any code (nor in Slam or SSKE) : time = 1.
  //  2. with OpenMP, globally used with only 1 thread : time = 1.
  //  3. with OpenMP, globally used with 2 threads : time = 2.
  //  4. with OpenMP used in other parts but removing here parallel section : time = 2. ???
  // => Even if we don't use OpenMP, it is slower ! We expect (4) to behaves at similarly as (1) or (2)...
  IF_VERBOSE(3, Utils::Timer::Init("EgoMotion : build KD tree"));
  KDTree kdtreePreviousEdges, kdtreePreviousPlanes;
  #pragma omp parallel sections num_threads(std::min(this->NbThreads, 2))
  {
    #pragma omp section
    kdtreePreviousEdges.Reset(this->PreviousEdgesPoints);
    #pragma omp section
    kdtreePreviousPlanes.Reset(this->PreviousPlanarsPoints);
  }

  PRINT_VERBOSE(2, "========== Ego-Motion ==========\n"
                   << "Keypoints extracted from previous frame : "
                   << this->PreviousEdgesPoints->size() << " edges, "
                   << this->PreviousPlanarsPoints->size() << " planes");

  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
  unsigned int totalMatchedKeypoints = 0;

  // 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.
    KeypointsRegistration::Parameters optimParams;
    optimParams.NbThreads = this->NbThreads;
    optimParams.SingleEdgePerRing = true;
    optimParams.MaxDistanceForICPMatching = this->MaxDistanceForICPMatching;
    optimParams.MinNbrMatchedKeypoints = this->MinNbrMatchedKeypoints;
    optimParams.LineDistanceNbrNeighbors = this->EgoMotionLineDistanceNbrNeighbors;
    optimParams.MinimumLineNeighborRejection = this->EgoMotionMinimumLineNeighborRejection;
    optimParams.LineDistancefactor = this->EgoMotionLineDistancefactor;
    optimParams.MaxLineDistance = this->EgoMotionMaxLineDistance;
    optimParams.PlaneDistanceNbrNeighbors = this->EgoMotionPlaneDistanceNbrNeighbors;
    optimParams.PlaneDistancefactor1 = this->EgoMotionPlaneDistancefactor1;
    optimParams.PlaneDistancefactor2 = this->EgoMotionPlaneDistancefactor2;
    optimParams.MaxPlaneDistance = this->EgoMotionMaxPlaneDistance;
    optimParams.LMMaxIter = this->EgoMotionLMMaxIter;
    optimParams.LossScale = this->EgoMotionInitLossScale + icpIter * (this->EgoMotionFinalLossScale - this->EgoMotionInitLossScale) / this->EgoMotionICPMaxIter;

    KeypointsRegistration optim(optimParams, UndistortionMode::NONE, this->Trelative);

    // Loop over edges to build the point to line residuals
    this->EgoMotionMatchingResults[EDGE] = optim.BuildAndMatchResiduals(this->CurrentEdgesPoints, kdtreePreviousEdges, Keypoint::EDGE);

    // Loop over surfaces to build the point to plane residuals
    this->EgoMotionMatchingResults[PLANE] = optim.BuildAndMatchResiduals(this->CurrentPlanarsPoints, kdtreePreviousPlanes, Keypoint::PLANE);

    // Skip this frame if there are too few geometric keypoints matched
    totalMatchedKeypoints = this->EgoMotionMatchingResults[EDGE].NbMatches() + this->EgoMotionMatchingResults[PLANE].NbMatches();
    if (totalMatchedKeypoints < this->MinNbrMatchedKeypoints)
    {
      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"));

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

    // Get back optimized Trelative
    this->Trelative = optim.GetOptimizedEndPose();

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

  PRINT_VERBOSE(2, "Matched keypoints: " << totalMatchedKeypoints << " ("
                    << this->EgoMotionMatchingResults[EDGE].NbMatches()  << " edges, "
                    << this->EgoMotionMatchingResults[PLANE].NbMatches() << " planes).");
}

//-----------------------------------------------------------------------------
void Slam::Localization()
{
  // Get keypoints from maps and build kd-trees for fast nearest neighbors search
  IF_VERBOSE(3, Utils::Timer::Init("Localization : keypoints extraction"));
  PointCloud::Ptr subEdgesPointsLocalMap, subPlanarPointsLocalMap, subBlobPointsLocalMap(new PointCloud);
  KDTree kdtreeEdges, kdtreePlanes, kdtreeBlobs;

  auto extractMapKeypointsAndBuildKdTree = [this](const PointCloud::Ptr& currKeypoints, RollingGrid& map, PointCloud::Ptr& prevKeypoints, KDTree& kdTree)
  {
    // Estimate current keypoints bounding box
    PointCloud currWordKeypoints;
    pcl::transformPointCloud(*currKeypoints, currWordKeypoints, this->Tworld.matrix());
    Eigen::Vector4f minPoint, maxPoint;
    pcl::getMinMax3D(currWordKeypoints, minPoint, maxPoint);

    // Extract all points in maps lying in this bounding box
    prevKeypoints = map.Get(minPoint.head<3>().cast<double>().array(), maxPoint.head<3>().cast<double>().array());
    kdTree.Reset(prevKeypoints);
  };

  // CHECK : This step behaves strangely much slower when using OpenMP.
  // This section processing duration (arbitrary unit) :
  //  1. without OpenMP included nor used in any code (nor in Slam or SSKE) : time = 1.
  //  2. with OpenMP, globally used with only 1 thread                      : time ~ 1.
  //  3. with OpenMP, globally used with 3 threads                          : time ~ 2.
  //  4. with OpenMP used in other parts but removing here parallel section : time ~ 2.2 ?!
  // => Even if we don't use OpenMP, it is slower ! We expect (4) to behaves at similarly as (1) or (2)...
  #pragma omp parallel sections num_threads(std::min(this->NbThreads, 3))
  {
    #pragma omp section
    extractMapKeypointsAndBuildKdTree(this->CurrentEdgesPoints, *this->EdgesPointsLocalMap, subEdgesPointsLocalMap, kdtreeEdges);
    #pragma omp section
    extractMapKeypointsAndBuildKdTree(this->CurrentPlanarsPoints, *this->PlanarPointsLocalMap, subPlanarPointsLocalMap, kdtreePlanes);
    #pragma omp section
    if (!this->FastSlam)
      extractMapKeypointsAndBuildKdTree(this->CurrentBlobsPoints, *this->BlobsPointsLocalMap, subBlobPointsLocalMap, kdtreeBlobs);
  }

  PRINT_VERBOSE(2, "========== Localization ==========\n"
                   << "Keypoints extracted from map : "
                   << subEdgesPointsLocalMap->size() << " edges, "
                   << subPlanarPointsLocalMap->size() << " planes, "
                   << subBlobPointsLocalMap->size() << " blobs");

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

  // Reset ICP results
  unsigned int totalMatchedKeypoints = 0;

  // 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.
    KeypointsRegistration::Parameters optimParams;
    optimParams.NbThreads = this->NbThreads;
    optimParams.SingleEdgePerRing = false;
    optimParams.MaxDistanceForICPMatching = this->MaxDistanceForICPMatching;
    optimParams.MinNbrMatchedKeypoints = this->MinNbrMatchedKeypoints;
    optimParams.LineDistanceNbrNeighbors = this->LocalizationLineDistanceNbrNeighbors;
    optimParams.MinimumLineNeighborRejection = this->LocalizationMinimumLineNeighborRejection;
    optimParams.LineDistancefactor = this->LocalizationLineDistancefactor;
    optimParams.MaxLineDistance = this->LocalizationMaxLineDistance;
    optimParams.PlaneDistanceNbrNeighbors = this->LocalizationPlaneDistanceNbrNeighbors;
    optimParams.PlaneDistancefactor1 = this->LocalizationPlaneDistancefactor1;
    optimParams.PlaneDistancefactor2 = this->LocalizationPlaneDistancefactor2;
    optimParams.MaxPlaneDistance = this->LocalizationMaxPlaneDistance;
    optimParams.BlobDistanceNbrNeighbors = this->LocalizationBlobDistanceNbrNeighbors;
    optimParams.LMMaxIter = this->LocalizationLMMaxIter;
    optimParams.LossScale = this->LocalizationInitLossScale + icpIter * (this->LocalizationFinalLossScale - this->LocalizationInitLossScale) / this->LocalizationICPMaxIter;

    KeypointsRegistration optim(optimParams, this->Undistortion, this->Tworld, this->TworldFrameStart);

    // Loop over edges to build the point to line residuals
    this->LocalizationMatchingResults[EDGE] = optim.BuildAndMatchResiduals(this->CurrentEdgesPoints, kdtreeEdges, Keypoint::EDGE);

    // Loop over surfaces to build the point to plane residuals
    this->LocalizationMatchingResults[PLANE] = optim.BuildAndMatchResiduals(this->CurrentPlanarsPoints, kdtreePlanes, Keypoint::PLANE);

    // Loop over blobs to build the point to blob residuals
    this->LocalizationMatchingResults[BLOB] = optim.BuildAndMatchResiduals(this->CurrentBlobsPoints, kdtreeBlobs, Keypoint::BLOB);

    // Skip this frame if there is too few geometric keypoints matched
    totalMatchedKeypoints =   this->LocalizationMatchingResults[EDGE].NbMatches()
                            + this->LocalizationMatchingResults[PLANE].NbMatches()
                            + this->LocalizationMatchingResults[BLOB].NbMatches();
    if (totalMatchedKeypoints < this->MinNbrMatchedKeypoints)
    {
      // Reset state to previous one to avoid instability
      this->Trelative = Eigen::Isometry3d::Identity();
      this->Tworld = this->PreviousTworld;
      this->TworldFrameStart = this->Tworld;
      this->WithinFrameMotion.SetTransforms(this->TworldFrameStart, this->Tworld);
      PRINT_ERROR("Not enough keypoints, Localization skipped for this frame.");
      break;
    }

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

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

    // Unpack Tworld and TworldFrameStart
    this->Tworld = optim.GetOptimizedEndPose();
    this->Trelative = this->PreviousTworld.inverse() * this->Tworld;
    if (this->Undistortion == UndistortionMode::OPTIMIZED)
    {
      this->TworldFrameStart = optim.GetOptimizedStartPose();
      this->WithinFrameMotion.SetTransforms(this->TworldFrameStart, this->Tworld);
    }
    else if (this->Undistortion == UndistortionMode::APPROXIMATED)
    {
      this->TworldFrameStart = this->InterpolateBeginScanPose();
      this->WithinFrameMotion.SetTransforms(this->TworldFrameStart, this->Tworld);
    }

    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 = optim.EstimateRegistrationError();
      break;
    }
  }

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

  // Optionally print localization optimization summary
  SET_COUT_FIXED_PRECISION(3);
  PRINT_VERBOSE(2, "Matched keypoints: " << totalMatchedKeypoints << " ("
                   << this->LocalizationMatchingResults[EDGE].NbMatches()  << " edges, "
                   << this->LocalizationMatchingResults[PLANE].NbMatches() << " planes, "
                   << this->LocalizationMatchingResults[BLOB].NbMatches()  << " blobs)."
                   "\nPosition uncertainty    = " << this->LocalizationUncertainty.PositionError    << " m"
                   " (along [" << this->LocalizationUncertainty.PositionErrorDirection.transpose()    << "])"
                   "\nOrientation uncertainty = " << this->LocalizationUncertainty.OrientationError << " °"
                   " (along [" << this->LocalizationUncertainty.OrientationErrorDirection.transpose() << "])");
  RESET_COUT_FIXED_PRECISION;
}

//-----------------------------------------------------------------------------
void Slam::UpdateMapsUsingTworld()
{
  // it would be nice to add the point from the frame directly to the map
  auto updateMap = [this](std::shared_ptr<RollingGrid> map, PointCloud::Ptr baseFrame, PointCloud::Ptr worldFrame)
  {
    // Transform keypoints to WORLD coordinates
    worldFrame->clear();
    worldFrame->points.reserve(baseFrame->size());
    worldFrame->header = baseFrame->header;
    worldFrame->header.frame_id = this->WorldFrameId;
    if (this->Undistortion)
      for (const Point& p : *baseFrame)
        worldFrame->push_back(Utils::TransformPoint(p, this->WithinFrameMotion(p.time)));
    else
      for (const Point& p : *baseFrame)
        worldFrame->push_back(Utils::TransformPoint(p, this->Tworld));
    // Add new keypoints to rolling grid
    map->Add(worldFrame);
  };

  // run maps update
  #pragma omp parallel sections num_threads(std::min(this->NbThreads, 3))
  {
    #pragma omp section
    updateMap(this->EdgesPointsLocalMap, this->CurrentEdgesPoints, this->CurrentWorldEdgesPoints);
    #pragma omp section
    updateMap(this->PlanarPointsLocalMap, this->CurrentPlanarsPoints, this->CurrentWorldPlanarsPoints);
    #pragma omp section
    if (!this->FastSlam)
      updateMap(this->BlobsPointsLocalMap, this->CurrentBlobsPoints, this->CurrentWorldBlobsPoints);
  }
}

//-----------------------------------------------------------------------------
void Slam::LogCurrentFrameState(double time, const std::string& frameId)
{
  // If logging is enabled
  if (this->LoggingTimeout)
  {
    // Save current frame data to buffer
    this->LogTrajectory.emplace_back(this->Tworld, time, frameId);
    this->LogCovariances.emplace_back(Utils::Matrix6dToStdArray36(this->LocalizationUncertainty.Covariance));
    this->LogEdgesPoints.emplace_back(this->CurrentEdgesPoints, this->LoggingStorage);
    this->LogPlanarsPoints.emplace_back(this->CurrentPlanarsPoints, this->LoggingStorage);
    if (!this->FastSlam)
      this->LogBlobsPoints.emplace_back(this->CurrentBlobsPoints, this->LoggingStorage);

    // If a timeout is defined, forget too old data
    if (this->LoggingTimeout > 0)
    {
      // Forget all previous data older than LoggingTimeout, but keep at least 2 last transforms
      while (time - this->LogTrajectory.front().time > this->LoggingTimeout
             && this->LogTrajectory.size() > 2)
      {
        this->LogTrajectory.pop_front();
        this->LogCovariances.pop_front();
        this->LogEdgesPoints.pop_front();
        this->LogPlanarsPoints.pop_front();
        if (!this->FastSlam)
          this->LogBlobsPoints.pop_front();
      }
    }
  }

  // If logging is disabled, only keep last 2 transforms for latency compensation
  else
  {
    this->LogTrajectory.emplace_back(this->Tworld, time, frameId);
    while (this->LogTrajectory.size() > 2)
      this->LogTrajectory.pop_front();
  }
}

//==============================================================================
//   Helpers
//==============================================================================

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::InterpolateBeginScanPose()
{
  if (this->NbrFrameProcessed > 0)
  {
    const double prevFrameEnd = this->LogTrajectory.back().time;
    const double currFrameEnd = Utils::PclStampToSec(this->CurrentFrame->header.stamp);
    const double currFrameStart = currFrameEnd - this->FrameDuration;
    // Check that time is roughly increasing and that there is no time jump
    // during current frame (which would lead to huge FrameDuration)
    if (prevFrameEnd < currFrameEnd && this->FrameDuration < 2 * (currFrameEnd - prevFrameEnd))
      return LinearInterpolation(this->PreviousTworld, this->Tworld, currFrameStart, prevFrameEnd, currFrameEnd);
    else
    {
      PRINT_WARNING("Motion interpolation skipped as time is not increasing.");
      return this->Tworld;
    }
  }
  else
    return Eigen::Isometry3d::Identity();
}

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

//-----------------------------------------------------------------------------
void Slam::ClearMaps()
{
  this->EdgesPointsLocalMap->Reset();
  this->PlanarPointsLocalMap->Reset();
  this->BlobsPointsLocalMap->Reset();
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridLeafSizeEdges(double size)
{
  this->EdgesPointsLocalMap->SetLeafSize(size);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridLeafSizePlanes(double size)
{
  this->PlanarPointsLocalMap->SetLeafSize(size);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridLeafSizeBlobs(double size)
{
  this->BlobsPointsLocalMap->SetLeafSize(size);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridSize(int size)
{
  this->EdgesPointsLocalMap->SetGridSize(size);
  this->PlanarPointsLocalMap->SetGridSize(size);
  this->BlobsPointsLocalMap->SetGridSize(size);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridResolution(double resolution)
{
  this->EdgesPointsLocalMap->SetVoxelResolution(resolution);
  this->PlanarPointsLocalMap->SetVoxelResolution(resolution);
  this->BlobsPointsLocalMap->SetVoxelResolution(resolution);
}

} // end of LidarSlam namespace