//=========================================================================
//
// Copyright 2018 Kitware, Inc.
// Author: Guilbert Pierre (spguilbert@gmail.com)
// Data: 03-27-2018
//
// 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 trated indepently.
// The laser lines are projected onto the XY plane and are rescale depending on
// their vertical angle. Then we compute their curvature and create two class of
// keypoints. The edges keypoints which correspond to points with a hight 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 sparses 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 geometrics features derived from the keypoints of the previous frame.
// The geometrics features are lines or planes and are computed using the edges keypoints
// 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.
//
// - Mapping: 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 : "vtkSlam.h" and "vtkSlam.cxx" the lidar
// coordinate system {L} is a 3D coordinate system with its origin at the
// geometric center of the lidar. The world coordinate system {W} is a 3D
// coordinate system which coinciding with {L] at the initial position. The
// points will be denoted by the ending letter L or W if they belong to
// the corresponding coordinate system

// LOCAL
#include "vtkSlam.h"
#include "vtkVelodyneTransformInterpolator.h"
#include "vtkPCLConversions.h"
#include "CeresCostFunctions.h"
// STD
#include <sstream>
#include <algorithm>
#include <cmath>
#include <cfloat>
#include <ctime>
// VTK
#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkDataArray.h>
#include <vtkDoubleArray.h>
#include <vtkFloatArray.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkMath.h>
#include <vtkNew.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPolyLine.h>
#include <vtkSmartPointer.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkUnsignedCharArray.h>
#include <vtkUnsignedShortArray.h>
#include <vtkTransform.h>
#include <vtkPoints.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkTable.h>
// EIGEN
#include <Eigen/Dense>
// PCL
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
// CERES
#include <ceres/ceres.h>
#include <glog/logging.h>

#include "vtkTemporalTransforms.h"

vtkStandardNewMacro(vtkSlam);

namespace {
//-----------------------------------------------------------------------------
class LineFitting
{
public:
  // Fitting using PCA
  bool FitPCA(std::vector<Eigen::Vector3d >& points);

  // Futting using very local line and
  // check if this local line is consistent
  // in a more global neighborhood
  bool FitPCAAndCheckConsistency(std::vector<Eigen::Vector3d >& points);

  // Poor but fast fitting using
  // extremities of the distribution
  void FitFast(std::vector<Eigen::Vector3d >& points);

  // Direction and position
  Eigen::Vector3d Direction;
  Eigen::Vector3d Position;
  Eigen::Matrix3d SemiDist;
  Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity();
  double MaxDistance = 0.02;
  double MaxSinAngle = 0.65;
};

//-----------------------------------------------------------------------------
bool LineFitting::FitPCA(std::vector<Eigen::Vector3d >& points)
{
  // Compute PCA to determine best line approximation
  // of the points distribution
  Eigen::MatrixXd data(points.size(), 3);

  for (unsigned int k = 0; k < points.size(); k++)
  {
    data.row(k) = points[k];
  }

  Eigen::Vector3d mean = data.colwise().mean();
  Eigen::MatrixXd centered = data.rowwise() - mean.transpose();
  Eigen::MatrixXd cov = centered.transpose() * centered;
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);

  // Eigen values
  Eigen::MatrixXd D(1,3);
  // Eigen vectors
  Eigen::MatrixXd V(3,3);

  D = eig.eigenvalues();
  V = eig.eigenvectors();

  // Direction
  this->Direction = V.col(2).normalized();

  // Position
  this->Position = mean;

  // Semi distance matrix
  // (polar form associated to
  // a bilineare symmetric positive
  // semi-definite matrix)
  this->SemiDist = (this->I3 - this->Direction * this->Direction.transpose());
  this->SemiDist = this->SemiDist.transpose() * this->SemiDist;

  bool isLineFittingAccurate = true;

  // if a point of the neighborhood is too far from
  // the fitting line we considere the neighborhood as
  // non flat
  for (unsigned int k = 0; k < points.size(); k++)
  {
    double d = std::sqrt((points[k] - this->Position).transpose() * this->SemiDist * (points[k] - this->Position));
    if (d > this->MaxDistance)
    {
      isLineFittingAccurate = false;
    }
  }

  return isLineFittingAccurate;
}

//-----------------------------------------------------------------------------
bool LineFitting::FitPCAAndCheckConsistency(std::vector<Eigen::Vector3d >& points)
{
  bool isLineFittingAccurate = true;

  // first check if the neighborhood is straight
  Eigen::Vector3d U, V;
  U = (points[1] - points[0]).normalized();
  for (unsigned int index = 1; index < points.size() - 1; index++)
  {
    V = (points[index + 1] - points[index]).normalized();
    double sinAngle = (U.cross(V)).norm();
    if (sinAngle > this->MaxSinAngle)
    {
      isLineFittingAccurate = false;
    }
  }

  // Then fit with PCA
  isLineFittingAccurate &= this->FitPCA(points);
  return isLineFittingAccurate;
}

//-----------------------------------------------------------------------------
void LineFitting::FitFast(std::vector<Eigen::Vector3d >& points)
{
  // Take the two extrems points of the neighborhood
  // i.e the farest and the closest to the current point
  Eigen::Vector3d U = points[0];
  Eigen::Vector3d V = points[points.size() - 1];

  // direction
  this->Direction = (V - U).normalized();

  // position
  this->Position = U;

  // Semi distance matrix
  // (polar form associated to
  // a bilineare symmetric positive
  // semi-definite matrix)
  this->SemiDist = (this->I3 - this->Direction * this->Direction.transpose());
  this->SemiDist = this->SemiDist.transpose() * this->SemiDist;
}

//-----------------------------------------------------------------------------
Eigen::Matrix3d GetRotationMatrix(Eigen::Matrix<double, 6, 1> T)
{
  return Eigen::Matrix3d(
          Eigen::AngleAxisd(T(2), Eigen::Vector3d::UnitZ())     /* rotation around Z-axis */
        * Eigen::AngleAxisd(T(1), Eigen::Vector3d::UnitY())     /* rotation around Y-axis */
        * Eigen::AngleAxisd(T(0), Eigen::Vector3d::UnitX()));   /* rotation around X-axis */
}

//-----------------------------------------------------------------------------
template <typename T>
vtkSmartPointer<T> CreateDataArray(const char* name, vtkIdType np, vtkPolyData* pd)
{
  vtkSmartPointer<T> array = vtkSmartPointer<T>::New();
  array->Allocate(np);
  array->SetName(name);

  if (pd)
    {
    pd->GetPointData()->AddArray(array);
    }

  return array;
}

//-----------------------------------------------------------------------------
template <typename T>
std::vector<size_t> sortIdx(const std::vector<T> &v)
{
  // initialize original index locations
  std::vector<size_t> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  std::sort(idx.begin(), idx.end(),
       [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});

  return idx;
}

//-----------------------------------------------------------------------------
std::clock_t startTime;

//-----------------------------------------------------------------------------
void InitTime()
{
  startTime = std::clock();
}

//-----------------------------------------------------------------------------
void StopTimeAndDisplay(std::string functionName)
{
  std::clock_t endTime = std::clock();
  double dt = static_cast<double>(endTime - startTime) / CLOCKS_PER_SEC;
  std::cout << "  -time elapsed in function <" << functionName << "> : " << dt << " sec" << std::endl;
}

//-----------------------------------------------------------------------------
double Rad2Deg(double val)
{
  return val / vtkMath::Pi() * 180;
}

double Deg2Rad(double val)
{
  return (val * vtkMath::Pi()) / 180;
}
}

// The map reconstructed from the slam algorithm is stored in a voxel grid
// which split the space in differents region. From this voxel grid it is possible
// to only load the parts of the map which are pertinents when we run the mapping
// optimization algorithm. Morevover, when a a region of the space is too far from
// the current sensor position it is possible to remove the points stored in this region
// and to move the voxel grid in a closest region of the sensor position. This is used
// to decrease the memory used by the algorithm
class RollingGrid {
public:
  RollingGrid() {}

  RollingGrid(double posX, double posY, double posZ)
  {
    // should initialize using Tworld + size / 2
    this->VoxelGridPosition[0] = static_cast<int>(posX);
    this->VoxelGridPosition[1] = static_cast<int>(posY);
    this->VoxelGridPosition[2] = static_cast<int>(posZ);;
  }

  // roll the grid to enable adding new point cloud
  void Roll(Eigen::Matrix<double, 6, 1> &T)
  {
    // Very basic implementation where the grid is not circular

    // compute the position of the new frame center in the grid
    int frameCenterX = std::floor(T[3] / this->VoxelSize) - this->VoxelGridPosition[0];
    int frameCenterY = std::floor(T[4] / this->VoxelSize) - this->VoxelGridPosition[1];
    int frameCenterZ = std::floor(T[5] / this->VoxelSize) - this->VoxelGridPosition[2];

    // shift the voxel grid to the left
    while (frameCenterX - std::ceil(this->PointCloudSize / 2) <= 0)
    {
      for (int j = 0; j < this->VoxelSize; j++)
      {
        for (int k = 0; k < this->VoxelSize; k++)
        {
          for (int i = this->VoxelSize - 1; i > 0; i--)
          {
            this->grid[i][j][k] = this->grid[i-1][j][k];
          }
          this->grid[0][j][k].reset(new pcl::PointCloud<Point>());
        }
      }
      frameCenterX++;
      this->VoxelGridPosition[0]--;
    }

    // shift the voxel grid to the right
    while (frameCenterX + std::ceil(this->PointCloudSize / 2) >= this->VoxelSize - 1)
    {
      for (int j = 0; j < this->VoxelSize; j++)
      {
        for (int k = 0; k < this->VoxelSize; k++)
        {
          for (int i = 0; i < this->VoxelSize - 1; i++)
          {
            this->grid[i][j][k] = this->grid[i+1][j][k];
          }
          this->grid[VoxelSize-1][j][k].reset(new pcl::PointCloud<Point>());
        }
      }
      frameCenterX--;
      this->VoxelGridPosition[0]++;
    }

    // shift the voxel grid to the bottom
    while (frameCenterY - std::ceil(this->PointCloudSize / 2) <= 0)
    {
      for (int i = 0; i < this->VoxelSize; i++)
      {
        for (int k = 0; k < this->VoxelSize; k++)
        {
          for (int j = this->VoxelSize - 1; j > 0; j--)
          {
            this->grid[i][j][k] = this->grid[i][j-1][k];
          }
          this->grid[i][0][k].reset(new pcl::PointCloud<Point>());
        }
      }
      frameCenterY++;
      this->VoxelGridPosition[1]--;
//      cout << "bottom";
    }

    // shift the voxel grid to the top
    while (frameCenterY + std::ceil(this->PointCloudSize / 2) >= this->VoxelSize - 1)
    {
      for (int i = 0; i < this->VoxelSize; i++)
      {
        for (int k = 0; k < this->VoxelSize; k++)
        {
          for (int j = 0; j < this->VoxelSize - 1; j++)
          {
            this->grid[i][j][k] = this->grid[i][j+1][k];
          }
          this->grid[i][VoxelSize-1][k].reset(new pcl::PointCloud<Point>());
        }
      }
      frameCenterY--;
      this->VoxelGridPosition[1]++;
    }

    // shift the voxel grid to the "camera"
    while (frameCenterZ - std::ceil(this->PointCloudSize / 2) <= 0)
    {
      for (int i = 0; i < this->VoxelSize; i++)
      {
        for (int j = 0; j < this->VoxelSize; j++)
        {
          for (int k = this->VoxelSize - 1; k > 0; k--)
          {
            this->grid[i][j][k] = this->grid[i][j][k-1];
          }
          this->grid[i][j][0].reset(new pcl::PointCloud<Point>());
        }
      }
      frameCenterZ++;
      this->VoxelGridPosition[2]--;
    }

    // shift the voxel grid to the "horizon"
    while (frameCenterZ + std::ceil(this->PointCloudSize  / 2) >= this->VoxelSize - 1)
    {
      for (int i = 0; i < this->VoxelSize; i++)
      {
        for (int j = 0; j < this->VoxelSize; j++)
        {
          for (int k = 0; k < this->VoxelSize - 1; k++)
          {
            this->grid[i][j][k] = this->grid[i][j][k+1];
          }
          this->grid[i][j][VoxelSize-1].reset(new pcl::PointCloud<Point>());
        }
      }
      frameCenterZ--;
      this->VoxelGridPosition[2]++;
    }
  }

  // get points arround T
  pcl::PointCloud<Point>::Ptr Get(Eigen::Matrix<double, 6, 1> &T)
  {
    // compute the position of the new frame center in the grid
    int frameCenterX = std::floor(T[3] / this->VoxelSize) - this->VoxelGridPosition[0];
    int frameCenterY = std::floor(T[4] / this->VoxelSize) - this->VoxelGridPosition[1];
    int frameCenterZ = std::floor(T[5] / this->VoxelSize) - this->VoxelGridPosition[2];

    pcl::PointCloud<Point>::Ptr intersection(new pcl::PointCloud<Point>);

    // Get all voxel in intersection should use ceil here
    for (int i = frameCenterX - std::ceil(this->PointCloudSize / 2); i <= frameCenterX + std::ceil(this->PointCloudSize / 2); i++)
    {
      for (int j = frameCenterY - std::ceil(this->PointCloudSize / 2); j <= frameCenterY + std::ceil(this->PointCloudSize / 2); j++)
      {
        for (int k = frameCenterZ - std::ceil(this->PointCloudSize / 2); k <= frameCenterZ + std::ceil(this->PointCloudSize / 2); k++)
        {
          if (i < 0 || i > (this->VoxelSize - 1) ||
              j < 0 || j > (this->VoxelSize - 1) ||
              k < 0 || k > (this->VoxelSize - 1))
          {
            continue;
          }
          pcl::PointCloud<Point>:: Ptr voxel = this->grid[i][j][k];
          for (unsigned int l = 0; l < voxel->size(); l++)
          {
            intersection->push_back(voxel->at(l));
          }
        }
      }
    }
    return intersection;
  }

  // get all points
  pcl::PointCloud<Point>::Ptr Get()
  {
    pcl::PointCloud<Point>::Ptr intersection(new pcl::PointCloud<Point>);

    // Get all voxel in intersection should use ceil here
    for (int i = 0; i < VoxelSize; i++)
    {
      for (int j = 0; j < VoxelSize; j++)
      {
        for (int k = 0; k < VoxelSize; k++)
        {
          pcl::PointCloud<Point>:: Ptr voxel = this->grid[i][j][k];
          for (unsigned int l = 0; l < voxel->size(); l++)
          {
            intersection->push_back(voxel->at(l));
          }
        }
      }
    }
    return intersection;
  }

  // add some points to the grid
  void Add(pcl::PointCloud<Point>::Ptr pointcloud)
  {
    if (pointcloud->size() == 0)
    {
      vtkGenericWarningMacro("Pointcloud empty, voxel grid not updated");
      return;
    }

    // Voxel to filte because new points were add
    std::vector<std::vector<std::vector<int> > > voxelToFilter(VoxelSize, std::vector<std::vector<int> >(VoxelSize, std::vector<int>(VoxelSize, 0)));

    // Add points in the rolling grid
    int outlier = 0; // point who are not in the rolling grid
    for (unsigned int i = 0; i < pointcloud->size(); i++)
    {
      Point pts = pointcloud->points[i];
      // find the closest coordinate
      int cubeIdxX = std::floor(pts.x / this->VoxelSize) - this->VoxelGridPosition[0];
      int cubeIdxY = std::floor(pts.y / this->VoxelSize) - this->VoxelGridPosition[1];
      int cubeIdxZ = std::floor(pts.z / this->VoxelSize) - this->VoxelGridPosition[2];

      if (cubeIdxX >= 0 && cubeIdxX < this->VoxelSize &&
        cubeIdxY >= 0 && cubeIdxY < this->VoxelSize &&
        cubeIdxZ >= 0 && cubeIdxZ < this->VoxelSize)
      {
        voxelToFilter[cubeIdxX][cubeIdxY][cubeIdxZ] = 1;
        grid[cubeIdxX][cubeIdxY][cubeIdxZ]->push_back(pts);
      }
      else
      {
        outlier++;
      }
    }

    // Filter the modified pointCloud
    pcl::VoxelGrid<Point> downSizeFilter;
    downSizeFilter.setLeafSize(this->LeafSize, this->LeafSize, this->LeafSize);
    for (int i = 0; i < this->VoxelSize; i++)
    {
      for (int j = 0; j < this->VoxelSize; j++)
      {
        for (int k = 0; k < this->VoxelSize; k++)
        {
          if (voxelToFilter[i][j][k] == 1)
          {
            pcl::PointCloud<Point>::Ptr tmp(new pcl::PointCloud<Point>());
            downSizeFilter.setInputCloud(grid[i][j][k]);
            downSizeFilter.filter(*tmp);
            grid[i][j][k] = tmp;
          }
        }
      }
    }
  }

  void SetPointCoudMaxRange(const double maxdist)
  {
    this->PointCloudSize = std::ceil(2 * maxdist / this->VoxelResolution);
  }

  void SetSize(int size)
  {
    this->VoxelSize = size;
    grid.resize(this->VoxelSize);
    for (int i = 0; i < this->VoxelSize; i++)
    {
      grid[i].resize(this->VoxelSize);
      for (int j = 0; j < this->VoxelSize; j++)
      {
        grid[i][j].resize(this->VoxelSize);
        for (int k = 0; k < this->VoxelSize; k++)
        {
          grid[i][j][k].reset(new pcl::PointCloud<Point>());
        }
      }
    }
  }

  void SetResolution(double resolution) { this->VoxelResolution = resolution; }

  void SetLeafSize(double size) { this->LeafSize = size; }

private:
  //! Size of the voxel grid: n*n*n voxels
  int VoxelSize = 50;

  //! Resolution of a voxel
  double VoxelResolution = 10;

  //! Size of a pointcloud in voxel
  int PointCloudSize = 25;

  //! Size of the leaf use to downsample the pointcloud
  double LeafSize = 0.2;

  //! VoxelGrid of pointcloud
  std::vector<std::vector<std::vector<pcl::PointCloud<Point>::Ptr> > > grid;

  // Position of the VoxelGrid
  int VoxelGridPosition[3] = {0,0,0};
};

//-----------------------------------------------------------------------------
int vtkSlam::RequestData(vtkInformation *vtkNotUsed(request),
vtkInformationVector **inputVector, vtkInformationVector *outputVector)
{
  if (this->LaserIdMapping.empty())
  {
    vtkTable *calib = vtkTable::GetData(inputVector[1]->GetInformationObject(0));
    this->UpdateLaserIdMapping(calib);
  }

  // Get the input
  vtkPolyData *input = vtkPolyData::GetData(inputVector[0]->GetInformationObject(0));

  if (this->NbrFrameProcessed == 0)
  {
    if (inputVector[1]->GetInformationObject(0) == inputVector[2]->GetInformationObject(0))
    {
      std::cout << "no IMU" << std::endl;
      this->UsingImu = false;
    }
    else
    {
      std::cout << "using IMU" << std::endl;
      this->UsingImu = true;
      this->ImuData = vtkTable::GetData(inputVector[2]->GetInformationObject(0));
      this->ImuDataRow = 0;
      this->Velocity << this->InitialVelocityX, this->InitialVelocityY, this->InitialVelocityZ;
      this->Heading << 0,0,0;
    }
  }

  this->AddFrame(input);
  // output 0 - Current Frame
  vtkInformation *outInfo0 = outputVector->GetInformationObject(0);
  vtkPolyData *output0 = vtkPolyData::SafeDownCast(
      outInfo0->Get(vtkDataObject::DATA_OBJECT()));
  // add all debug information if displayMode == True
  if (this->DisplayMode == true && this->NbrFrameProcessed > 0)
  {
    std::cout << "display" << std::endl;

    this->DisplayLaserIdMapping(this->vtkCurrentFrame);
    this->DisplayRelAdv(this->vtkCurrentFrame);
    this->DisplayUsedKeypoints(this->vtkCurrentFrame);
    AddVectorToPolydataPoints<double, vtkDoubleArray>(this->Angles, "angles_line", this->vtkCurrentFrame);
    AddVectorToPolydataPoints<double, vtkDoubleArray>(this->LengthResolution, "length_resolution", this->vtkCurrentFrame);
    AddVectorToPolydataPoints<double, vtkDoubleArray>(this->SaillantPoint, "saillant_point", this->vtkCurrentFrame);
    AddVectorToPolydataPoints<double, vtkDoubleArray>(this->DepthGap, "depth_gap", this->vtkCurrentFrame);
    AddVectorToPolydataPoints<double, vtkDoubleArray>(this->IntensityGap, "intensity_gap", this->vtkCurrentFrame);
    AddVectorToPolydataPoints<double, vtkDoubleArray>(this->BlobScore, "blob_score", this->vtkCurrentFrame);
    AddVectorToPolydataPoints<int, vtkIntArray>(this->IsPointValid, "is_point_valid", this->vtkCurrentFrame);
    AddVectorToPolydataPoints<int, vtkIntArray>(this->Label, "keypoint_label", this->vtkCurrentFrame);
  }
  // get transform
  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  transform->Translate(Tworld[3], Tworld[4], Tworld[5]);
  transform->RotateX(Rad2Deg(Tworld[0]));
  transform->RotateY(Rad2Deg(Tworld[1]));
  transform->RotateZ(Rad2Deg(Tworld[2]));
  // create transform filter and transformt the current frame
  vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
  transformFilter->SetInputData(this->vtkCurrentFrame);
  transformFilter->SetTransform(transform);
  transformFilter->Update();
  output0->ShallowCopy(transformFilter->GetOutput());

  // output 1 - Trajectory
  auto *output1 = vtkPolyData::GetData(outputVector->GetInformationObject(1));
  output1->ShallowCopy(this->Trajectory);

  // output 2 - Edges Points Map
  auto *output2 = vtkPolyData::GetData(outputVector->GetInformationObject(2));
  auto EdgeMap = vtkPCLConversions::PolyDataFromPointCloud(this->EdgesPointsLocalMap->Get());
  output2->ShallowCopy(EdgeMap);

  // output 3 - Planar Points Map
  auto *output3 = vtkPolyData::GetData(outputVector->GetInformationObject(3));
  auto PlanarMap = vtkPCLConversions::PolyDataFromPointCloud(this->PlanarPointsLocalMap->Get());
  output3->ShallowCopy(PlanarMap);

  // output 4 - Blob Points Map
  auto *output4 = vtkPolyData::GetData(outputVector->GetInformationObject(4));
  auto BlobMap = vtkPCLConversions::PolyDataFromPointCloud(this->BlobsPointsLocalMap->Get());
  output4->ShallowCopy(BlobMap);

  return 1;
}

//-----------------------------------------------------------------------------
void vtkSlam::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "Slam Parameters: " << std::endl;
  vtkIndent paramIndent = indent.GetNextIndent();
  #define PrintParameter(param) os << paramIndent << #param << "\t" << this->param << std::endl;
  PrintParameter(EgoMotionLMMaxIter)
  PrintParameter(EgoMotionICPMaxIter)
  PrintParameter(MappingLMMaxIter)
  PrintParameter(MappingICPMaxIter)
  PrintParameter(EdgeSinAngleThreshold)
  PrintParameter(PlaneSinAngleThreshold)
  PrintParameter(EdgeDepthGapThreshold)
  PrintParameter(EgoMotionLineDistanceNbrNeighbors)
  PrintParameter(EgoMotionLineDistancefactor)
  PrintParameter(MappingMaxLineDistance)
  PrintParameter(MappingPlaneDistanceNbrNeighbors)
  PrintParameter(MappingPlaneDistancefactor1)
  PrintParameter(MappingPlaneDistancefactor2)
  PrintParameter(MappingMaxPlaneDistance)
  PrintParameter(MaxDistanceForICPMatching)
  PrintParameter(AngleResolution)
  PrintParameter(EgoMotionMinimumLineNeighborRejection)
  PrintParameter(MappingMinimumLineNeighborRejection)
  PrintParameter(MappingLineMaxDistInlier)
}

//-----------------------------------------------------------------------------
vtkSlam::vtkSlam()
{
  this->SetNumberOfInputPorts(3);
  this->SetNumberOfOutputPorts(5);
  this->Reset();
}

//-----------------------------------------------------------------------------
void vtkSlam::Reset()
{
  this->EdgesPointsLocalMap = std::make_shared<RollingGrid>();
  this->PlanarPointsLocalMap = std::make_shared<RollingGrid>();
  this->BlobsPointsLocalMap = std::make_shared<RollingGrid>();

  this->EdgesPointsLocalMap->SetResolution(10);
  this->PlanarPointsLocalMap->SetResolution(10);
  this->BlobsPointsLocalMap->SetResolution(10);

  this->EdgesPointsLocalMap->SetSize(50);
  this->PlanarPointsLocalMap->SetSize(50);
  this->BlobsPointsLocalMap->SetSize(50);

  // output of the vtk filter

  this->Trajectory = vtkSmartPointer<vtkTemporalTransforms>::New();
  Tworld = Eigen::Matrix<double, 6, 1>::Zero();

  this->LaserIdMapping.clear();
  this->NbrFrameProcessed = 0;
  this->Tworld = Eigen::Matrix<double, 6, 1>::Zero();

  // add the required array in the trajectory
  CreateDataArray<vtkDoubleArray>("Variance Error", 0, this->Trajectory);
  CreateDataArray<vtkIntArray>("Mapping: edges used", 0, this->Trajectory);
  CreateDataArray<vtkIntArray>("Mapping: planes used", 0, this->Trajectory);
  CreateDataArray<vtkIntArray>("Mapping: blobs used", 0, this->Trajectory);
  CreateDataArray<vtkIntArray>("Mapping: total keypoints used", 0, this->Trajectory);
  CreateDataArray<vtkIntArray>("EgoMotion: edges used", 0, this->Trajectory);
  CreateDataArray<vtkIntArray>("EgoMotion: planes used", 0, this->Trajectory);
  CreateDataArray<vtkIntArray>("EgoMotion: total keypoints used", 0, this->Trajectory);

  this->ImuConfidence << 0.95, 0.995 ,0.8, 0.65, 0.65, 0.8;
}

//-----------------------------------------------------------------------------
vtkSlam::~vtkSlam()
{

}

//-----------------------------------------------------------------------------
int vtkSlam::FillInputPortInformation(int port, vtkInformation *info)
{
  if ( port == 0 )
  {
    info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData" );
    return 1;
  }
  if ( port == 1 )
  {
    info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkTable" );
    return 1;
  }
  if ( port == 2 )
  {
    info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkTableImu" );
    return 1;
  }
  return 0;
}

//-----------------------------------------------------------------------------
void vtkSlam::GetWorldTransform(double* Tworld)
{
  // Rotation and translation relative
  Eigen::Matrix3d Rw;

  // full rotation
  Rw = GetRotationMatrix(this->Tworld);

  double rx = std::atan2(Rw(2, 1), Rw(2, 2));
  double ry = -std::asin(Rw(2, 0));
  double rz = std::atan2(Rw(1, 0), Rw(0, 0));
//  std::vector<double> res(6, 0);

  Tworld[0] = rx;
  Tworld[1] = ry;
  Tworld[2] = rz;
  Tworld[3] = this->Tworld(3);
  Tworld[4] = this->Tworld(4);
  Tworld[5] = this->Tworld(5);

//  return res;
}

//-----------------------------------------------------------------------------
void vtkSlam::PrepareDataForNextFrame()
{
  // Reset the pcl format pointcloud to store the new frame
  this->pclCurrentFrame.reset(new pcl::PointCloud<Point>());
  this->pclCurrentFrameByScan.resize(this->NLasers);
  for (unsigned int k = 0; k < this->NLasers; ++k)
  {
    this->pclCurrentFrameByScan[k].reset(new pcl::PointCloud<Point>());
  }

  this->CurrentEdgesPoints.reset(new pcl::PointCloud<Point>());
  this->CurrentPlanarsPoints.reset(new pcl::PointCloud<Point>());
  this->CurrentBlobsPoints.reset(new pcl::PointCloud<Point>());

  // reset vtk <-> pcl id mapping
  this->FromVTKtoPCLMapping.clear();
  this->FromVTKtoPCLMapping.resize(0);
  this->FromPCLtoVTKMapping.clear();
  this->FromPCLtoVTKMapping.resize(this->NLasers);
  this->Angles.clear();
  this->Angles.resize(this->NLasers);
  this->LengthResolution.clear();
  this->LengthResolution.resize(this->NLasers);
  this->SaillantPoint.clear();
  this->SaillantPoint.resize(this->NLasers);
  this->DepthGap.clear();
  this->DepthGap.resize(this->NLasers);
  this->IntensityGap.clear();
  this->IntensityGap.resize(this->NLasers);
  this->BlobScore.clear();
  this->BlobScore.resize(this->NLasers);
  this->IsPointValid.clear();
  this->IsPointValid.resize(this->NLasers);
  this->Label.clear();
  this->Label.resize(this->NLasers);
}

//-----------------------------------------------------------------------------
template <typename T, typename Tvtk>
void vtkSlam::AddVectorToPolydataPoints(const std::vector<std::vector<T>>& vec, const char* name, vtkPolyData* pd)
{
  vtkSmartPointer<Tvtk> array = vtkSmartPointer<Tvtk>::New();
  array->Allocate(pd->GetNumberOfPoints());
  array->SetName(name);
  for (unsigned int k = 0; k < pd->GetNumberOfPoints(); ++k)
  {
    unsigned int scan = this->FromVTKtoPCLMapping[k].first;
    unsigned int index = this->FromVTKtoPCLMapping[k].second;
    array->InsertNextTuple1(vec[scan][index]);
  }
  pd->GetPointData()->AddArray(array);
}

//-----------------------------------------------------------------------------
void vtkSlam::DisplayLaserIdMapping(vtkSmartPointer<vtkPolyData> input)
{
  vtkDataArray* idsArray = input->GetPointData()->GetArray("laser_id");
  vtkSmartPointer<vtkIntArray> laserMappingArray = vtkSmartPointer<vtkIntArray>::New();
  laserMappingArray->Allocate(input->GetNumberOfPoints());
  laserMappingArray->SetName("laser_mapping");
  for (unsigned int k = 0; k < input->GetNumberOfPoints(); ++k)
  {
    int id = static_cast<int>(idsArray->GetTuple1(k));
    id = this->LaserIdMapping[id];
    laserMappingArray->InsertNextTuple1(id);
  }
  input->GetPointData()->AddArray(laserMappingArray);
}

//-----------------------------------------------------------------------------
void vtkSlam::DisplayRelAdv(vtkSmartPointer<vtkPolyData> input)
{
  vtkSmartPointer<vtkDoubleArray> relAdvArray = vtkSmartPointer<vtkDoubleArray>::New();
  relAdvArray->Allocate(input->GetNumberOfPoints());
  relAdvArray->SetName("relative_adv");
  for (unsigned int k = 0; k < input->GetNumberOfPoints(); ++k)
  {
    unsigned int scan = this->FromVTKtoPCLMapping[k].first;
    unsigned int index = this->FromVTKtoPCLMapping[k].second;
    relAdvArray->InsertNextTuple1(this->pclCurrentFrameByScan[scan]->points[index].intensity);
  }
  input->GetPointData()->AddArray(relAdvArray);
}

//-----------------------------------------------------------------------------
void vtkSlam::DisplayUsedKeypoints(vtkSmartPointer<vtkPolyData> input)
{
  vtkSmartPointer<vtkIntArray> edgeUsedEgoMotion = vtkSmartPointer<vtkIntArray>::New();
  edgeUsedEgoMotion->Allocate(input->GetNumberOfPoints());
  edgeUsedEgoMotion->SetName("Edges_Used_EgoMotion");

  vtkSmartPointer<vtkIntArray> edgeUsedMapping = vtkSmartPointer<vtkIntArray>::New();
  edgeUsedMapping->Allocate(input->GetNumberOfPoints());
  edgeUsedMapping->SetName("Edges_Used_Mapping");

  vtkSmartPointer<vtkIntArray> planarUsedEgoMotion = vtkSmartPointer<vtkIntArray>::New();
  planarUsedEgoMotion->Allocate(input->GetNumberOfPoints());
  planarUsedEgoMotion->SetName("Planes_Used_EgoMotion");

  vtkSmartPointer<vtkIntArray> planarUsedMapping = vtkSmartPointer<vtkIntArray>::New();
  planarUsedMapping->Allocate(input->GetNumberOfPoints());
  planarUsedMapping->SetName("Planes_Used_Mapping");

  // fill with -1 for points that are not keypoints
  for (unsigned int k = 0; k < input->GetNumberOfPoints(); ++k)
  {
    edgeUsedEgoMotion->InsertNextTuple1(-1);
    edgeUsedMapping->InsertNextTuple1(-1);
    planarUsedEgoMotion->InsertNextTuple1(-1);
    planarUsedMapping->InsertNextTuple1(-1);
  }

  // fill with 1 if the point is a keypoint
  // fill with 2 if the point is a used keypoint
  for (unsigned int k = 0; k < this->EdgesIndex.size(); ++k)
  {
    unsigned int scan = this->EdgesIndex[k].first;
    unsigned int index = this->EdgesIndex[k].second;

    edgeUsedEgoMotion->SetTuple1(this->FromPCLtoVTKMapping[scan][index], EdgePointRejectionEgoMotion[k]);
    edgeUsedMapping->SetTuple1(this->FromPCLtoVTKMapping[scan][index], EdgePointRejectionMapping[k]);
  }
  for (unsigned int k = 0; k < this->PlanarIndex.size(); ++k)
  {
    unsigned int scan = this->PlanarIndex[k].first;
    unsigned int index = this->PlanarIndex[k].second;

    planarUsedEgoMotion->SetTuple1(this->FromPCLtoVTKMapping[scan][index], this->PlanarPointRejectionEgoMotion[k]);
    planarUsedMapping->SetTuple1(this->FromPCLtoVTKMapping[scan][index], this->PlanarPointRejectionMapping[k]);
  }

  input->GetPointData()->AddArray(edgeUsedEgoMotion);
  input->GetPointData()->AddArray(edgeUsedMapping);
  input->GetPointData()->AddArray(planarUsedEgoMotion);
  input->GetPointData()->AddArray(planarUsedMapping);
}

//-----------------------------------------------------------------------------
void vtkSlam::AddFrame(vtkPolyData* newFrame)
{
  if (!newFrame)
  {
    vtkGenericWarningMacro("Slam entry is a null pointer data");
    return;
  }

  this->vtkCurrentFrame = newFrame;

  this->PreviousFrameTime = this->CurrentFrameTime;
  this->CurrentFrameTime = this->vtkCurrentFrame->GetPointData()->GetArray("adjustedtime")->GetTuple1(0);

  // Check if the number of lasers has been set
  if (this->NLasers == 0)
  {
    vtkGenericWarningMacro("Frame added without specifying the number of lasers");
  }

  std::cout << "#########################################################" << std::endl
            << "Processing frame : " << this->NbrFrameProcessed << std:: endl
            << "#########################################################" << std::endl
            << std::endl;

  // Reset the members variables used during the last
  // processed frame so that they can be used again
  PrepareDataForNextFrame();

  // Update the kalman filter time
  double time = newFrame->GetPointData()->GetArray("adjustedtime")->GetTuple1(0) * 1e-6;

  // If the new frame is the first one we just add the
  // extracted keypoints into the map without running
  // odometry and mapping steps
  if (this->NbrFrameProcessed == 0)
  {
    // Convert the new frame into pcl format and sort
    // the laser scan-lines by vertical angle
    this->ConvertAndSortScanLines(newFrame);

    // Compute the edges and planars keypoints
    this->ComputeKeyPoints(newFrame);

    // update map using tworld
    this->UpdateMapsUsingTworld();

    // Current keypoints become previous ones
    this->PreviousEdgesPoints = this->CurrentEdgesPoints;
    this->PreviousPlanarsPoints = this->CurrentPlanarsPoints;
    this->PreviousBlobsPoints = this->CurrentBlobsPoints;
    this->NbrFrameProcessed++;
    return;
  }

  // Convert the new frame into pcl format and sort
  // the laser scan-lines by vertical angle
  InitTime();
  this->ConvertAndSortScanLines(vtkCurrentFrame);
  StopTimeAndDisplay("Sorting lines");

  // Compute the edges and planars keypoints
  InitTime();
  this->ComputeKeyPoints(vtkCurrentFrame);
  StopTimeAndDisplay("Keypoints extraction");

  if (this->UsingImu)
  {
    // Perfom ImuMotion
    InitTime();
    this->ComputeImuMotionTest();
    StopTimeAndDisplay("Imu-Motion");
  }

  // Perfom EgoMotion
  if (!this->OnlyImu)//for testing
  {
    InitTime();
    this->ComputeEgoMotion();
    StopTimeAndDisplay("Ego-Motion");
  }

  // Transform the current keypoints to the
  // referential of the sensor at the end of
  // frame acquisition
  //InitTime();
  //this->TransformCurrentKeypointsToEnd();
  //StopTimeAndDisplay("Undistortion");

  // Perform Mapping
  if (!this->OnlyImu)//for testing
  {
    InitTime();
    this->Mapping();
    StopTimeAndDisplay("Mapping");
  }

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

  // Motion and localization parameters estimation information display
  Eigen::Vector3d angles, trans;
  angles << Rad2Deg(this->Trelative(0)), Rad2Deg(this->Trelative(1)), Rad2Deg(this->Trelative(2));
  trans << this->Trelative(3), this->Trelative(4), this->Trelative(5);
  std::cout << "Ego-Motion estimation: angles = [" << angles.transpose() << "] translation: [" << trans.transpose() << "]" << std::endl;
  angles << Rad2Deg(this->Tworld(0)), Rad2Deg(this->Tworld(1)), Rad2Deg(this->Tworld(2));
  trans << this->Tworld(3), this->Tworld(4), this->Tworld(5);
  std::cout << "Localiazion estimation: angles = [" << angles.transpose() << "] translation: [" << trans.transpose() << "]"
            << std::endl << std::endl << std::endl;

  // Update Trajectory
  Eigen::AngleAxisd orientation = Eigen::AngleAxisd(
      Eigen::AngleAxisd(this->Tworld[0], Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(this->Tworld[1],  Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(this->Tworld[2], Eigen::Vector3d::UnitZ()));
  this->Trajectory->PushBack(time, orientation, Tworld.tail(3));

  // Indicate the filter has been modify
  this->Modified();
  return;
}

//-----------------------------------------------------------------------------
void vtkSlam::ConvertAndSortScanLines(vtkSmartPointer<vtkPolyData> input)
{
  // temp var
  double xL[3]; // in {L}
  Point yL; // in {L}

  // Get informations about input pointcloud
  vtkDataArray* lasersId = input->GetPointData()->GetArray("laser_id");
  vtkDataArray* time = input->GetPointData()->GetArray("timestamp");
  vtkDataArray* reflectivity = input->GetPointData()->GetArray("intensity");
  vtkPoints* Points = input->GetPoints();
  unsigned int Npts = input->GetNumberOfPoints();
  double t0 = static_cast<double>(time->GetTuple1(0));
  double t1 = static_cast<double>(time->GetTuple1(Npts - 1));
  this->FromVTKtoPCLMapping.resize(Npts);

  for (unsigned int index = 0; index < Npts; ++index)
  {
    // Get information about current point
    Points->GetPoint(index, xL);
    yL.x = xL[0]; yL.y = xL[1]; yL.z = xL[2];

    double relAdv = (static_cast<double>(time->GetTuple1(index)) - t0) / (t1 - t0);
    unsigned int id = static_cast<int>(lasersId->GetTuple1(index));
    double reflec = static_cast<double>(reflectivity->GetTuple1(index));
    id = this->LaserIdMapping[id];
    yL.intensity = relAdv;
    yL.normal_y = id;
    yL.normal_z = reflec;

    // add the current point to its corresponding laser scan
    this->pclCurrentFrame->push_back(yL);
    this->pclCurrentFrameByScan[id]->push_back(yL);
    this->FromVTKtoPCLMapping[index] = std::pair<int, int>(id, this->pclCurrentFrameByScan[id]->size() - 1);
    this->FromPCLtoVTKMapping[id].push_back(index);
  }
}

//-----------------------------------------------------------------------------
void vtkSlam::ComputeKeyPoints(vtkSmartPointer<vtkPolyData> input)
{
  // Initialize the vectors with the correct length
  for (unsigned int k = 0; k < this->NLasers; ++k)
  {
    this->IsPointValid[k].resize(this->pclCurrentFrameByScan[k]->size(), 1);
    this->Label[k].resize(this->pclCurrentFrameByScan[k]->size(), 0);
    this->Angles[k].resize(this->pclCurrentFrameByScan[k]->size(),0);
    this->LengthResolution[k].resize(this->pclCurrentFrameByScan[k]->size(),0);
    this->SaillantPoint[k].resize(this->pclCurrentFrameByScan[k]->size(),0);
    this->DepthGap[k].resize(this->pclCurrentFrameByScan[k]->size(), 0);
    this->IntensityGap[k].resize(this->pclCurrentFrameByScan[k]->size(), 0);
    this->BlobScore[k].resize(this->pclCurrentFrameByScan[k]->size(), 0);
  }

  // compute keypoints scores
  this->ComputeCurvature(input);

  // Invalid points with bad criteria
  this->InvalidPointWithBadCriteria();

  // labelize keypoints
  this->SetKeyPointsLabels(input);
}

//-----------------------------------------------------------------------------
void vtkSlam::ComputeCurvature(vtkSmartPointer<vtkPolyData> vtkNotUsed(input))
{
  Point currentPoint, nextPoint, previousPoint;
  Eigen::Vector3d X, centralPoint;
  LineFitting leftLine, rightLine, farNeighborsLine;

  // loop over scans lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    // loop over points in the current scan line
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();

    // if the line is almost empty, skip it
    if (Npts < 2 * this->NeighborWidth + 1)
    {
      continue;
    }

    for (int index = this->NeighborWidth; (index + this->NeighborWidth) < Npts; ++index)
    {
      // central point
      currentPoint = this->pclCurrentFrameByScan[scanLine]->points[index];
      centralPoint << currentPoint.x, currentPoint.y, currentPoint.z;

      // compute intensity gap
      nextPoint = this->pclCurrentFrameByScan[scanLine]->points[index + 1];
      previousPoint = this->pclCurrentFrameByScan[scanLine]->points[index - 1];
      this->IntensityGap[scanLine][index] = std::abs(nextPoint.normal_z - previousPoint.normal_z);
      // We will compute the line that fit the neighbors located
      // previously the current. We will do the same for the
      // neighbors located after the current points. We will then
      // compute the angle between these two lines as an approximation
      // of the "sharpness" of the current point.
      std::vector<Eigen::Vector3d > leftNeighbor;
      std::vector<Eigen::Vector3d > rightNeighbor;
      std::vector<Eigen::Vector3d > farNeighbors;

      // Fill right and left neighborhood
      // /!\ The way the neighbors are added
      // to the vectors matters. Especially when
      // computing the saillancy
      for (int j = index - this->NeighborWidth; j <= index + this->NeighborWidth; ++j)
      {
        currentPoint = this->pclCurrentFrameByScan[scanLine]->points[j];
        X << currentPoint.x, currentPoint.y, currentPoint.z;
        if (j < index)
          leftNeighbor.push_back(X);
        if (j > index)
          rightNeighbor.push_back(X);
      }

      // Fit line on the neighborhood and
      // Indicate if the left and right side
      // neighborhood of the current point is flat or not
      bool leftFlat = leftLine.FitPCAAndCheckConsistency(leftNeighbor);
      bool rightFlat = rightLine.FitPCAAndCheckConsistency(rightNeighbor);

      // Measurement of the gap
      double dist1 = 0; double dist2 = 0;

      // if both neighborhood are flat we can compute
      // the angle between them as an approximation of the
      // sharpness of the current point
      if (rightFlat && leftFlat)
      {
        // We check that the current point is not too far from its
        // neighborhood lines. This is because we don't want a point
        // to be considered as a angles point if it is due to gap
        dist1 = std::sqrt((centralPoint - leftLine.Position).transpose() * leftLine.SemiDist * (centralPoint - leftLine.Position));
        dist2 = std::sqrt((centralPoint - rightLine.Position).transpose() * rightLine.SemiDist * (centralPoint - rightLine.Position));

        if ((dist1 < this->DistToLineThreshold) && (dist2 < this->DistToLineThreshold))
          this->Angles[scanLine][index] = std::abs((leftLine.Direction.cross(rightLine.Direction)).norm()); // sin of angle actually
      }
      // Here one side of the neighborhood is non flat
      // Hence it is not worth to estimate the sharpness.
      // Only the gap will be considered here.
      else if (rightFlat && !leftFlat)
      {
        dist1 = 1000.0;
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          dist1 = std::min(dist1,
                  std::sqrt((leftNeighbor[neighIndex] - rightLine.Position).transpose() * rightLine.SemiDist * (leftNeighbor[neighIndex] - rightLine.Position)));
        }
        dist1 = 0.5 * dist1;
      }
      else if (!rightFlat && leftFlat)
      {
        dist2 = 1000.0;
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          dist2 = std::min(dist2,
                  std::sqrt((rightNeighbor[neighIndex] - leftLine.Position).transpose() * leftLine.SemiDist * (rightNeighbor[neighIndex] - leftLine.Position)));
        }
        dist2 = 0.5 * dist2;
      }
      else
      {
        // Compute saillant point score
        double currDepth = centralPoint.norm();
        unsigned int diffDepth = 0;
        bool canLeftBeAdded = true; bool hasLeftEncounteredDepthGap = false;
        bool canRightBeAdded = true; bool hasRightEncounteredDepthGap = false;

        // The saillant point score is the distance between the current point
        // and the points that have a depth gap with the current point
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          // Left neighborhood depth gap computation
          if ((std::abs(leftNeighbor[leftNeighbor.size() - 1 - neighIndex].norm() - currDepth) > 1.5) && canLeftBeAdded)
          {
            hasLeftEncounteredDepthGap = true;
            diffDepth++;
            farNeighbors.push_back(leftNeighbor[neighIndex]);
          }
          else
          {
            if (hasLeftEncounteredDepthGap)
            {
              canLeftBeAdded = false;
            }
          }
          // Right neigborhood depth gap computation
          if ((std::abs(rightNeighbor[neighIndex].norm() - currDepth) > 1.5) && canRightBeAdded)
          {
            hasRightEncounteredDepthGap = true;
            diffDepth++;
            farNeighbors.push_back(rightNeighbor[neighIndex]);
          }
          else
          {
            if (hasRightEncounteredDepthGap)
            {
              canRightBeAdded = false;
            }
          }
        }

        // If there is enought neighbors with a big depth gap
        // we propose to compute the saillancy of the current
        // as the distance between the line that fits the neighbors
        // with a depth gap and the current point
        if (static_cast<double>(diffDepth) / (2.0 * this->NeighborWidth) > 0.5)
        {
          farNeighborsLine.FitPCA(farNeighbors);
          this->SaillantPoint[scanLine][index] = std::sqrt(
            (centralPoint - farNeighborsLine.Position).transpose() * farNeighborsLine.SemiDist * (centralPoint - farNeighborsLine.Position));
        }

        this->BlobScore[scanLine][index] = 1;
      }

      this->DepthGap[scanLine][index] = std::max(dist1, dist2);
    }
  }
}

//-----------------------------------------------------------------------------
void vtkSlam::InvalidPointWithBadCriteria()
{
  // Temporary variables used in the next loop
  Eigen::Vector3d dX, X, Xn, Xp, Xproj, dXproj;
  Eigen::Vector3d Y, Yn, Yp, dY;
  double L, Ln, expectedLength, dLn, dLp;
  Point currentPoint, nextPoint, previousPoint;
  Point temp;

  // loop over scan lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();

    // if the line is almost empty, skip it
    if (Npts < 3 * this->NeighborWidth)
    {
      continue;
    }
    // invalidate first and last points
    for (int index = 0; index <= this->NeighborWidth; ++index)
    {
      this->IsPointValid[scanLine][index] = 0;
    }
    for (int index = Npts - 1 - this->NeighborWidth - 1; index < Npts; ++index)
    {
      this->IsPointValid[scanLine][index] = 0;
    }

    // loop over points into the scan line
    for (int index = this->NeighborWidth; index <  Npts - this->NeighborWidth - 1; ++index)
    {
      currentPoint = this->pclCurrentFrameByScan[scanLine]->points[index];
      nextPoint = this->pclCurrentFrameByScan[scanLine]->points[index + 1];
      previousPoint = this->pclCurrentFrameByScan[scanLine]->points[index - 1];
      X << currentPoint.x, currentPoint.y, currentPoint.z;
      Xn << nextPoint.x, nextPoint.y, nextPoint.z;
      Xp << previousPoint.x, previousPoint.y, previousPoint.z;
      dX = Xn - X;
      L = X.norm();
      Ln = Xn.norm();
      dLn = dX.norm();

      // the expected length between two firing of the same laser
      // depend on the distance and the angular resolution of the
      // sensor.
      expectedLength = 2.0 *  std::tan(this->AngleResolution / 2.0) * L;
      double ratioExpectedLength = 10.0;

      // if the length between the two firing
      // is more than n-th the expected length
      // it means that there is a gap. We now must
      // determine if the gap is due to the geometry of
      // the scene or if the gap is due to an occluded area
      if (dLn > ratioExpectedLength * expectedLength)
      {
        // Project the next point onto the
        // sphere of center 0 and radius =
        // norm of the current point. If the
        // gap has disappeared it means that
        // the gap was due to an occlusion
        Xproj = L / Ln * Xn;
        dXproj = Xproj - X;
        // it is a depth gap, invalidate the part which belong
        // to the occluded area (farest)
        // invalid next part
        if (L < Ln)
        {
          for (int i = index + 1; i <= index + this->NeighborWidth; ++i)
          {
            if (i > index + 1)
            {
              temp = this->pclCurrentFrameByScan[scanLine]->points[i - 1];
              Yp << temp.x, temp.y, temp.z;
              temp = this->pclCurrentFrameByScan[scanLine]->points[i];
              Y << temp.x, temp.y, temp.z;
              dY = Y - Yp;
              // if there is a gap in the neihborhood
              // we do not invalidate the rest of neihborhood
              if (dY.norm() > ratioExpectedLength * expectedLength)
              {
                break;
              }
            }
            this->IsPointValid[scanLine][i] = 0;
          }
        }
        // invalid previous part
        else
        {
          for (int i = index - this->NeighborWidth; i <= index; ++i)
          {
            if (i < index)
            {
              temp = this->pclCurrentFrameByScan[scanLine]->points[i + 1];
              Yn << temp.x, temp.y, temp.z;
              temp = this->pclCurrentFrameByScan[scanLine]->points[i];
              Y << temp.x, temp.y, temp.z;
              dY = Yn - Y;
              // if there is a gap in the neihborhood
              // we do not invalidate the rest of neihborhood
              if (dY.norm() > ratioExpectedLength * expectedLength)
              {
                break;
              }
            }
            this->IsPointValid[scanLine][i] = 0;
          }
        }
      }
      // Invalid points which are too close from the sensor
      if (L < this->MinDistanceToSensor)
      {
        this->IsPointValid[scanLine][index] = 0;
      }

      // Invalid points which are on a planar
      // surface nearly parallel to the laser
      // beam direction
      dLp = (X - Xp).norm();
      if ((dLp > 1 / 4.0 * ratioExpectedLength * expectedLength) && (dLn > 1 / 4.0 * ratioExpectedLength * expectedLength))
      {
        this->IsPointValid[scanLine][index] = 0;
      }
    }
  }
}

//-----------------------------------------------------------------------------
void vtkSlam::SetKeyPointsLabels(vtkSmartPointer<vtkPolyData> vtkNotUsed(input))
{
  this->EdgesIndex.clear(); this->EdgesIndex.resize(0);
  this->PlanarIndex.clear(); this->PlanarIndex.resize(0);
  this->BlobIndex.clear(); this->BlobIndex.resize(0);

  // loop over the scan lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();
    unsigned int nbrEdgePicked = 0;
    unsigned int nbrPlanarPicked = 0;

    // We split the validity of points between the edges
    // keypoints and planar keypoints. This allows to take
    // some points as planar keypoints even if they are close
    // to an edge keypoint.
    std::vector<int> IsPointValidForPlanar = this->IsPointValid[scanLine];

    // if the line is almost empty, skip it
    if (Npts < 3 * this->NeighborWidth)
    {
      continue;
    }

    // Sort the curvature score in a decreasing order
    std::vector<size_t> sortedDepthGapIdx = sortIdx<double>(this->DepthGap[scanLine]);
    std::vector<size_t> sortedAnglesIdx = sortIdx<double>(this->Angles[scanLine]);
    std::vector<size_t> sortedSaillancyIdx = sortIdx<double>(this->SaillantPoint[scanLine]);
    std::vector<size_t> sortedIntensityGap = sortIdx<double>(this->IntensityGap[scanLine]);

    double depthGap, sinAngle, saillancy, intensity;
    int index = 0;

    // Edges using depth gap
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedDepthGapIdx[k];
      depthGap = this->DepthGap[scanLine][index];

      // thresh
      if (depthGap < this->EdgeDepthGapThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth + 1;
      int indexEnd = index + this->NeighborWidth - 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using angles
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedAnglesIdx[k];
      sinAngle = this->Angles[scanLine][index];

      // thresh
      if (sinAngle < this->EdgeSinAngleThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth;
      int indexEnd = index + this->NeighborWidth;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using saillancy
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedSaillancyIdx[k];
      saillancy = this->SaillantPoint[scanLine][index];

      // thresh
      if (saillancy < 1.5)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth + 1;
      int indexEnd = index + this->NeighborWidth - 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using intensity
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedIntensityGap[k];
      intensity = this->IntensityGap[scanLine][index];

      // thresh
      if (intensity < 50.0)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhood
      int indexBegin = index - 1;
      int indexEnd = index + 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Blobs Points
    if (!this->FastSlam)
    {
      for (int k = 0; k < Npts; k = k + 3)
      {
        this->BlobIndex.push_back(std::pair<int, int>(scanLine, k));
      }
    }

    // Planes
    for (int k = Npts - 1; k >= 0; --k)
    {
      index = sortedAnglesIdx[k];
      sinAngle = this->Angles[scanLine][index];

      // thresh
      if (sinAngle > this->PlaneSinAngleThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (IsPointValidForPlanar[index] == 0)
      {
        continue;
      }

      // else indicate that the point is a planar one
      if ((this->Label[scanLine][index] != 4) && (this->Label[scanLine][index] != 3))
        this->Label[scanLine][index] = 2;
      this->PlanarIndex.push_back(std::pair<int, int>(scanLine, index));
      IsPointValidForPlanar[index] = 0;
      this->IsPointValid[scanLine][index] = 0;

      // Invalid its neighbor so that we don't have too
      // many planar keypoints in the same region. This is
      // required because of the k-nearest search + plane
      // approximation realized in the odometry part. Indeed,
      // if all the planar points are on the same scan line the
      // problem is degenerated since all the points are distributed
      // on a line.
      int indexBegin = index - 4;
      int indexEnd = index + 4;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        IsPointValidForPlanar[j] = 0;
      }
      nbrPlanarPicked++;
    }
  }

  // add keypoints in increasing scan id order
  std::sort(this->EdgesIndex.begin(), this->EdgesIndex.end());
  std::sort(this->PlanarIndex.begin(), this->PlanarIndex.end());
  std::sort(this->BlobIndex.begin(), this->BlobIndex.end());

  // fill the keypoints vectors and compute the max dist keypoints
  this->FarestKeypointDist = 0.0;
  Point p;
  for (unsigned int k = 0; k < this->EdgesIndex.size(); ++k)
  {
    p = this->pclCurrentFrameByScan[this->EdgesIndex[k].first]->points[this->EdgesIndex[k].second];
    this->CurrentEdgesPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::sqrt(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2))));
  }
  for (unsigned int k = 0; k < this->PlanarIndex.size(); ++k)
  {
    p = this->pclCurrentFrameByScan[this->PlanarIndex[k].first]->points[this->PlanarIndex[k].second];
    this->CurrentPlanarsPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::sqrt(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2))));
  }
  for (unsigned int k = 0; k < this->BlobIndex.size();  ++k)
  {
    p = this->pclCurrentFrameByScan[this->BlobIndex[k].first]->points[this->BlobIndex[k].second];
    this->CurrentBlobsPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::sqrt(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2))));
  }

  // Initialize the IsKeypointUsed vectors
  this->EdgePointRejectionEgoMotion.clear(); this->EdgePointRejectionEgoMotion.resize(this->CurrentEdgesPoints->size());
  this->PlanarPointRejectionEgoMotion.clear(); this->PlanarPointRejectionEgoMotion.resize(this->CurrentPlanarsPoints->size());
  this->EdgePointRejectionMapping.clear(); this->EdgePointRejectionMapping.resize(this->CurrentEdgesPoints->size());
  this->PlanarPointRejectionMapping.clear(); this->PlanarPointRejectionMapping.resize(this->CurrentPlanarsPoints->size());

  // keypoints extraction informations
  std::cout << "Extracted Edges: " << this->CurrentEdgesPoints->size() << " Planars: "
            << this->CurrentPlanarsPoints->size() << " Blobs: "
            << this->CurrentBlobsPoints->size() << std::endl;
}

//-----------------------------------------------------------------------------
void vtkSlam::TransformToWorld(Point& p)
{
  if (this->Undistortion)
  {
    this->ExpressPointInOtherReferencial(p, this->MappingInterpolator);
  }
  else
  {
    // Rotation and translation and points
    Eigen::Matrix3d Rw;
    Eigen::Vector3d Tw;
    Eigen::Vector3d P;

    Rw = GetRotationMatrix(this->Tworld);
    Tw << this->Tworld(3), this->Tworld(4), this->Tworld(5);
    P << p.x, p.y, p.z;

    P = Rw * P + Tw;

    p.x = P(0);
    p.y = P(1);
    p.z = P(2);
  }
}

//-----------------------------------------------------------------------------
int vtkSlam::ComputeLineDistanceParameters(pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousEdges, Eigen::Matrix3d& R,
                                                   Eigen::Vector3d& dT, Point p, std::string step)
{
  // number of neighbors edge points required to approximate
  // the corresponding egde line
  unsigned int requiredNearest;
  unsigned int eigenValuesRatio;

  // maximum distance between keypoints
  // and their computed line
  double maxDist;

  if (step == "egoMotion")
  {
    requiredNearest = 2;//this->EgoMotionLineDistanceNbrNeighbors;
    eigenValuesRatio = this->EgoMotionLineDistancefactor;
    maxDist = std::pow(this->EgoMotionMaxLineDistance, 2);
  }
  else if (step == "mapping")
  {
    requiredNearest = this->MappingLineDistanceNbrNeighbors;
    eigenValuesRatio = this->MappingLineDistancefactor;
    maxDist = std::pow(this->MappingMaxLineDistance, 2);
  }
  else
  {
    throw "ComputeLineDistanceParameters function got invalide step parameter";
  }

  Eigen::Vector3d P0, P, n;
  Eigen::Matrix3d A;

  // Transform the point using the current pose estimation
  P0 << p.x, p.y, p.z;

  if (this->Undistortion) // linear interpolated transform
  {
    if (step == "egoMotion")
    {
      this->ExpressPointInOtherReferencial(p, this->EgoMotionInterpolator);
    }
    else if (step == "mapping")
    {
      this->ExpressPointInOtherReferencial(p, this->MappingInterpolator);
    }
  }
  else // rigid transform
  {
    P = R * P0 + dT;
    p.x = P(0); p.y = P(1); p.z = P(2);
  }

  std::vector<int> nearestIndex;
  std::vector<float> nearestDist;

  if (step == "egoMotion")
  {
    GetEgoMotionLineSpecificNeighbor(nearestIndex, nearestDist, requiredNearest, kdtreePreviousEdges, p);
    if (nearestIndex.size() < this->EgoMotionMinimumLineNeighborRejection)
    {
      return 0;
    }
    requiredNearest = nearestIndex.size();
  }
  else if (step == "mapping")
  {
    GetMappingLineSpecificNeigbbor(nearestIndex, nearestDist, this->MappingLineMaxDistInlier, requiredNearest, kdtreePreviousEdges, p);
    if (nearestIndex.size() < this->MappingMinimumLineNeighborRejection)
    {
      return 0;
    }
    requiredNearest = nearestIndex.size();
    //kdtreePreviousEdges->nearestKSearch(p, requiredNearest, nearestIndex, nearestDist);
  }

  // if the nearest edges are too far from the
  // current edge keypoint we skip this point.
  double distScale = std::min(sqrt(pow(p.x,2) + pow(p.y,2) + pow(p.z,2)) / this->MinDistanceToSensor,10.0);
  if (nearestDist[requiredNearest - 1] > this->MaxDistanceForICPMatching * distScale)
  {
    return 1;
  }

  // Compute PCA to determine best line approximation
  // of the requiredNearest nearest edges points extracted
  // Thans to the PCA we will check the shape of the neighborhood
  // and keep it if it is distributed along a line
  Eigen::MatrixXd data(requiredNearest, 3);

  for (unsigned int k = 0; k < requiredNearest; k++)
  {
    Point pt = kdtreePreviousEdges->getInputCloud()->points[nearestIndex[k]];
    data.row(k) << pt.x, pt.y, pt.z;
  }

  Eigen::Vector3d mean = data.colwise().mean();
  Eigen::MatrixXd centered = data.rowwise() - mean.transpose();
  Eigen::MatrixXd cov = centered.transpose() * centered;
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);

  // Eigen values
  Eigen::MatrixXd D(1,3);
  // Eigen vectors
  Eigen::MatrixXd V(3,3);

  D = eig.eigenvalues();
  V = eig.eigenvectors();

  // if the first eigen value is significantly higher than
  // the second one, it means the sourrounding points are
  // distributed on a edge line
  if (D(2) > eigenValuesRatio * D(1))
  {
    // n is the director vector of the line
    n = V.col(2);
    n.normalized();
  }
  else
  {
    return 2;
  }

  // Compute a coefficient based on the linearity of the neighborhood.
  // We reject the neighborhood if D(2) < k * D(1) and we want our coefficient
  // varying from 0 to 1.0 with 0 being reached when D(2) = 5 * D(1)
  double linearityCoeff = 1.0 - eigenValuesRatio * D(1) / D(2);

  // A = (I-n*n.t).t * (I-n*n.t) = (I - n*n.t)^2
  // since (I-n*n.t) is a symmetric matrix
  // Then it comes A (I-n*n.t)^2 = (I-n*n.t) since
  // A is the matrix of a projection endomorphism
  A = (this->I3 - n * n.transpose());
  A = A.transpose() * A;

  // it would be the case if P1 = P2 For instance
  // if the sensor has some dual returns that hit the same point
  if (!vtkMath::IsFinite(A(0, 0)))
  {
    return 3;
  }

  // Evaluate the distance from the fitted line distribution
  // of the neighborhood
  Eigen::Vector3d Xtemp;
  Point pt;
  double meanSquaredDist = 0;
  for (unsigned int k = 0; k < requiredNearest; ++k)
  {
    pt = kdtreePreviousEdges->getInputCloud()->points[nearestIndex[k]];
    Xtemp(0) = pt.x; Xtemp(1) = pt.y; Xtemp(2) = pt.z;
    double squaredDist = (Xtemp - mean).transpose() * A * (Xtemp - mean);
    if (squaredDist > maxDist)
    {
      return 4;
    }
    meanSquaredDist += squaredDist;
  }
  meanSquaredDist /= static_cast<double>(requiredNearest);
  double fitQualityCoeff = 1.0 - std::sqrt(meanSquaredDist / maxDist);

  // distance between current point and the corresponding matching line
  double s = 1.0;
  if (step == "mapping")
  {
    s = 0.5 * fitQualityCoeff + 0.5 * linearityCoeff;
  }
  else if (step == "egoMotion")
  {
    double orthogonalityCoeff = 0.5 + 0.5 * n(2) * n(2); // score the match by its angle with ez
    // Score the point - line matching by the angle of the
    // line with ez. The idea is that the lidar is more accurate
    // in line detection when those lines are colinear with the
    // azimutal rotation axis.
    s = 0.33 * fitQualityCoeff + 0.33 * linearityCoeff + 0.33 * orthogonalityCoeff;
  }

  if (s <= 0 || !vtkMath::IsFinite(s))
    return 5;

  // store the distance parameters values
  this->Avalues.push_back(A);
  this->Pvalues.push_back(mean);
  this->Xvalues.push_back(P0);
  this->TimeValues.push_back(p.intensity);
  this->residualCoefficient.push_back(s);
  this->RadiusIncertitude.push_back(0.0);
  return 6;
}

//-----------------------------------------------------------------------------
int vtkSlam::ComputePlaneDistanceParameters(pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousPlanes, Eigen::Matrix3d& R,
                                                    Eigen::Vector3d& dT, Point p, std::string step)
{
  // number of neighbors edge points required to approximate
  // the corresponding egde line
  unsigned int requiredNearest;
  unsigned int significantlyFactor1, significantlyFactor2;

  // maximum distance between keypoints
  // and their computed plane
  double maxDist;

  if (step == "egoMotion")
  {
    significantlyFactor1 = this->EgoMotionPlaneDistancefactor1;
    significantlyFactor2 = this->EgoMotionPlaneDistancefactor2;
    requiredNearest = this->EgoMotionPlaneDistanceNbrNeighbors;
    maxDist = std::pow(this->EgoMotionMaxPlaneDistance, 2);
  }
  else if (step == "mapping")
  {
    significantlyFactor1 = this->MappingPlaneDistancefactor1;
    significantlyFactor2 = this->MappingPlaneDistancefactor2;
    requiredNearest = this->MappingPlaneDistanceNbrNeighbors;
    maxDist = std::pow(this->MappingMaxPlaneDistance, 2);
  }
  else
  {
    throw "ComputeLineDistanceParameters function got invalide step parameter";
  }

  Eigen::Vector3d P0, P, n;
  Eigen::Matrix3d A;

  // Transform the point using the current pose estimation
  P0 << p.x, p.y, p.z;

  if (this->Undistortion) // linear interpolated transform
  {
    if (step == "egoMotion")
    {
      this->ExpressPointInOtherReferencial(p, this->EgoMotionInterpolator);
    }
    else if (step == "mapping")
    {
      this->ExpressPointInOtherReferencial(p, this->MappingInterpolator);
    }
  }
  else // rigid transform
  {
    P = R * P0 + dT;
    p.x = P(0); p.y = P(1); p.z = P(2);
  }

  std::vector<int> nearestIndex;
  std::vector<float> nearestDist;
  kdtreePreviousPlanes->nearestKSearch(p, requiredNearest, nearestIndex, nearestDist);

  // It means that there is not enought keypoints in the neighbohood
  if (nearestIndex.size() < requiredNearest)
  {
    return 0;
  }

  // if the nearest planars are too far from the
  // current planar keypoint we skip this point.
  double distScale = std::min(sqrt(pow(p.x,2) + pow(p.y,2) + pow(p.z,2)) / this->MinDistanceToSensor,10.0);
  if (nearestDist[requiredNearest - 1] > this->MaxDistanceForICPMatching * distScale)
  {
    return 1;
  }

  // Compute PCA to determine best line approximation
  // of the requiredNearest nearest edges points extracted
  // Thanks to the PCA we will check the shape of the neighborhood
  // and keep it if it is distributed along a line
  Eigen::MatrixXd data(requiredNearest,3);

  for (unsigned int k = 0; k < requiredNearest; k++)
  {
    Point pt = kdtreePreviousPlanes->getInputCloud()->points[nearestIndex[k]];
    data.row(k) << pt.x, pt.y, pt.z;
  }

  Eigen::Vector3d mean = data.colwise().mean();
  Eigen::MatrixXd centered = data.rowwise() - mean.transpose();
  Eigen::MatrixXd cov = centered.transpose() * centered;
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);

  // Eigen values
  Eigen::MatrixXd D(1,3);
  // Eigen vectors
  Eigen::MatrixXd V(3,3);

  D = eig.eigenvalues();
  V = eig.eigenvectors();

  // if the second eigen value is close to the highest one
  // and bigger than the smallest one it means that the points
  // are distributed among a plane
  Eigen::Vector3d u, v;
  if ( (significantlyFactor2 * D(1) > D(2)) && (D(1) > significantlyFactor1 * D(0)) )
  {
    u = V.col(2);
    v = V.col(1);
  }
  else
  {
    return 2;
  }

  // Compute a coefficient based on the planarity of the neighborhood.
  double planarityCoeff = (D(1) - D(0)) / D(2);

  n = u.cross(v);
  n.normalized();

  // A = n*n.t
  A = n * n.transpose();

  // it would be the case if P1 = P2, P1 = P3
  // or P3 = P2. For instance if the sensor has
  // some dual returns that hit the same point
  if (!vtkMath::IsFinite(A(0, 0)))
  {
    return 3;
  }

  Eigen::Vector3d Xtemp;
  Point pt;
  double meanSquaredDist = 0;
  for (unsigned int k = 0; k < requiredNearest; ++k)
  {
    pt = kdtreePreviousPlanes->getInputCloud()->points[nearestIndex[k]];
    Xtemp(0) = pt.x; Xtemp(1) = pt.y; Xtemp(2) = pt.z;
    double squaredDist = (Xtemp - mean).transpose() * A * (Xtemp - mean);
    if (squaredDist > maxDist)
    {
      return 4;
    }
    meanSquaredDist += squaredDist;
  }
  meanSquaredDist /= static_cast<double>(requiredNearest);
  double fitQualityCoeff = 1.0 - std::sqrt(meanSquaredDist / maxDist);

  // distance between current point and the corresponding matching plane
  double s = 0.5 * fitQualityCoeff + 0.5 * planarityCoeff;

  if (s <= 0 || !vtkMath::IsFinite(s))
    return 5;

  // store the distance parameters values
  this->Avalues.push_back(A);
  this->Pvalues.push_back(mean);
  this->Xvalues.push_back(P0);
  this->residualCoefficient.push_back(s);
  this->TimeValues.push_back(p.intensity);
  this->RadiusIncertitude.push_back(0.0);
  return 6;
}

//-----------------------------------------------------------------------------
int vtkSlam::ComputeBlobsDistanceParameters(pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousBlobs, Eigen::Matrix3d& R,
                                                    Eigen::Vector3d& dT, Point p, std::string vtkNotUsed(step))
{
  // number of neighbors blobs points required to approximate
  // the corresponding ellipsoide
  unsigned int requiredNearest = 25;

  // maximum distance between keypoints
  // and its neighbor
  double maxDist = this->MaxDistanceForICPMatching;
  float maxDiameterTol = std::pow(4.0, 2);

  // Usefull variables
  Eigen::Vector3d P0, P, n;
  Eigen::Matrix3d A;

  // Transform the point using the current pose estimation
  P << p.x, p.y, p.z;
  P0 = P;
  P = R * P + dT;
  p.x = P(0); p.y = P(1); p.z = P(2);

  std::vector<int> nearestIndex;
  std::vector<float> nearestDist;
  kdtreePreviousBlobs->nearestKSearch(p, requiredNearest, nearestIndex, nearestDist);

  // It means that there is not enought keypoints in the neighbohood
  if (nearestIndex.size() < requiredNearest)
  {
    return 0;
  }

  // if the nearest blobs is too far from the
  // current blob keypoint we skip this point.
  if (nearestDist[requiredNearest - 1] > maxDist)
  {
    return 1;
  }

  // check the diameter of the neighborhood
  // if the diameter is too big we don't want
  // to keep this blobs. We must do that since
  // the blobs fitted ellipsoide is assume to
  // encode the local neighborhood shape.
  float maxDiameter = 0;
  for (unsigned int i = 0; i < requiredNearest; ++i)
  {
    for (unsigned int j = 0; j < requiredNearest; ++j)
    {
      Point pt1 = kdtreePreviousBlobs->getInputCloud()->points[nearestIndex[i]];
      Point pt2 = kdtreePreviousBlobs->getInputCloud()->points[nearestIndex[j]];
      float neighborhoodDiameter = std::pow(pt1.x - pt2.x, 2) + std::pow(pt1.y - pt2.y, 2) + std::pow(pt1.z - pt2.z, 2);
      maxDiameter = std::max(maxDiameter, neighborhoodDiameter);
    }
  }
  if (maxDiameter > maxDiameterTol)
  {
    return 2;
  }

  // Compute PCA to determine best ellipsoide approximation
  // of the requiredNearest nearest blobs points extracted
  // Thanks to the PCA we will check the shape of the neighborhood
  // tune a distance function adapter to the distribution
  // (Mahalanobis distance)
  Eigen::MatrixXd data(requiredNearest, 3);

  for (unsigned int k = 0; k < requiredNearest; k++)
  {
    Point pt = kdtreePreviousBlobs->getInputCloud()->points[nearestIndex[k]];
    data.row(k) << pt.x, pt.y, pt.z;
  }

  Eigen::Vector3d mean = data.colwise().mean();
  Eigen::MatrixXd centered = data.rowwise() - mean.transpose();
  Eigen::MatrixXd cov = centered.transpose() * centered;

  // Sigma is the inverse of the covariance
  // Matrix encoding the mahalanobis distance
  // check that the covariance matrix is inversible
  if (std::abs(cov.determinant()) < 1e-6)
  {
    return 3;
  }
  Eigen::MatrixXd sigma = cov.inverse();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(sigma);

  // rescale the variance covariance matrix to preserve the
  // shape of the mahalanobis distance but removing the
  // variance values scaling
  Eigen::MatrixXd D = eig.eigenvalues();
  Eigen::MatrixXd U = eig.eigenvectors();
  D = D / D(2);
  Eigen::Matrix3d diagD = Eigen::Matrix3d::Zero();
  diagD(0, 0) = D(0); diagD(1, 1) = D(1); diagD(2, 2) = D(2);
  A = U * diagD * U.transpose();

  if (!vtkMath::IsFinite(A.determinant()))
  {
    return 4;
  }

  // Coefficient the distance
  // using the distance between the point
  // and its matching blob; The aim is to prevent
  // wrong matching to pull the point cloud in the
  // bad direction
  double s = 1.0;//1.0 - nearestDist[requiredNearest - 1] / maxDist;

  // store the distance parameters values
  this->Avalues.push_back(A);
  this->Pvalues.push_back(mean);
  this->Xvalues.push_back(P0);
  this->residualCoefficient.push_back(s);
  this->RadiusIncertitude.push_back(0.0);
  return 5;
}

//-----------------------------------------------------------------------------
void vtkSlam::GetEgoMotionLineSpecificNeighbor(std::vector<int>& nearestValid, std::vector<float>& nearestValidDist,
                                               unsigned int nearestSearch, pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousEdges, Point p)
{
  // clear vector
  nearestValid.clear();
  nearestValid.resize(0);
  nearestValidDist.clear();
  nearestValidDist.resize(0);

  // get nearest neighbor of the query point
  std::vector<int> nearestIndex;
  std::vector<float> nearestDist;
  kdtreePreviousEdges->nearestKSearch(p, nearestSearch*2, nearestIndex, nearestDist);

  // take the closest point
  std::vector<int> idAlreadyTook(this->NLasers, 0);
  Point closest = kdtreePreviousEdges->getInputCloud()->points[nearestIndex[0]];
  nearestValid.push_back(nearestIndex[0]);
  nearestValidDist.push_back(nearestDist[0]);

  // invalid all possible points that
  // are on the same scan line than the
  // closest one
  idAlreadyTook[(int)closest.normal_y] = 1;

  // invalid all possible points from scan
  // lines that are too far from the closest one
  for (unsigned int k = 0; k < this->NLasers; ++k)
  {
    if (std::abs(closest.normal_y - k) > 3)
    {
      idAlreadyTook[k] = 1;
    }
  }

  // Make a selection among the neighborhood
  // of the query point. We can only take one edge
  // per scan line
  int id;
  for (unsigned int k = 1; k < nearestIndex.size(); ++k)
  {
    id = kdtreePreviousEdges->getInputCloud()->points[nearestIndex[k]].normal_y;
    if (idAlreadyTook[id] < 1)
    {
      idAlreadyTook[id] = 1;
      nearestValid.push_back(nearestIndex[k]);
      nearestValidDist.push_back(nearestDist[k]);
    }
  }
}

//-----------------------------------------------------------------------------
void vtkSlam::GetMappingLineSpecificNeigbbor(std::vector<int>& nearestValid, std::vector<float>& nearestValidDist, double maxDistInlier,
                                             unsigned int nearestSearch, pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousEdges, Point p)
{
  // reset vectors
  nearestValid.clear();
  nearestValid.resize(0);
  nearestValidDist.clear();
  nearestValidDist.resize(0);

  // to prevent square root when making camparisons
  maxDistInlier = std::pow(maxDistInlier, 2);

  // Take the neighborhood of the query point
  // get nearest neighbor of the query point
  std::vector<int> nearestIndex;
  std::vector<float> nearestDist;
  kdtreePreviousEdges->nearestKSearch(p, nearestSearch, nearestIndex, nearestDist);

  // take the closest point
  std::vector<std::vector<int> > inliersList;
  Point closest = kdtreePreviousEdges->getInputCloud()->points[nearestIndex[0]];
  nearestValid.push_back(nearestIndex[0]);
  nearestValidDist.push_back(nearestDist[0]);

  Eigen::Vector3d P1, P2, dir, Pcdt;
  Eigen::Matrix3d D;
  P1 << closest.x, closest.y, closest.z;
  Point pclP2;
  Point inlierCandidate;

  // Loop over other neighbors of the neighborhood. For each of them
  // compute the line between closest point and current point and
  // compute the number of inlier that fit this line. Keep the line and its
  // inmliers with the most inliers
  for (unsigned int ptIndex = 1; ptIndex < nearestIndex.size(); ++ptIndex)
  {
    std::vector<int> inlierIndex;
    pclP2 = kdtreePreviousEdges->getInputCloud()->points[nearestIndex[ptIndex]];
    P2 << pclP2.x, pclP2.y, pclP2.z;
    dir = (P2 - P1).normalized();
    D = this->I3 - dir * dir.transpose();
    D = D.transpose() * D;

    for (unsigned int candidateIndex = 1; candidateIndex < nearestIndex.size(); ++candidateIndex)
    {
      inlierCandidate = kdtreePreviousEdges->getInputCloud()->points[nearestIndex[candidateIndex]];
      Pcdt << inlierCandidate.x, inlierCandidate.y, inlierCandidate.z;
      if ( (Pcdt - P1).transpose() * D * (Pcdt - P1) < maxDistInlier)
      {
        inlierIndex.push_back(candidateIndex);
      }
    }
    inliersList.push_back(inlierIndex);
  }

  std::size_t maxInliers = 0;
  int indexMaxInliers = -1;
  for (unsigned int k = 0; k < inliersList.size(); ++k)
  {
    if (inliersList[k].size() > maxInliers)
    {
      maxInliers = inliersList[k].size();
      indexMaxInliers = k;
    }
  }

  // fill
  for (unsigned int k = 0; k < inliersList[indexMaxInliers].size(); ++k)
  {
    nearestValid.push_back(nearestIndex[inliersList[indexMaxInliers][k]]);
    nearestValidDist.push_back(nearestDist[inliersList[indexMaxInliers][k]]);
  }

  return;
}

//-----------------------------------------------------------------------------
void vtkSlam::ComputeImuMotion()
{
  Eigen::Vector3d avgAcc, Acc;
  Eigen::Vector3d avgGyro, Gyro;
  avgAcc << 0,0,0;
  avgGyro << 0,0,0;
  Acc << 0,0,0;
  Gyro << 0,0,0;
  double timeDiff;
  this->ImuTrelative = Eigen::Matrix<double, 6, 1>::Zero();

  if (this->UsingImu && this->CurrentFrameTime != this->PreviousFrameTime)
  {
      int count = 0, row = this->ImuDataRow;
      while (this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row) > this->PreviousFrameTime && row > 0)
      {
        row--;
      }
      while (this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row) <= this->PreviousFrameTime)
      {
        row++;
      }
      auto imuTable = this->ImuData->GetRowData();

      while (this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row) <= this->CurrentFrameTime)
      {

        avgAcc(0) += imuTable->GetArray("Acc_X")->GetTuple1(row);
        avgAcc(1) += imuTable->GetArray("Acc_Y")->GetTuple1(row);
        avgAcc(2) += imuTable->GetArray("Acc_Z")->GetTuple1(row);
        avgGyro(0) += Deg2Rad(imuTable->GetArray("Gyro_X")->GetTuple1(row));
        avgGyro(1) += Deg2Rad(imuTable->GetArray("Gyro_Y")->GetTuple1(row));
        avgGyro(2) += Deg2Rad(imuTable->GetArray("Gyro_Z")->GetTuple1(row));

        count++;
        row++;

      }
      avgAcc /= count;
      avgGyro /= count;

      Eigen::Matrix<double, 6, 1> calib;
      calib << 0, 0, 0, 0, 0, 0;
      Eigen::Matrix3d calibR = GetRotationMatrix(calib);

      timeDiff = (this->CurrentFrameTime - this->PreviousFrameTime) * 1e-6;

      //adjust gyro
      Eigen::Vector3d avgGyroC;// = calibR * avgGyro;
      avgGyroC << avgGyro(1), avgGyro(0), -avgGyro(2);

      //anglular change
      Eigen::Vector3d angularChangeC = avgGyroC*timeDiff;

      //angular change matrix
      Eigen::Matrix<double,6,1> angularCM;
      angularCM << angularChangeC,0,0,0;

      //Angle change rotation matrix
      Eigen::Matrix3d angularRotationCM = GetRotationMatrix(angularCM);

      // Angle change rotation matrix calibrated0

      // acceleration calibrated
      Eigen::Vector3d avgAccC;// = calibR * avgAcc;
      avgAccC << avgAcc(1), avgAcc(0), -avgAcc(2);

      // velocity change
      Eigen::Vector3d velocityChange = avgAccC * timeDiff;

      //linear velocity
      Eigen::Vector3d linearChange = (this->Velocity + (velocityChange * 0.5)) * timeDiff;
      this->Velocity += velocityChange;

      Eigen::Vector3d transform;
      transform = angularRotationCM * linearChange;

      double rx = std::atan2(angularRotationCM(2, 1), angularRotationCM(2, 2));
      double ry = -std::asin(angularRotationCM(2, 0));
      double rz = std::atan2(angularRotationCM(1, 0), angularRotationCM(0, 0));

      this->ImuTrelative << rx, ry, rz, transform;

      this->Trelative << ImuTrelative;

      this->ImuVariation << abs(this->ImuTrelative(0) * (1 - this->ImuConfidence(0))),
        abs(this->ImuTrelative(1) * (1 - this->ImuConfidence(1))),
        abs(this->ImuTrelative(2) * (1 - this->ImuConfidence(2))),
        abs(this->ImuTrelative(3) * (1 - this->ImuConfidence(3))),
        abs(this->ImuTrelative(4) * (1 - this->ImuConfidence(4))),
        abs(this->ImuTrelative(5) * (1 - this->ImuConfidence(5)));
      std::cout << "ImuVariation: " << this->ImuVariation.transpose() << std::endl;

      std::cout << "========== IMU-Motion ==========" << std::endl;
      std::cout << "Time: " << timeDiff << std::endl;
      std::cout << "row: " << row << std::endl;
      std::cout << "count: " << count << std::endl;
      std::cout << "Gyro: " << avgGyro.transpose() << std::endl;
      std::cout << "GyroC: " << avgGyroC.transpose() << std::endl;
      //std::cout << "angularChangeC: " << angularChangeC.transpose() << std::endl;
      std::cout << "Acc: " << avgAcc.transpose() << std::endl;
      std::cout << "AccC: " << avgAccC.transpose() << std::endl;
      std::cout << "Velocity: " << this->Velocity.transpose() << std::endl;

      if (this->OnlyImu) //for testing only
      {
        UpdateTworldUsingTrelative();
      }
  }

}

void vtkSlam::ComputeImuMotionTest()
{
  Eigen::Vector3d avgAcc, Acc;
  Eigen::Vector3d avgGyro, Gyro;
  avgAcc << 0,0,0;
  avgGyro << 0,0,0;
  Acc << 0,0,0;
  Gyro << 0,0,0;
  double timeDiff;
  this->ImuTrelative = Eigen::Matrix<double, 6, 1>::Zero();

  if (this->UsingImu && this->CurrentFrameTime != this->PreviousFrameTime)
  {
      int count = 0, row = this->ImuDataRow;
      while (this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row) > this->PreviousFrameTime && row > 0)
      {
        row--;
      }
      while (this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row) <= this->PreviousFrameTime)
      {
        row++;
      }
      auto imuTable = this->ImuData->GetRowData();

      while (this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row) <= this->CurrentFrameTime)
      {
        timeDiff = (this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row) - this->ImuData->GetRowData()->GetArray("adjustedtime")->GetTuple1(row - 1)) * 1e-6;
        Acc(0) = (imuTable->GetArray("Acc_X")->GetTuple1(row) + imuTable->GetArray("Acc_X")->GetTuple1(row - 1)) / 2;
        Acc(1) = (imuTable->GetArray("Acc_Y")->GetTuple1(row) + imuTable->GetArray("Acc_Y")->GetTuple1(row - 1)) / 2;
        Acc(2) = (imuTable->GetArray("Acc_Z")->GetTuple1(row) + imuTable->GetArray("Acc_Z")->GetTuple1(row - 1)) / 2;
        Gyro(0) = Deg2Rad(imuTable->GetArray("Gyro_X")->GetTuple1(row));// + imuTable->GetArray("Gyro_X")->GetTuple1(row - 1) / 2);
        Gyro(1) = Deg2Rad(imuTable->GetArray("Gyro_Y")->GetTuple1(row));// + imuTable->GetArray("Gyro_Y")->GetTuple1(row - 1) / 2);
        Gyro(2) = Deg2Rad(imuTable->GetArray("Gyro_Z")->GetTuple1(row));// + imuTable->GetArray("Gyro_Z")->GetTuple1(row - 1) / 2);

        Eigen::Vector3d AccC;// = calibR * avgAcc;
        //AccC << Acc(0), -Acc(1), -Acc(2);
        //AccC << Acc;

        Eigen::Vector3d GyroC;// = calibR * avgGyro;
        //GyroC << Gyro(0), -Gyro(1), -Gyro(2);
        GyroC << Gyro;

        //anglular change
        Eigen::Vector3d angularChangeC = GyroC*timeDiff;

        //angular change matrix
        Eigen::Matrix<double,6,1> angularCM;
        angularCM << angularChangeC,0,0,0;

        //Angle change rotation matrix
        Eigen::Matrix3d angularRotationCM = GetRotationMatrix(angularCM);

        Eigen::Matrix3d ImuTrRotationM = GetRotationMatrix(this->ImuTrelative);

        Eigen::Matrix3d newImuTrRotation = ImuTrRotationM * angularRotationCM;

        double rx = std::atan2(newImuTrRotation(2, 1), newImuTrRotation(2, 2));
        double ry = -std::asin(newImuTrRotation(2, 0));
        double rz = std::atan2(newImuTrRotation(1, 0), newImuTrRotation(0, 0));

        AccC << ((Acc(0) * sin(angularChangeC(2))) + (Acc(1) * (cos(angularChangeC(2)) - 1 )))/GyroC(2),//((Acc(0) * sin(angularChangeC(2))) + (Acc(1) * (1 - cos(angularChangeC(2)))))/GyroC(2),
                0,//((Acc(0) * (1-cos(angularChangeC(2)))) + (Acc(1) * sin(angularChangeC(2))))/GyroC(2),
                0;//Acc(2) * timeDiff;

        Eigen::Vector3d velocityChange =  AccC;// * timeDiff;

        //linear velocity
        Eigen::Vector3d linearChange = (this->Velocity + (velocityChange * 0.5)) * timeDiff;
        this->Velocity += velocityChange;

        Eigen::Vector3d transform;
        transform = angularRotationCM * linearChange;

        Eigen::Vector3d ImuTrTransform;
        ImuTrTransform << this->ImuTrelative(3), this->ImuTrelative(4), this->ImuTrelative(5);

        Eigen::Vector3d newImuTrTransform = (ImuTrRotationM * transform) + ImuTrTransform;

        this->ImuTrelative << rx, ry, rz, newImuTrTransform;

        /*std::cout << "//////////////////////////////////////////////" << std::endl;
        std::cout << "Time: " << timeDiff << std::endl;
        std::cout << "row: " << row << std::endl;
        std::cout << "count: " << count << std::endl;
        std::cout << "Gyro: " << Gyro.transpose() << std::endl;
        std::cout << "GyroC: " << GyroC.transpose() << std::endl;
        std::cout << "angularChangeC: " << angularChangeC.transpose() << std::endl;
        std::cout << "Acc: " << Acc.transpose() << std::endl;
        std::cout << "AccC: " << AccC.transpose() << std::endl;
        std::cout << "linearChangeC: " << linearChange.transpose() << std::endl;*/

        //count++;
        row++;

      }

      this->Trelative << ImuTrelative;

      this->ImuVariation << abs(this->ImuTrelative(0) * (1 - this->ImuConfidence(0))),
        abs(this->ImuTrelative(1) * (1 - this->ImuConfidence(1))),
        abs(this->ImuTrelative(2) * (1 - this->ImuConfidence(2))),
        abs(this->ImuTrelative(3) * (1 - this->ImuConfidence(3))),
        abs(this->ImuTrelative(4) * (1 - this->ImuConfidence(4))),
        abs(this->ImuTrelative(5) * (1 - this->ImuConfidence(5)));
      std::cout << "ImuVariation: " << this->ImuVariation.transpose() << std::endl;

      std::cout << "========== IMU-Motion ==========" << std::endl;
      std::cout << "Time: " << timeDiff << std::endl;
      std::cout << "row: " << row << std::endl;
      std::cout << "count: " << count << std::endl;
      //std::cout << "Gyro: " << avgGyro.transpose() << std::endl;
      //std::cout << "GyroC: " << avgGyroC.transpose() << std::endl;
      //std::cout << "angularChangeC: " << angularChangeC.transpose() << std::endl;
      //std::cout << "Acc: " << avgAcc.transpose() << std::endl;
      //std::cout << "AccC: " << avgAccC.transpose() << std::endl;
      std::cout << "Velocity: " << this->Velocity.transpose() << std::endl;

      if (this->OnlyImu) //for testing only
      {
        UpdateTworldUsingTrelative();
      }
  }

}

//-----------------------------------------------------------------------------
void vtkSlam::ComputeEgoMotion()
{
  // Check that there is enought points to compute the EgoMotion
  if ((this->CurrentEdgesPoints->size() == 0 || this->PreviousEdgesPoints->size() == 0) &&
      (this->CurrentPlanarsPoints->size() == 0 || this->PreviousPlanarsPoints->size() == 0))
  {
    this->FillEgoMotionInfoArrayWithDefaultValues();
    vtkGenericWarningMacro("Not enought keypoints, EgoMotion skipped for this frame");
    return;
  }

  // reset the relative transform
  if (this->UsingImu && this->CurrentFrameTime != this->PreviousFrameTime)
  {
    this->Trelative = this->ImuTrelative;
  }
  else
  {
    this->Trelative = Eigen::Matrix<double, 6, 1>::Zero();
  }

  // kd-tree to process fast nearest neighbor
  // among the keypoints of the previous pointcloud
  pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousEdges(new pcl::KdTreeFLANN<Point>());
  pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousPlanes(new pcl::KdTreeFLANN<Point>());
  pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousBlobs(new pcl::KdTreeFLANN<Point>());
  kdtreePreviousEdges->setInputCloud(this->PreviousEdgesPoints);
  kdtreePreviousPlanes->setInputCloud(this->PreviousPlanarsPoints);
  kdtreePreviousBlobs->setInputCloud(this->PreviousBlobsPoints);

  std::cout << "========== Ego-Motion ==========" << std::endl;
  std::cout << "previous <-> current edges : " << this->PreviousEdgesPoints->size() << " <-> " << this->CurrentEdgesPoints->size()
            << "previous <-> current planes : " << this->PreviousPlanarsPoints->size() << " <-> " << this->CurrentPlanarsPoints->size() << std::endl;

  unsigned int usedEdges = 0;
  unsigned int usedPlanes = 0;
  Point currentPoint, transformedPoint;

  std::cout << "Trelative: " << this->Trelative.transpose() << std::endl;
  // ICP - Levenberg-Marquardt loop:
  // At each step of this loop an ICP matching is performed
  // Once the keypoints matched, we estimate the the 6-DOF
  // parameters by minimizing a non-linear least square cost
  // function using a Levenberg-Marquardt algorithm
  for (unsigned int icpCount = 0; icpCount < this->EgoMotionICPMaxIter; ++icpCount)
  {
    // Rotation and translation at this step
    Eigen::Matrix3d R = GetRotationMatrix(this->Trelative);
    Eigen::Vector3d T;
    T << this->Trelative(3), this->Trelative(4), this->Trelative(5);

    // clear all keypoints matching data
    this->ResetDistanceParameters();

    // Init the undistortion interpolator
    if (this->Undistortion)
    {
      this->EgoMotionInterpolator = this->InitUndistortionInterpolatorEgoMotion();
    }

    // loop over edges
    for (unsigned int edgeIndex = 0; edgeIndex < this->CurrentEdgesPoints->size(); ++edgeIndex)
    {
      currentPoint = this->CurrentEdgesPoints->points[edgeIndex];

      // Find the closest correspondence edge line of the current edge point
      if ((this->PreviousEdgesPoints->size() > 7) && (this->CurrentEdgesPoints->size() > 0))
      {
        // Compute the parameters of the point - line distance
        // i.e A = (I - n*n.t)^2 with n being the director vector
        // and P a point of the line
        int rejectionIndex = this->ComputeLineDistanceParameters(kdtreePreviousEdges, R, T, currentPoint, "egoMotion");
        this->EdgePointRejectionEgoMotion[edgeIndex] = rejectionIndex;
        this->MatchRejectionHistogramLine[rejectionIndex] += 1;
      }
    }

    // loop over surfaces
    for (unsigned int planarIndex = 0; planarIndex < this->CurrentPlanarsPoints->size(); ++planarIndex)
    {
      currentPoint = this->CurrentPlanarsPoints->points[planarIndex];

      // Find the closest correspondence plane of the current planar point
      if ((this->PreviousPlanarsPoints->size() > 7) && (this->CurrentPlanarsPoints->size() > 0))
      {
        // Compute the parameters of the point - plane distance
        // i.e A = n * n.t with n being a normal of the plane
        // and is a point of the plane
        int rejectionIndex = this->ComputePlaneDistanceParameters(kdtreePreviousPlanes, R, T, currentPoint, "egoMotion");
        this->PlanarPointRejectionEgoMotion[planarIndex] = rejectionIndex;
        this->MatchRejectionHistogramPlane[rejectionIndex] += 1;
      }
    }

    usedEdges = this->MatchRejectionHistogramLine[6];
    usedPlanes = this->MatchRejectionHistogramPlane[6];
    // Skip this frame if there is too few geometric
    // keypoints matched
    if ((usedPlanes + usedEdges) < 20)
    {
      vtkGenericWarningMacro("Too few geometric features, frame skipped");
      break;
    }

    // We want to estimate our 6-DOF parameters using a non
    // linear least square minimization. The non linear part
    // comes from the Euler Angle parametrization of the rotation
    // endomorphism SO(3). To minimize it we use CERES to perform
    // the Levenberg-Marquardt algorithm.
    ceres::Problem problem;
    for (unsigned int k = 0; k < Xvalues.size(); ++k)
    {
      if (this->Undistortion)
      {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctions::MahalanobisDistanceLinearDistortionResidual, 1, 6>(
                                              new CostFunctions::MahalanobisDistanceLinearDistortionResidual(
                                                this->Avalues[k], this->Pvalues[k], this->Xvalues[k],
                                                Eigen::Matrix<double, 3, 1>::Zero(),
                                                Eigen::Matrix<double, 3, 3>::Identity(),
                                                this->TimeValues[k], this->residualCoefficient[k]));
        problem.AddResidualBlock(cost_function, new ceres::ArctanLoss(2.0), this->Trelative.data());
      }
      else
      {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctions::MahalanobisDistanceAffineIsometryResidual, 1, 6>(
                                             new CostFunctions::MahalanobisDistanceAffineIsometryResidual(this->Avalues[k], this->Pvalues[k],
                                                                                this->Xvalues[k], this->residualCoefficient[k]));
        problem.AddResidualBlock(cost_function, new ceres::ArctanLoss(2.0), this->Trelative.data());
      }
    }

    ceres::Solver::Options options;
    options.max_num_iterations = this->EgoMotionLMMaxIter;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = false;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;

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

  // Provide information about keypoints-neighborhood matching rejections
  this->RejectionInformationDisplay();

  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("EgoMotion: edges used"))->InsertNextValue(usedEdges);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("EgoMotion: planes used"))->InsertNextValue(usedPlanes);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("EgoMotion: total keypoints used"))->InsertNextValue(this->Xvalues.size());
  std::cout << "used keypoints : " << this->Xvalues.size() << std::endl;
  std::cout << "edges : " << usedEdges << " planes : " << usedPlanes << std::endl;
  // Integrate the relative motion
  // to the world transformation

  if (this->UsingImu && this->CurrentFrameTime != this->PreviousFrameTime)
  {

    // ensures change made by SLAM is within its limits set by the IMU confidence levels
    for (int i = 0; i < this->Trelative.rows(); i++)
    {
      if (this->Trelative(i) < this->ImuTrelative(i) - this->ImuVariation(i))
      {
        this->Trelative(i) = this->ImuTrelative(i) - this->ImuVariation(i);
        std::cout << "Correction limit hit on: " << i << std::endl;
      }
      else if (this->Trelative(i) > this->ImuTrelative(i) + this->ImuVariation(i))
      {
        this->Trelative(i) = this->ImuTrelative(i) + this->ImuVariation(i);
        std::cout << "Correction limit hit on: " << i << std::endl;
      }
    }

    //Velocity error correction
    Eigen::Matrix3d Trr = GetRotationMatrix(this->Trelative);
    Eigen::Vector3d Trt;
    Trt <<  this->Trelative(3), this->Trelative(4), this->Trelative(5);

    Eigen::Vector3d TrLinearDist = Trr.transpose() * Trt;

    Eigen::Matrix3d Imur = GetRotationMatrix(this->ImuTrelative);
    Eigen::Vector3d Imut;
    Imut <<  this->ImuTrelative(3), this->ImuTrelative(4), this->ImuTrelative(5);

    Eigen::Vector3d ImuLinearDist = Imur.transpose() * Imut;

    Eigen::Vector3d ErrorDist = TrLinearDist - ImuLinearDist;

    double timeDiff = this->CurrentFrameTime - this->PreviousFrameTime;

    Eigen::Vector3d ErrorVelocity = ErrorDist/timeDiff;

    this->Velocity -= 0.5 * ErrorVelocity;

  }

  this->UpdateTworldUsingTrelative();
}

//-----------------------------------------------------------------------------
void vtkSlam::Mapping()
{
  // Check that there is enought points to compute the EgoMotion
  if (this->CurrentEdgesPoints->size() == 0 && this->CurrentPlanarsPoints->size() == 0 || true)
  {
    this->FillMappingInfoArrayWithDefaultValues();
    // update maps
    this->UpdateMapsUsingTworld();
    vtkGenericWarningMacro("Not enought keypoints, Mapping skipped for this frame");
    return;
  }

  // contruct kd-tree for fast search
  pcl::KdTreeFLANN<Point>::Ptr kdtreeEdges(new pcl::KdTreeFLANN<Point>());
  pcl::KdTreeFLANN<Point>::Ptr kdtreePlanes(new pcl::KdTreeFLANN<Point>());
  pcl::KdTreeFLANN<Point>::Ptr kdtreeBlobs(new pcl::KdTreeFLANN<Point>());

  // Set the FarestPoint to reduce the map to the minimun since
  this->SetLidarMaximunRange(this->FarestKeypointDist);

  pcl::PointCloud<Point>::Ptr subEdgesPointsLocalMap = this->EdgesPointsLocalMap->Get(this->Tworld);
  pcl::PointCloud<Point>::Ptr subPlanarPointsLocalMap = this->PlanarPointsLocalMap->Get(this->Tworld);

  std::cout << "========== Mapping ==========" << std::endl;
  std::cout << "Edges extracted from map: " << subEdgesPointsLocalMap->points.size()
            << "Planes extracted from map: " << subPlanarPointsLocalMap->points.size() << std::endl;

  kdtreeEdges->setInputCloud(subEdgesPointsLocalMap);
  kdtreePlanes->setInputCloud(subPlanarPointsLocalMap);

  if (!this->FastSlam)
  {
    pcl::PointCloud<Point>::Ptr subBlobPointsLocalMap = this->BlobsPointsLocalMap->Get(this->Tworld);
    kdtreeBlobs->setInputCloud(subBlobPointsLocalMap);
    std::cout << "blobs map : " << subBlobPointsLocalMap->points.size() << std::endl;
  }

  unsigned int usedEdges = 0;
  unsigned int usedPlanes = 0;
  unsigned int usedBlobs = 0;
  Point currentPoint;
  Eigen::MatrixXd estimatorCovariance(6, 6);

  Eigen::Matrix<double, 6, 1> MappingTworld;
  MappingTworld << this->Tworld;

  std::cout << "Mapping start Tworld: " << this->Tworld.transpose() << std::endl;

  // ICP - Levenberg-Marquardt loop:
  // At each step of this loop an ICP matching is performed
  // Once the keypoints matched, we estimate the the 6-DOF
  // parameters by minimizing a non-linear least square cost
  // function using a Levenberg-Marquardt algorithm
  for (unsigned int icpCount = 0; icpCount < this->MappingICPMaxIter; ++icpCount)
  {
    // clear all keypoints matching data
    this->ResetDistanceParameters();

    // Init the undistortion interpolator
    if (this->Undistortion)
    {
      this->MappingInterpolator = this->InitUndistortionInterpolatorMapping();
    }

    // Rotation and position at this step
    Eigen::Matrix3d R = GetRotationMatrix(MappingTworld);
    Eigen::Vector3d T;
    T << MappingTworld(3), MappingTworld(4), MappingTworld(5);

    // loop over edges
    for (unsigned int edgeIndex = 0; edgeIndex < this->CurrentEdgesPoints->size(); ++edgeIndex)
    {
      currentPoint = this->CurrentEdgesPoints->points[edgeIndex];

      if (this->CurrentEdgesPoints->size() > 0 && subEdgesPointsLocalMap->points.size() > 10)
      {
        // Find the closest correspondence edge line of the current edge point
        int rejectionIndex = this->ComputeLineDistanceParameters(kdtreeEdges, R, T, currentPoint, "mapping");
        this->EdgePointRejectionMapping[edgeIndex] = rejectionIndex;
        this->MatchRejectionHistogramLine[rejectionIndex] += 1;
        usedEdges = this->Xvalues.size();
      }
    }

    // loop over surfaces
    for (unsigned int planarIndex = 0; planarIndex < this->CurrentPlanarsPoints->size(); ++planarIndex)
    {
      currentPoint = this->CurrentPlanarsPoints->points[planarIndex];

      if (this->CurrentPlanarsPoints->size() > 0 && subPlanarPointsLocalMap->size() > 10)
      {
        // Find the closest correspondence plane of the current planar point
        int rejectionIndex = this->ComputePlaneDistanceParameters(kdtreePlanes, R, T, currentPoint, "mapping");
        this->PlanarPointRejectionMapping[planarIndex] = rejectionIndex;
        this->MatchRejectionHistogramPlane[rejectionIndex] += 1;
        usedPlanes = this->Xvalues.size() - usedEdges;
      }
    }

    if (!this->FastSlam && this->NbrFrameProcessed > 10)
    {
      // loop over blobs
      for (unsigned int blobIndex = 0; blobIndex < this->CurrentBlobsPoints->size(); ++blobIndex)
      {
        currentPoint = this->CurrentBlobsPoints->points[blobIndex];

        // Find the closest correspondence plane of the current planar point
        this->ComputeBlobsDistanceParameters(kdtreeBlobs, R, T, currentPoint, "mapping");
        usedBlobs = this->Xvalues.size() - usedPlanes - usedEdges;
      }
    }

    // Skip this frame if there is too few geometric keypoints matched
    if ((usedPlanes + usedEdges + usedBlobs) < 20)
    {
      vtkGenericWarningMacro("Too few geometric features, loop breaked");
      std::cout << "planes: " << usedPlanes << " edges: " << usedEdges << " Blobs: " << usedBlobs << std::endl;
      break;
    }

    // Get the previous sensor pose
    Eigen::Matrix3d R0 = GetRotationMatrix(this->PreviousTworld);
    Eigen::Vector3d T0; T0 << this->PreviousTworld[3], this->PreviousTworld[4], this->PreviousTworld[5];

    // We want to estimate our 6-DOF parameters using a non
    // linear least square minimization. The non linear part
    // comes from the Euler Angle parametrization of the rotation
    // endomorphism SO(3). To minimize it we use CERES to perform
    // the Levenberg-Marquardt algorithm.
    ceres::Problem problem;
    for (unsigned int k = 0; k < Xvalues.size(); ++k)
    {
      if (this->Undistortion)
      {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctions::MahalanobisDistanceLinearDistortionResidual, 1, 6>(
                                              new CostFunctions::MahalanobisDistanceLinearDistortionResidual(
                                                this->Avalues[k], this->Pvalues[k], this->Xvalues[k], T0, R0,
                                                this->TimeValues[k], this->residualCoefficient[k]));
        problem.AddResidualBlock(cost_function, new ceres::ArctanLoss(2.0), MappingTworld.data());
      }
      else
      {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctions::MahalanobisDistanceAffineIsometryResidual, 1, 6>(
                                             new CostFunctions::MahalanobisDistanceAffineIsometryResidual(this->Avalues[k], this->Pvalues[k],
                                                                                this->Xvalues[k], this->residualCoefficient[k]));
        problem.AddResidualBlock(cost_function, new ceres::ArctanLoss(2.0), MappingTworld.data());
      }
    }

    ceres::Solver::Options options;
    options.max_num_iterations = this->MappingLMMaxIter;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = false;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;

    // If no L-M iteration has been made since the
    // last ICP matching it means we reached a local
    // minimum for the ICP-LM algorithm
    if (summary.num_successful_steps == 1)
    {
      // Now evaluate the quality of the parameters
      // estimated using an approximate computation
      // of the variance covariance matrix
      // Covariance computation options
      ceres::Covariance::Options covOptions;
      covOptions.apply_loss_function = true;
      covOptions.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD;

      // Computation of the variance-covariance matrix
      ceres::Covariance covariance(covOptions);
      std::vector<std::pair<const double*, const double* > > covariance_blocks;
      covariance_blocks.push_back(std::make_pair(MappingTworld.data(), MappingTworld.data()));
      covariance.Compute(covariance_blocks, &problem);
      double covarianceMat[6 * 6];
      covariance.GetCovarianceBlock(MappingTworld.data(), MappingTworld.data(), covarianceMat);
      for (int i = 0; i < 6; ++i)
        for (int j = 0; j < 6; ++j)
          estimatorCovariance(i, j) = covarianceMat[i + 6 * j];
      break;
      }
  }

  Eigen::Vector3d imuTr;
  imuTr << this->ImuTrelative(3), this->ImuTrelative(4), this->ImuTrelative(5);

  Eigen::Vector3d TworldVec;
  TworldVec << this->Tworld(3), this->Tworld(4), this->Tworld(5);

  Eigen::Vector3d imuTw = GetRotationMatrix(this->PreviousTworld) * imuTr + TworldVec;

  Eigen::Matrix<double, 6, 1> imuRel = this->ImuTrelative;
  Eigen::Matrix3d imuRelRotationMat = GetRotationMatrix(imuRel);

  Eigen::Matrix3d prevWorldRotationMat = GetRotationMatrix(this->PreviousTworld);

  Eigen::Matrix3d imuWorldRotationMat = prevWorldRotationMat * imuRelRotationMat;

  double rx = std::atan2(imuWorldRotationMat(2, 1), imuWorldRotationMat(2, 2));
  double ry = -std::asin(imuWorldRotationMat(2, 0));
  double rz = std::atan2(imuWorldRotationMat(1, 0), imuWorldRotationMat(0, 0));

  Eigen::Matrix<double, 6, 1> imuWorld;
  imuWorld << rx, ry, rz, imuTw(0), imuTw(1), imuTw(2);

  std::cout << "IMU rel:         " <<  this->ImuTrelative.transpose() << std::endl;
  std::cout << "IMU Tworld:      " << imuWorld.transpose() << std::endl;
  std::cout << "Mapping changes: " << (MappingTworld - imuWorld).transpose() << std::endl;
  std::cout << "Tworld:          " << this->Tworld.transpose() << std::endl;
  std::cout << "Mapping Tworld:  " << MappingTworld.transpose() << std::endl;

  if (this->UsingImu && this->CurrentFrameTime != this->PreviousFrameTime)
  {
    Eigen::Vector3d ImuVariationT;
    ImuVariationT << this->ImuVariation(3), this->ImuVariation(4), this->ImuVariation(5);

    Eigen::Vector3d ImuVariationTw = prevWorldRotationMat * ImuVariationT;

    Eigen::Matrix<double, 6, 1> mappingImuVariance;
    mappingImuVariance << this->ImuVariation(0), this->ImuVariation(1), this->ImuVariation(2), ImuVariationTw;

    // ensures change made by SLAM is within its limits set by the IMU confidence levels
    for (int i = 0; i < MappingTworld.rows(); i++)
    {
      if (MappingTworld(i) < imuWorld(i) - this->ImuVariation(i))
      {
        MappingTworld(i) = imuWorld(i) - this->ImuVariation(i);
        std::cout << "Mapping Correction limit hit on: " << i << std::endl;
      }
      else if (MappingTworld(i) > imuWorld(i) + this->ImuVariation(i))
      {
        MappingTworld(i) = imuWorld(i) + this->ImuVariation(i);
        std::cout << "Mapping Correction limit hit on: " << i << std::endl;
      }
    }

    //Velocity error correction
    /*Eigen::Matrix3d Trr = GetRotationMatrix(this->Trelative);
    Eigen::Vector3d Trt;
    Trt <<  this->Trelative(3), this->Trelative(4), this->Trelative(5);

    Eigen::Vector3d TrLinearDist = Trr.transpose() * Trt;

    Eigen::Matrix3d Imur = GetRotationMatrix(this->ImuTrelative);
    Eigen::Vector3d Imut;
    Imut <<  this->ImuTrelative(3), this->ImuTrelative(4), this->ImuTrelative(5);

    Eigen::Vector3d ImuLinearDist = Imur.transpose() * Imut;

    Eigen::Vector3d ErrorDist = TrLinearDist - ImuLinearDist;

    double timeDiff = this->CurrentFrameTime - this->PreviousFrameTime;

    Eigen::Vector3d ErrorVelocity = ErrorDist/timeDiff;

    this->Velocity -= 0.5 * ErrorVelocity;*/

  }

  this->Tworld << MappingTworld;
  // Provide information about keypoints-neighborhood matching rejections
  this->RejectionInformationDisplay();

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(estimatorCovariance);
  Eigen::MatrixXd D = eig.eigenvalues();

  static_cast<vtkDoubleArray*>(this->Trajectory->GetPointData()->GetArray("Variance Error"))->InsertNextValue(D(5));
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: edges used"))->InsertNextValue(usedEdges);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: planes used"))->InsertNextValue(usedPlanes);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: blobs used"))->InsertNextValue(usedBlobs);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: total keypoints used"))->InsertNextValue(this->Xvalues.size());

  std::cout << "Matches used: Total: " << this->Xvalues.size()
            << " edges: " << usedEdges << " planes: " << usedPlanes << " blobs: " << usedBlobs << std::endl;
  std::cout << "Covariance Eigen values: " << D.transpose() << std::endl;
  std::cout << "Maximum variance: " << D(5) << std::endl;

  // Add the current computed transform to the list
  this->TworldList.push_back(this->Tworld);

  // Update the PreviousTworld data
  this->PreviousTworld = this->Tworld;

  // update maps
  this->UpdateMapsUsingTworld();
}

//-----------------------------------------------------------------------------
void vtkSlam::UpdateMapsUsingTworld()
{
  // Init the mapping interpolator
  if (this->Undistortion)
  {
    this->MappingInterpolator = this->InitUndistortionInterpolatorMapping();
  }

  // Update EdgeMap
  pcl::PointCloud<Point>::Ptr MapEdgesPoints(new pcl::PointCloud<Point>());
  for (unsigned int i = 0; i < this->CurrentEdgesPoints->size(); ++i)
  {
    MapEdgesPoints->push_back(this->CurrentEdgesPoints->at(i));
    this->TransformToWorld(MapEdgesPoints->at(i));
  }
  EdgesPointsLocalMap->Roll(this->Tworld);
  EdgesPointsLocalMap->Add(MapEdgesPoints);

  // Update PlanarMap
  pcl::PointCloud<Point>::Ptr MapPlanarsPoints(new pcl::PointCloud<Point>());
  for (unsigned int i = 0; i < this->CurrentPlanarsPoints->size(); ++i)
  {
    MapPlanarsPoints->push_back(this->CurrentPlanarsPoints->at(i));
    this->TransformToWorld(MapPlanarsPoints->at(i));
  }
  PlanarPointsLocalMap->Roll(this->Tworld);
  PlanarPointsLocalMap->Add(MapPlanarsPoints);

  // Update BlobsMap. The all current frame is added
  if (!this->FastSlam)
  {
    pcl::PointCloud<Point>::Ptr MapBlobsPoints(new pcl::PointCloud<Point>());
    for (unsigned int i = 0; i < this->pclCurrentFrame->size(); ++i)
    {
      MapBlobsPoints->push_back(this->pclCurrentFrame->at(i));
      this->TransformToWorld(MapBlobsPoints->at(i));
    }
    BlobsPointsLocalMap->Roll(this->Tworld);
    BlobsPointsLocalMap->Add(MapBlobsPoints);
  }
}

//-----------------------------------------------------------------------------
void vtkSlam::FillMappingInfoArrayWithDefaultValues()
{
  static_cast<vtkDoubleArray*>(this->Trajectory->GetPointData()->GetArray("Variance Error"))->InsertNextValue(10.0);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: edges used"))->InsertNextValue(0);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: planes used"))->InsertNextValue(0);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: blobs used"))->InsertNextValue(0);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("Mapping: total keypoints used"))->InsertNextValue(0);
}

//-----------------------------------------------------------------------------
void vtkSlam::FillEgoMotionInfoArrayWithDefaultValues()
{
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("EgoMotion: edges used"))->InsertNextValue(0);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("EgoMotion: planes used"))->InsertNextValue(0);
  static_cast<vtkIntArray*>(this->Trajectory->GetPointData()->GetArray("EgoMotion: total keypoints used"))->InsertNextValue(0);
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkVelodyneTransformInterpolator> vtkSlam::InitUndistortionInterpolatorEgoMotion()
{
  vtkSmartPointer<vtkVelodyneTransformInterpolator> resultInterp = vtkSmartPointer<vtkVelodyneTransformInterpolator>::New();
  resultInterp->SetInterpolationTypeToLinear();

  // Transforms representing the passage from the
  // referential of the sensor at time t0 resp t1
  // to the referential of the sensor at the time 0
  vtkNew<vtkTransform> transform0, transform1;

  // transform 0 is identity
  transform0->Identity();
  transform0->Modified();
  transform0->Update();

  // transform 1 is the delta transform
  // between T0 and T1 computed in the EgoMotion
  // i.e Trelative
  Eigen::Matrix3d R = GetRotationMatrix(this->Trelative);
  Eigen::Vector3d T;
  T << this->Trelative(3), this->Trelative(4), this->Trelative(5);

  vtkNew<vtkMatrix4x4> M;
  for (unsigned int i = 0; i < 3; ++i)
  {
    for (unsigned int j = 0; j < 3; ++j)
    {
      M->Element[i][j] = R(i, j);
    }
    M->Element[i][3] = T(i);
    M->Element[3][i] = 0;
  }
  M->Element[3][3] = 1.0;

  transform1->SetMatrix(M.Get());
  transform1->Modified();
  transform1->Update();

  // Add the transforms and update
  resultInterp->AddTransform(0.0, transform0.GetPointer());
  resultInterp->AddTransform(1.0, transform1.GetPointer());
  resultInterp->Modified();

  return resultInterp;
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkVelodyneTransformInterpolator> vtkSlam::InitUndistortionInterpolatorMapping()
{
  vtkSmartPointer<vtkVelodyneTransformInterpolator> resultInterp = vtkSmartPointer<vtkVelodyneTransformInterpolator>::New();
  resultInterp->SetInterpolationTypeToLinear();

  // Transforms representing the passage from the
  // referential of the sensor at time t0 resp t1
  // to the referential of the sensor at the time 0
  vtkNew<vtkTransform> transform0, transform1;

  // transform 1 is the delta transform
  // between T0 and T1
  Eigen::Matrix3d R0, R1;
  R0 = GetRotationMatrix(this->PreviousTworld);
  R1 = GetRotationMatrix(this->Tworld);

  Eigen::Vector3d T0, T1;
  T0 << this->PreviousTworld(3), this->PreviousTworld(4), this->PreviousTworld(5);
  T1 << this->Tworld(3), this->Tworld(4), this->Tworld(5);

  vtkNew<vtkMatrix4x4> M0, M1;
  for (unsigned int i = 0; i < 3; ++i)
  {
    for (unsigned int j = 0; j < 3; ++j)
    {
      M0->Element[i][j] = R0(i, j);
      M1->Element[i][j] = R1(i, j);
    }
    M0->Element[i][3] = T0(i);
    M0->Element[3][i] = 0;
    M1->Element[i][3] = T1(i);
    M1->Element[3][i] = 0;
  }
  M0->Element[3][3] = 1.0;
  M1->Element[3][3] = 1.0;

  transform0->SetMatrix(M0.Get());
  transform0->Modified();
  transform0->Update();

  transform1->SetMatrix(M1.Get());
  transform1->Modified();
  transform1->Update();

  // Add the transforms and update
  resultInterp->AddTransform(0.0, transform0.GetPointer());
  resultInterp->AddTransform(1.0, transform1.GetPointer());
  resultInterp->Modified();

  return resultInterp;
}

//-----------------------------------------------------------------------------
void vtkSlam::ExpressPointInOtherReferencial(Point& p, vtkSmartPointer<vtkVelodyneTransformInterpolator> transform)
{
  // interpolate the transform
  vtkNew<vtkTransform> currTransform;
  transform->InterpolateTransform(p.intensity, currTransform.GetPointer());
  currTransform->Modified();
  currTransform->Update();

  double pos[3] = {p.x, p.y, p.z};
  currTransform->InternalTransformPoint(pos, pos);
  p.x = pos[0];
  p.y = pos[1];
  p.z = pos[2];
}

//-----------------------------------------------------------------------------
void vtkSlam::ResetDistanceParameters()
{
  this->Xvalues.clear();
  this->Xvalues.resize(0);
  this->Avalues.clear();
  this->Avalues.resize(0);
  this->Pvalues.clear();
  this->Pvalues.resize(0);
  this->TimeValues.clear();
  this->TimeValues.resize(0);
  this->residualCoefficient.clear();
  this->residualCoefficient.resize(0);
  this->RadiusIncertitude.clear();
  this->RadiusIncertitude.resize(0);
  this->MatchRejectionHistogramLine.clear();
  this->MatchRejectionHistogramLine.resize(NrejectionCauses);
  this->MatchRejectionHistogramPlane.clear();
  this->MatchRejectionHistogramPlane.resize(NrejectionCauses);
  this->MatchRejectionHistogramBlob.clear();
  this->MatchRejectionHistogramBlob.resize(NrejectionCauses);
}

//-----------------------------------------------------------------------------
void vtkSlam::UpdateTworldUsingTrelative()
{
  // Rotation and translation relative
  Eigen::Matrix3d Rr, Rw;
  Eigen::Vector3d Tr, Tw;
  Rr = GetRotationMatrix(this->Trelative);
  Tr << this->Trelative(3), this->Trelative(4), this->Trelative(5);

  // full rotation
  Rw = GetRotationMatrix(this->Tworld);
  Tw << this->Tworld(3), this->Tworld(4), this->Tworld(5);

  Eigen::Vector3d newTw;
  Eigen::Matrix3d newRw;

  // The new pos of the sensor in the world
  // referential is the previous one composed
  // with the relative motion estimated at the
  // odometry step
  newRw = Rw * Rr;
  newTw = Rw * Tr + Tw;

  double rx = std::atan2(newRw(2, 1), newRw(2, 2));
  double ry = -std::asin(newRw(2, 0));
  double rz = std::atan2(newRw(1, 0), newRw(0, 0));

  // Next estimation of Tworld using
  // the odometry result. This estimation
  // will be used to undistorded the frame
  // if required and to initialize the
  this->Tworld(0) = rx;
  this->Tworld(1) = ry;
  this->Tworld(2) = rz;
  this->Tworld(3) = newTw(0);
  this->Tworld(4) = newTw(1);
  this->Tworld(5) = newTw(2);
}

//-----------------------------------------------------------------------------
void vtkSlam::SetVoxelGridLeafSize(double size)
{
  this->PlanarPointsLocalMap->SetLeafSize(size);
  this->EdgesPointsLocalMap->SetLeafSize(0.75 * size);
  this->BlobsPointsLocalMap->SetLeafSize(0.20 * size);
  this->ParametersModificationTime.Modified();
}

//-----------------------------------------------------------------------------
void vtkSlam::SetVoxelGridSize(unsigned int size)
{
  this->EdgesPointsLocalMap->SetSize(size);
  this->PlanarPointsLocalMap->SetSize(size);
  this->BlobsPointsLocalMap->SetSize(size);
  this->ParametersModificationTime.Modified();
}

//-----------------------------------------------------------------------------
void vtkSlam::SetVoxelGridResolution(double resolution)
{
  this->EdgesPointsLocalMap->SetResolution(resolution);
  this->PlanarPointsLocalMap->SetResolution(resolution);
  this->BlobsPointsLocalMap->SetResolution(resolution);
  this->ParametersModificationTime.Modified();
}

//-----------------------------------------------------------------------------
void vtkSlam::SetLidarMaximunRange(const double maxRange)
{
  this->EdgesPointsLocalMap->SetPointCoudMaxRange(maxRange);
  this->PlanarPointsLocalMap->SetPointCoudMaxRange(maxRange);
  this->BlobsPointsLocalMap->SetPointCoudMaxRange(maxRange);
}

//-----------------------------------------------------------------------------
void vtkSlam::UpdateLaserIdMapping(vtkTable *calib)
{
  this->NLasers = calib->GetNumberOfRows();
  auto array = vtkDataArray::SafeDownCast(calib->GetColumnByName("verticalCorrection"));
  if (array)
  {
    std::vector<double> verticalCorrection;
    verticalCorrection.resize(array->GetNumberOfTuples());
    for (int i =0; i < array->GetNumberOfTuples(); ++i)
    {
      verticalCorrection[i] = array->GetTuple1(i);
    }
    this->LaserIdMapping = sortIdx(verticalCorrection);
  }
  else
  {
    vtkErrorMacro("<< The calibration data has no colomn named 'verticalCorrection'");
  }
}

//-----------------------------------------------------------------------------
void vtkSlam::RejectionInformationDisplay()
{
  double totalRejectionsLine = 0;
  double totalRejectionsPlane = 0;
  for (int k = 0; k < this->NrejectionCauses; ++k)
  {
    totalRejectionsLine += this->MatchRejectionHistogramLine[k];
    totalRejectionsPlane += this->MatchRejectionHistogramPlane[k];
  }
  std::cout << "Rejection frequencies lines: [";
  for (int k = 0; k < this->NrejectionCauses; ++k)
  {
    std::cout << this->MatchRejectionHistogramLine[k] / totalRejectionsLine * 100.0 << ", ";
  }
  std::cout << std::endl;
  std::cout << "Rejection frequencies planes: [";
  for (int k = 0; k < this->NrejectionCauses; ++k)
  {
    std::cout << this->MatchRejectionHistogramPlane[k] / totalRejectionsPlane * 100.0 << ", ";
  }
  std::cout << std::endl;
}