#define PCL_NO_PRECOMPILE
#include <vector>
#include <iostream>
#include <Eigen/Core>
#include "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include <pcl/correspondence.h>
#include "pcl/io/pcd_io.h"
#include <pcl/search/kdtree.h>
#include "pcl/features/normal_3d_omp.h"
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/transformation_estimation_svd_scale.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/esf.h>
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/features/rops_estimation.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types_conversion.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/pcl_macros.h>
#include <pcl/register_point_struct.h>
#include <pcl/console/parse.h>

std::string model_filename_;
std::string scene_filename_;
std::string room_filename_;

void
showHelp (char *filename)
{
  std::cout << std::endl;
  std::cout << "***************************************************************************" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "*             Correspondence Grouping Tutorial - Usage Guide              *" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "***************************************************************************" << std::endl << std::endl;
  std::cout << "Usage: "  << filename  << " <scene_filename.pcd> <model_filename.pcd> <model_room_filename.pcd>" << std::endl << std::endl;
  std::cout << "Example: " << filename << " method1_seg.pcd  maquette.pcd  chambre.pcd" << std::endl << std::endl;
}

struct ROPS135
{
  float histogram[135];
  static int descriptorSize () { return 135; }
  friend std::ostream& operator << (std::ostream& os, const ROPS135& p);
};
PCL_EXPORTS std::ostream& operator << (std::ostream& os, const ROPS135& p);

POINT_CLOUD_REGISTER_POINT_STRUCT (ROPS135,
    (float[135], histogram, rops)
)

void
downsample (pcl::PointCloud<pcl::PointXYZ>::Ptr &points, float leaf_size,
            pcl::PointCloud<pcl::PointXYZ>::Ptr &downsampled_out)
{
  pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
  vox_grid.setLeafSize (leaf_size, leaf_size, leaf_size);
  vox_grid.setInputCloud (points);
  vox_grid.filter (*downsampled_out);
}

void
compute_surface_normals (pcl::PointCloud<pcl::PointXYZ>::Ptr &points, float normal_radius,
                         pcl::PointCloud<pcl::Normal>::Ptr &normals_out)
{

  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> norm_est;

  // Use a FLANN-based KdTree to perform neighborhood searches
  //norm_est.setSearchMethod (pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr (new pcl::KdTreeFLANN<pcl::PointXYZ>));
  norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));

  // Specify the size of the local neighborhood to use when computing the surface normals
  norm_est.setRadiusSearch (normal_radius);

  // Set the input points
  norm_est.setInputCloud (points);

  // Estimate the surface normals and store the result in "normals_out"
  norm_est.compute (*normals_out);
}

void
compute_ROPS_features (pcl::PointCloud<pcl::PointXYZ>::Ptr &points,
                       pcl::PointCloud<pcl::Normal>::Ptr &normals,
                       float feature_radius,
                       pcl::PointCloud<ROPS135>::Ptr &descriptors_out)
{
// Perform triangulation.
  pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
  pcl::concatenateFields(*points, *normals, *cloudNormals);
  pcl::search::KdTree<pcl::PointNormal>::Ptr kdtree2(new pcl::search::KdTree<pcl::PointNormal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);

  kdtree2->setInputCloud(cloudNormals);
  pcl::GreedyProjectionTriangulation<pcl::PointNormal> triangulation;
  pcl::PolygonMesh triangles;
  triangulation.setSearchRadius(feature_radius);
  triangulation.setMu(2.5);
  triangulation.setMaximumNearestNeighbors(100);
  triangulation.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees.
  triangulation.setNormalConsistency(false);
  triangulation.setMinimumAngle(M_PI / 18); // 10 degrees.
  triangulation.setMaximumAngle(2 * M_PI / 3); // 120 degrees.
  triangulation.setInputCloud(cloudNormals);
  triangulation.setSearchMethod(kdtree2);
  triangulation.reconstruct(triangles);

  // Note: only compute descriptors for chosen keypoints. It has been omitted here for simplicity.
  // RoPs estimation object.
  pcl::ROPSEstimation<pcl::PointXYZ, ROPS135> rops;
  rops.setInputCloud(points);
  rops.setSearchMethod(kdtree);
  rops.setRadiusSearch(0.025);
  rops.setTriangles(triangles.polygons);
  // Number of partition bins that is used for distribution matrix calculation.
  rops.setNumberOfPartitionBins(5);
  // The greater the number of rotations is, the bigger the resulting descriptor.
  // Make sure to change the histogram size accordingly.
  rops.setNumberOfRotations(3);
  // Support radius that is used to crop the local surface of the point.
  rops.setSupportRadius(feature_radius);
  rops.compute(*descriptors_out);

}

void estimation_feature_correspondences_ROPS(pcl::PointCloud<ROPS135>::Ptr &source_descriptors,
                                             pcl::PointCloud<ROPS135>::Ptr &target_descriptors,
                                             pcl::CorrespondencesPtr &correspondences_all)
{
  PCL_INFO("Finding correspondences...\n");
  pcl::registration::CorrespondenceEstimation<ROPS135, ROPS135> ce;
  ce.setInputSource (source_descriptors);
  ce.setInputTarget (target_descriptors);
  ce.determineReciprocalCorrespondences (*correspondences_all);
  PCL_INFO("%d correspondences found\n", correspondences_all->size ());
}

void
find_feature_ROPS_correspondences (pcl::PointCloud<ROPS135>::Ptr &source_descriptors,
                                   pcl::PointCloud<ROPS135>::Ptr &target_descriptors,
                                   pcl::CorrespondencesPtr &correspondences_all)
{
  pcl::search::KdTree<ROPS135> matching;
  matching.setInputCloud(target_descriptors);

  // A Correspondence object stores the indices of the query and the match,
  // and the distance/weight.

  // Check every descriptor computed for the scene.
  for (size_t i = 0; i < source_descriptors->size(); ++i)
  {
    std::vector<int> neighbors(1);
    std::vector<float> squaredDistances(1);

    // Ignore NaNs.
    if (!pcl_isfinite (source_descriptors->at (i).histogram[0])) //skipping NaNs
    {
      continue;
    }
    // Find the nearest neighbor (in descriptor space)...
    int neighborCount = matching.nearestKSearch(source_descriptors->at(i), 1, neighbors, squaredDistances);
    // ...and add a new correspondence if the distance is less than a threshold
    // (ROPS distances are between 0 and 1, other descriptors use different metrics).
    pcl::Correspondence correspondence(static_cast<int>(i), neighbors[0], squaredDistances[0]);
    correspondences_all->push_back(correspondence);

  }
  std::cout << "Found " << correspondences_all->size() << " ROPS correspondences." << std::endl;
}

void
compute_ESF_features (pcl::PointCloud<pcl::PointXYZ>::Ptr &points,
                      pcl::PointCloud<pcl::ESFSignature640>::Ptr &descriptors_out)
{
  // Create a PFHEstimation object
  pcl::ESFEstimation<pcl::PointXYZ, pcl::ESFSignature640> esf_est;

  // Set the input points and surface normals
  esf_est.setInputCloud (points);
  std::cout << "start compute ESF features descriptors" << std::endl;
  // Compute the features
  esf_est.compute (*descriptors_out);
  std::cout << "Compute ESF descriptors successfully" << std::endl;
/*
  pcl::visualization::PCLPlotter plotter;
  // need to set the size of the descriptor beforehand.
  plotter.addFeatureHistogram(*descriptors_out, 640);
  plotter.plot();
*/
}

void estimation_feature_correspondences(pcl::PointCloud<pcl::PFHSignature125>::Ptr &source_descriptors,
                              pcl::PointCloud<pcl::PFHSignature125>::Ptr &target_descriptors, pcl::CorrespondencesPtr &correspondences_all)
{
  PCL_INFO("Finding correspondences...\n");
  pcl::registration::CorrespondenceEstimation<pcl::PFHSignature125, pcl::PFHSignature125> ce;
  ce.setInputSource (source_descriptors);
  ce.setInputTarget (target_descriptors);
  ce.determineReciprocalCorrespondences (*correspondences_all);
  PCL_INFO("%d correspondences found\n", correspondences_all->size ());
}

void rejection_correspondences(pcl::PointCloud<pcl::PointXYZ>::Ptr &points1, pcl::PointCloud<pcl::PointXYZ>::Ptr &points2, int num_iteration,
                               pcl::CorrespondencesPtr &correspondences_all, pcl::CorrespondencesPtr &correspondences_inliers)
{

  pcl::CorrespondencesPtr correspondences_x (new pcl::Correspondences);
  pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> sac;
  sac.setInputSource (points1);
  sac.setInputTarget (points2);
  sac.setInlierThreshold (0.1);
  sac.setMaximumIterations (num_iteration);
  sac.setInputCorrespondences (correspondences_all);
  sac.getCorrespondences (*correspondences_x);

  for (size_t i=0; i < correspondences_x -> size (); i++){
    if (correspondences_x ->at(i).distance < 1.5e-07) {
      correspondences_inliers->push_back(correspondences_x->at(i));
    }
  }
  PCL_INFO("%d inliers correspondences\n", correspondences_inliers->size ());
/*
  for(size_t i =0; i< correspondences_inliers->size ();i++){
    std::cerr<<"the linliers correspondences"<< correspondences_inliers->at(i)<<std::endl;
  }
*/
}

void
find_feature_ESF_correspondences (pcl::PointCloud<pcl::ESFSignature640>::Ptr &source_descriptors,
                                  pcl::PointCloud<pcl::ESFSignature640>::Ptr &target_descriptors,
                                  pcl::CorrespondencesPtr &correspondences_all)
{

  // Use a KdTree to search for the nearest matches in feature space
  pcl::search::KdTree<pcl::ESFSignature640> descriptor_kdtree;
  descriptor_kdtree.setInputCloud (target_descriptors);
  PCL_INFO("source_descriptors' number is %d \n", source_descriptors->size ());
  PCL_INFO("target_descriptors' number is %d \n", target_descriptors->size ());
  // Find the index of the best match for each point, and store it in "correspondences_out"

  for (size_t i = 0; i < source_descriptors->size (); ++i)
  {
    std::vector<int> neigh_indices (1);
    std::vector<float> neigh_sqr_dists (1);

    if (!pcl_isfinite (source_descriptors->at (i).histogram[0])) //skipping NaNs
    {
      continue;
    }

    int found_neighs = descriptor_kdtree.nearestKSearch (source_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
    pcl::Correspondence corr ( static_cast<int> (i), neigh_indices[0], neigh_sqr_dists[0]);
    correspondences_all->push_back (corr);
    std::cout<<corr<<std::endl;

  }
  PCL_INFO("correspondences found %d \n", correspondences_all->size ());
}

void calculate_transformation (pcl::PointCloud<pcl::PointXYZ>::Ptr &points1,
                               pcl::PointCloud<pcl::PointXYZ>::Ptr &points2,
                               pcl::CorrespondencesPtr &correspondences_inliers,
                               Eigen::Matrix4f &transform)
{
  pcl::registration::TransformationEstimationSVDScale<pcl::PointXYZ,pcl::PointXYZ> trans_SVD;
  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 transformation2;
  trans_SVD.estimateRigidTransformation (*points1,*points2, *correspondences_inliers, transformation2);

  std::cout << "The Estimated Rotation and translation matrices (using getTransformation function) are : \n" << std::endl;
  printf ("\n");
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (0,0), transformation2 (0,1), transformation2 (0,2), transformation2 (0,3));
  printf ("R = | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (1,0), transformation2 (1,1), transformation2 (1,2), transformation2 (1,3));
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (2,0), transformation2 (2,1), transformation2 (2,2), transformation2 (2,3));
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (3,0), transformation2 (3,1), transformation2 (3,2), transformation2 (3,3));
  printf ("\n");
  printf ("t = < %0.3f, %0.3f, %0.3f >\n", transformation2 (0,3), transformation2 (1,3), transformation2 (2,3));
  PCL_INFO("start transform\n");
  transform = transformation2;
}

void calculate_transformation_clusters (pcl::PointCloud<pcl::PointXYZ>::Ptr &points1,
                                	pcl::PointCloud<pcl::PointXYZ>::Ptr &points2,
                              		pcl::PointCloud<pcl::PointXYZ>::Ptr &points_all,
                                	pcl::CorrespondencesPtr &correspondences_inliers,
                                        Eigen::Matrix4f& transform,
                                	pcl::PointCloud<pcl::PointXYZ>::Ptr &points_trans)
{

  pcl::registration::TransformationEstimationSVDScale<pcl::PointXYZ,pcl::PointXYZ> trans_SVD;
  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 transformation2;
  trans_SVD.estimateRigidTransformation (*points1,*points2, *correspondences_inliers, transformation2);

  std::cout << "The Estimated Rotation and translation matrices (using getTransformation function) are : \n" << std::endl;
  printf ("\n");
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (0,0), transformation2 (0,1), transformation2 (0,2), transformation2 (0,3));
  printf ("R = | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (1,0), transformation2 (1,1), transformation2 (1,2), transformation2 (1,3));
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (2,0), transformation2 (2,1), transformation2 (2,2), transformation2 (2,3));
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (3,0), transformation2 (3,1), transformation2 (3,2), transformation2 (3,3));
  printf ("\n");
  printf ("t = < %0.3f, %0.3f, %0.3f >\n", transformation2 (0,3), transformation2 (1,3), transformation2 (2,3));
  PCL_INFO("start transform\n");
  pcl::transformPointCloud (*points_all, *points_trans, transformation2);
  transform = transformation2;
}

void alignement (pcl::PointCloud<pcl::PointXYZ>::Ptr &points1,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr &points2,
                 pcl::PointCloud<ROPS135>::Ptr &points1_descriptors,
                 pcl::PointCloud<ROPS135>::Ptr &points2_descriptors,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr &object_aligned )
{
  const float leaf = 0.005f;
  pcl::console::print_highlight ("Starting alignment...\n");
  pcl::SampleConsensusPrerejective<pcl::PointXYZ, pcl::PointXYZ, ROPS135> align;
  align.setInputSource (points1);
  align.setSourceFeatures (points1_descriptors);
  align.setInputTarget (points2);
  align.setTargetFeatures (points2_descriptors);
  align.setMaximumIterations (70000); // Number of RANSAC iterations
  align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
  align.setCorrespondenceRandomness (15); // Number of nearest features to use
  align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
  align.setInlierFraction (0.3f); // Required inlier fraction for accepting a pose hypothesis
  align.align (*object_aligned);

  if (align.hasConverged ())
  {
    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), points1->size ());

    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment");
    visu.addPointCloud (points2, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (points2, 255.0, 255.0, 0.0), "model");
    visu.addPointCloud (object_aligned, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (object_aligned, 255.0, 0.0, 0.0), "object_aligned");
    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");
  }
}

pcl::PointCloud<pcl::PointXYZ>::Ptr runICP(pcl::PointCloud<pcl::PointXYZ>::Ptr irCloud,
                                           pcl::PointCloud<pcl::PointXYZ>::Ptr eoCloud,
                                           Eigen::Matrix4f& transform)
{
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  // Set the input source and target
  icp.setInputSource(irCloud);
  icp.setInputTarget(eoCloud);

  // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
  icp.setMaxCorrespondenceDistance (0.1);
  // Set the maximum number of iterations (criterion 1)
  icp.setMaximumIterations (500);
  // Set the transformation epsilon (criterion 2)
  // icp.setTransformationEpsilon (1e-16);//16
  // Set the euclidean distance difference epsilon (criterion 3)
  // icp.setEuclideanFitnessEpsilon (.5);

  // Perform the alignment
  pcl::PointCloud<pcl::PointXYZ>::Ptr registered(new pcl::PointCloud<pcl::PointXYZ>());
  icp.align (*registered);
  cout<<"ICP has converged:"<<icp.hasConverged()<<" score: "<<icp.getFitnessScore()<<endl;

  // Obtain the transformation that aligned cloud_source to cloud_source_registered
  //Eigen::Matrix4f transformation = icp.getFinalTransformation ();
  transform = icp.getFinalTransformation ();

  //report transform and error
  return registered;
}

void visualize_normals (const pcl::PointCloud<pcl::PointXYZ>::Ptr points,
                        const pcl::PointCloud<pcl::PointXYZ>::Ptr normal_points,
                        const pcl::PointCloud<pcl::Normal>::Ptr normals)
{
  // Add the points and normals to the vizualizer
  pcl::visualization::PCLVisualizer viz;
  viz.addPointCloud (points, "points");
  viz.addPointCloud (normal_points, "normal_points");

  viz.addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (normal_points, normals, 1, 0.01, "normals");

  // Give control over to the visualizer
  viz.spin ();
}

void visualize_correspondences (const pcl::PointCloud<pcl::PointXYZ>::Ptr points1,
                                const pcl::PointCloud<pcl::PointXYZ>::Ptr points2,
                                const pcl::CorrespondencesPtr &correspondences_all)
{
  // Create some new point clouds to hold our transformed data
  pcl::PointCloud<pcl::PointXYZ>::Ptr points_left (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr points_right (new pcl::PointCloud<pcl::PointXYZ>);

  // Shift the first clouds' points to the left
  //const Eigen::Vector3f translate (0.0, 0.0, 0.3);
  const Eigen::Vector3f translate (0.0, 0.5, 0.0);
  const Eigen::Quaternionf no_rotation (0, 0, 0, 0);

  pcl::transformPointCloud (*points1, *points_left, -translate, no_rotation);

  // Shift the second clouds' points to the right
  pcl::transformPointCloud (*points2, *points_right, translate, no_rotation);

  // Add the clouds to the vizualizer
  pcl::visualization::PCLVisualizer viz;
  viz.addPointCloud (points_left, "points_left");
  viz.addPointCloud (points_right, "points_right");

  // Draw lines between the best corresponding points
  PCL_INFO("%d correspondences found\n", correspondences_all->size ());

  for (size_t i = 0; i < correspondences_all->size (); ++i)
  {

    // Get the pair of points
    const pcl::PointXYZ & p_left = points_left-> points[correspondences_all->at(i).index_query];
    const pcl::PointXYZ & p_right = points_right->points[correspondences_all->at(i).index_match];

    // Generate a random (bright) color
    double r = (rand() % 100);
    double g = (rand() % 100);
    double b = (rand() % 100);
    double max_channel = std::max (r, std::max (g, b));
    r /= max_channel;
    g /= max_channel;
    b /= max_channel;

    // Generate a unique string for each line
    std::stringstream ss ("line");
    ss << i;
    // Draw the line

    viz.addLine<pcl::PointXYZ, pcl::PointXYZ> (p_left, p_right, r, g, b, ss.str ());
  }

  // Give control over to the visualizer
  viz.spin ();
}

void visualize_correspondences_clusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr points1,
                               		 const pcl::PointCloud<pcl::PointXYZ>::Ptr points2,
                               		 const pcl::PointCloud<pcl::PointXYZ>::Ptr points_all,
                                	 const pcl::CorrespondencesPtr &correspondences_all)
{
  // Create some new point clouds to hold our transformed data
  pcl::PointCloud<pcl::PointXYZ>::Ptr points_left (new pcl::PointCloud<pcl::PointXYZ>), points_left_ (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr points_right (new pcl::PointCloud<pcl::PointXYZ>);

  // Shift the first clouds' points to the left
  //const Eigen::Vector3f translate (0.0, 0.0, 0.3);
  const Eigen::Vector3f translate (0.0, 0.00005, 0.0);
  const Eigen::Quaternionf no_rotation (0, 0, 0, 0);

  pcl::transformPointCloud (*points1, *points_left, translate, no_rotation);
  pcl::transformPointCloud (*points_all, *points_left_, translate, no_rotation);

  // Shift the second clouds' points to the right
  pcl::transformPointCloud (*points2, *points_right, -translate, no_rotation);

  // Add the clouds to the vizualizer
  pcl::visualization::PCLVisualizer viz;
  viz.addPointCloud (points_left, "points_left");
  viz.addPointCloud (points_left_, "points_left_");
  viz.addPointCloud (points_right, "points_right");

  // Draw lines between the best corresponding points
  PCL_INFO("%d correspondences found\n", correspondences_all->size ());

  for (size_t i = 0; i < correspondences_all->size (); ++i)
  {

    // Get the pair of points
    const pcl::PointXYZ & p_left = points_left-> points[correspondences_all->at(i).index_query];
    const pcl::PointXYZ & p_right = points_right->points[correspondences_all->at(i).index_match];

    // Generate a random (bright) color
    double r = (rand() % 100);
    double g = (rand() % 100);
    double b = (rand() % 100);
    double max_channel = std::max (r, std::max (g, b));
    r /= max_channel;
    g /= max_channel;
    b /= max_channel;

    // Generate a unique string for each line
    std::stringstream ss ("line");
    ss << i;
    // Draw the line
//    if (i % 7 == 0){
      viz.addLine<pcl::PointXYZ, pcl::PointXYZ> (p_left, p_right, r, g, b, ss.str ());
//    }
  }
  viz.addCoordinateSystem (1.0, "cloud", 0);
  viz.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "points_left");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "points_right");

  // Give control over to the visualizer
  viz.spin ();
}

void visualize_transformation(const pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud,
                              const pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoint,
                              const pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud)
{

  printf(  "\nPoint cloud colors :  white  = original point cloud\n"
      "                        red  = transformed point cloud\n");

  pcl::visualization::PCLVisualizer viewer ("Matrix transformation");

   // Define R,G,B colors for the point cloud
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
  // We add the point cloud to the viewer and pass the color handler
  viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
  viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> viewpoint_color_handler (viewpoint, 0.0, 255, 255.0); //green?
  viewer.addPointCloud (viewpoint, viewpoint_color_handler, "transformed_viewpoint");

 viewer.addCoordinateSystem (1.0, "cloud", 0);
  viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_viewpoint");
  //viewer.setPosition(800, 400); // Setting visualiser window position

  while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
    viewer.spinOnce ();
  }
}

void cluster_euclidean(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                       std::vector<pcl::PointIndices> &cluster_indices)
{

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud);
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.05); // 2cm
  ec.setMinClusterSize (500);
  ec.setMaxClusterSize (50000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud);
  ec.extract (cluster_indices);
  std::cout << "totally  " << cluster_indices.size() << " clusters " << std::endl;
}

void transform_viewpoint (pcl::PointCloud<pcl::PointXYZ>::Ptr &source_cloud,
                          Eigen::Matrix4f& transform,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr &transformed_cloud)
{
  // Generate the data
  for (float y=-0.01f; y<=0.03f; y+=0.001f) {
    for (float z=-0.01f; z<=0.03f; z+=0.001f) {
      pcl::PointXYZ point;
      point.x = 0.01f - y;
      point.y = y;
      point.z = z;
      source_cloud->points.push_back(point);
    }
  }
  source_cloud->width = (uint32_t) source_cloud->points.size();
  source_cloud->height = 1;

  pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform);
}

void
parseCommandLine (int argc, char *argv[])
{
  //Show help
  if (pcl::console::find_switch (argc, argv, "-h"))
  {
    showHelp (argv[0]);
    exit (0);
  }

  //Model & scene filenames
  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size () != 3)
  {
    std::cout << "Filenames missing.\n";
    showHelp (argv[0]);
    exit (-1);
  }
  scene_filename_ = argv[filenames[0]];
  model_filename_ = argv[filenames[1]];
  room_filename_  = argv[filenames[2]];
}

int main (int argc, char *argv[])
{

  parseCommandLine (argc, argv);

  // Create some new point clouds to hold our data
  pcl::PointCloud<pcl::PointXYZ>::Ptr points1 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled1 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr normals_cluster1 (new pcl::PointCloud<pcl::Normal>);

  pcl::PointCloud<pcl::ESFSignature640>::Ptr descriptors_esf1 (new pcl::PointCloud<pcl::ESFSignature640>);
  pcl::PointCloud<ROPS135>::Ptr descriptors_rops1 (new pcl::PointCloud<ROPS135>);

  pcl::PointCloud<pcl::PointXYZ>::Ptr points2 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled2 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr normals2 (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::ESFSignature640>::Ptr descriptors_esf2 (new pcl::PointCloud<pcl::ESFSignature640>);
  pcl::PointCloud<ROPS135>::Ptr descriptors_rops2 (new pcl::PointCloud<ROPS135>);

  pcl::PointCloud<pcl::PointXYZ>::Ptr points3 (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud_all (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr object_aligned (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled1_trans (new pcl::PointCloud<pcl::PointXYZ>);
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::CorrespondencesPtr correspondences_all (new pcl::Correspondences);
  pcl::CorrespondencesPtr correspondences_inliers (new pcl::Correspondences);
  pcl::CorrespondencesPtr correspondences_max (new pcl::Correspondences);
  Eigen::Matrix4f transform_total;
  pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoint_ (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoint_ori (new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoint_refine (new pcl::PointCloud<pcl::PointXYZ>());

  //  Load clouds
  if (pcl::io::loadPCDFile (scene_filename_, *points1) < 0)
  {
    std::cout << "Error loading model cloud." << std::endl;
    showHelp (argv[0]);
    return (-1);
  }
  if (pcl::io::loadPCDFile (model_filename_, *points2) < 0)
  {
    std::cout << "Error loading scene cloud." << std::endl;
    showHelp (argv[0]);
    return (-1);
  }
  if (pcl::io::loadPCDFile (room_filename_, *points3) < 0)
  {
    std::cout << "Error loading model cloud." << std::endl;
    showHelp (argv[0]);
    return (-1);
  }
  std::cout << "load point clouds successfully"  << std::endl;

  std::cout << "Clustering starts !!!!!" << std::endl;

  cluster_euclidean(points1, cluster_indices);
  const float voxel_grid_leaf_size2 = 0.02;
  downsample (points2, voxel_grid_leaf_size2, downsampled2);
  compute_ESF_features (downsampled2, descriptors_esf2);

  float cor_size_i = 50000.0;
  float cor_size_m = 0.0;
  int i = 0;
  int index_max = 0;
  int restraint =0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr points_clustered (new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    		points_clustered->points.push_back (points1->points[*pit]);

    pcl::CorrespondencesPtr correspondences_ (new pcl::Correspondences);
    std::cout << "index" << i << "point cloud"<< std::endl;
    const float voxel_grid_leaf_size1 = 0.02;
    downsample (points_clustered, voxel_grid_leaf_size1, downsampled1);

    std::cout << "downsample points1 :" << downsampled1->size () << " total points." << std::endl;
    std::cout << "downsample points2 :" << downsampled2->size () << " total points." << std::endl;

    *clustered_cloud_all += *downsampled1;

    // Compute ESF features
    compute_ESF_features (downsampled1, descriptors_esf1);

    // Find feature correspondences
    find_feature_ESF_correspondences (descriptors_esf1, descriptors_esf2, correspondences_);

    cor_size_m = correspondences_->at(0).distance;

    if (cor_size_m < cor_size_i || cor_size_m == 0.0 )
    {
      cor_size_i = cor_size_m;
      index_max = i;
      *clustered_cloud = *downsampled1;
      *correspondences_max = *correspondences_;
    }
    i++;
  }

  std::cout << "Maximum number of inlier correspondences is " << cor_size_i << ". Index of max correspondence is " << index_max <<std::endl;

  const float normal_radius1 = 0.025;
  compute_surface_normals (clustered_cloud, normal_radius1, normals_cluster1);

  const float feature_radius1 = 0.025;
  compute_ROPS_features (clustered_cloud, normals_cluster1, feature_radius1, descriptors_rops1);
  pcl::io::loadPCDFile ("maquette_rops_features.pcd", *descriptors_rops2);
  estimation_feature_correspondences_ROPS(descriptors_rops1, descriptors_rops2, correspondences_all);
  int num_iteration = 800;
  rejection_correspondences(clustered_cloud, downsampled2, num_iteration, correspondences_all, correspondences_inliers);
  std::cout << "find correspondences successfully" << std::endl;

  // Visualize the two point clouds and their feature correspondences
  // visualize_correspondences_clusters (clustered_cloud, downsampled2, clustered_cloud_all, correspondences_inliers);

  calculate_transformation (clustered_cloud, downsampled2, correspondences_inliers, transform_total);

//  while (transform_total(1 , 1) < 0.8 && restraint < 10)
//  {
    std::cerr<<"cannot match correctly, but we try one more time"<<std::endl;
    num_iteration+= 500;
    rejection_correspondences(clustered_cloud, downsampled2, num_iteration, correspondences_all, correspondences_inliers);
    calculate_transformation (clustered_cloud, downsampled2, correspondences_inliers, transform_total);
    pcl::transformPointCloud (*clustered_cloud, *downsampled1_trans, transform_total);
    restraint ++;
//  }
//  if(transform_total(1 , 1) < 0.8){

//    std::cerr<<"cannot match correctly, we have tried 10 times, get out of programme."<<std::endl;

//  }else
//  {
    pcl::transformPointCloud (*clustered_cloud, *downsampled1_trans, transform_total);
    transform_viewpoint(viewpoint_ori, transform_total, viewpoint_);

    std::cerr<<"ICP STARTS !"<<std::endl;
  //  alignement (clustered_cloud, downsampled2, descriptors_rops1, descriptors_rops2, object_aligned);
    pcl::PointCloud<pcl::PointXYZ>::Ptr finalpoints (new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::Matrix4f transform;
    finalpoints = runICP(downsampled1_trans, downsampled2, transform);

    std::cerr<<"ICP FINISHED !"<<std::endl;
    pcl::transformPointCloud (*viewpoint_, *viewpoint_refine, transform);
    // Visualize the superposition of point clouds and the another transformed point clouds
    visualize_transformation (points3, viewpoint_,  finalpoints);
//  }
  return (0);
}


/*
 *  Copyright (c) 2004-present, Facebook, Inc.
 *  All rights reserved.
 *
 *  This source code is licensed under the BSD-style license found in the
 *  LICENSE file in the root directory of this source tree. An additional grant
 *  of patent rights can be found in the PATENTS file in the same directory.
 *
 */

#include "fboss/agent/hw/sai/api/HashApi.h"

#include <boost/functional/hash.hpp>

#include <functional>

namespace std {
size_t hash<facebook::fboss::SaiHashTraits::AdapterHostKey>::operator()(
    const facebook::fboss::SaiHashTraits::AdapterHostKey& key) const {
  size_t seed = 0;
  if (std::get<0>(key)) {
    for (auto field : std::get<0>(key).value().value()) {
      boost::hash_combine(seed, field);
    }
  }
  if (std::get<1>(key)) {
    for (auto udf : std::get<1>(key).value().value()) {
      boost::hash_combine(seed, udf);
    }
  }
  return seed;
}
} // namespace std


#include <boost/python.hpp>
#include <iostream>
#include <stdio.h>
#include <fstream>
#include <iomanip>
#include <sstream> //Just to record frames
#include <opencv2/opencv.hpp>
#include "stereocam.h"
#include <string>
#include <climits>
#include <deque>

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
using namespace cv;
using namespace std;

namespace bp = boost::python;

struct point
{
    int x;
    int y;
    point(int x, int y): x(x), y(y){}
};

boost::python::list mainRoutine(const std::string & left_path,
                const std::string & right_path,
                const std::string & pathParam)
{
        const int N = 100;

        //Prepare the class stereocam and read the parameters
        StereoCam StereoRig;
        StereoRig.OpenParams(pathParam);

        // Load images
        Mat ImageL = imread(left_path,CV_LOAD_IMAGE_COLOR);
        Mat ImageR = imread(right_path,CV_LOAD_IMAGE_COLOR);
        ImageL.copyTo(StereoRig.ImageL);
        ImageR.copyTo(StereoRig.ImageR);

        // Rectify
        StereoRig.RectifyStereo();
        // Compute disparity
        Mat DispMap = StereoRig.ComputeDisparity(1,true);

        // Depth from disparity
        StereoRig.ComputeDepthMap();
        StereoRig.DepthMap.convertTo(StereoRig.DepthMap, CV_32F);
        Mat DepthMat(StereoRig.DepthMap.cols,StereoRig.DepthMap.rows,CV_32FC1, 0);

        medianBlur ( StereoRig.DepthMap, DepthMat, 3 );

        double minVal = (double) INT_MAX;
        int xL = 0;
        int yL = 0;
        deque<point> q;
        // Average N closest points
        for (int i = 0; i < DepthMat.size().height; i++)
            for (int j = 0; j < DepthMat.size().width; j++)
            {
                double val = DepthMat.at<float>(i, j);
                if (minVal < 0 || (val > 0 && minVal > val))
                {
                    if (minVal - val > 0.75) q.clear();
                    minVal = val;
                    yL = i;
                    xL = j;
                    q.push_back(point(xL, yL));
                    if (q.size() > N)
                        q.pop_front();
                }
            }
        int num = q.size();
        int avg_x = 0;
        int avg_y = 0;
        for (int i = 0; i < num; ++i)
        {
            point tmp = q.front();
            q.pop_front();
            avg_y += tmp.y;
            avg_x += tmp.x;
        }
        xL = avg_x / num;
        yL = avg_y / num;

        int xR = xL - (int)(StereoRig.Disparity.at<double>(yL, xL));
        int wL = ImageL.size().width;
        int wR = ImageR.size().width;

        int res = 0;

        // Find on which size object is located
        if (xL >= wL/3)
            res += 1;
        if (xL >= 2*wL/3)
            res += 2;

        res += 10;
        if (xR >= wR/3)
            res += 20;
        if (xR >= 2*wR/3)
            res += 30;

        int side = 0;

        if (res <= 20)
            side = 0;
        else if (res == 31)
            side = 1;
        else side = 2;

        boost::python::list ret;
        ret.append(minVal);
        ret.append(side);
        return ret;
}

BOOST_PYTHON_MODULE(Depth_extraction)
{
          using namespace boost::python;
            def("mainRoutine", mainRoutine);
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LO_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LO_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-bitwise
    This function object returns the low part of the entry
    as an unsigned integer of the size of
    the input.

    @par Header <boost/simd/function/lo.hpp>

    @see  hi

    @par Header <boost/simd/function/lo.hpp>

    @par Example:

      @snippet lo.cpp lo

    @par Possible output:

      @snippet lo.txt lo

  **/
  as_integer_t<Value> lo(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/lo.hpp>
#include <boost/simd/function/simd/lo.hpp>

#endif


/**

Copyright (c) 2016, Borella Jocelyn, Karrenbauer Oliver, Meißner Pascal
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "gtest/gtest.h"
#include "ros/ros.h"
#include "helper/pose_helper.hpp"

#include <math.h>
#include <boost/shared_ptr.hpp>
#include <boost/thread/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <eigen3/Eigen/Geometry>

#include "asr_robot_model_services/GetDistance.h"

#define EPSILON 0.001

using namespace directSearchWS;

class PoseHelperSetup: public ::testing::Test {

public:

    ros::NodeHandle n;
    boost::thread t;

    PoseHelperSetup() : n(ros::this_node::getName()) {
    }

    void SetUp() {
        n.setParam("distanceFunc", 2);
        n.setParam("/nbv/ncp", 0.4);
        n.setParam("/nbv/fcp", 1.5);
        n.setParam("viewCenterPositionDistanceThreshold", 0.17);
        n.setParam("/nbv/mHypothesisUpdaterAngleThreshold", 45.0);

        PoseHelper::resetInstance();
    }

    void TearDown() {
    }

    ~PoseHelperSetup()  {
    }
};

TEST_F(PoseHelperSetup, CalcDistPositionEucl) {
    n.setParam("distanceFunc", 2);
    PoseHelper::resetInstance();
    PoseHelperPtr poseHelperPtr = PoseHelper::getInstance();

    geometry_msgs::Pose pose1;
    geometry_msgs::Pose pose2;

    EXPECT_DOUBLE_EQ(0.0, poseHelperPtr->calculateDistance(pose1, pose2));

    // orientation should not change anything
    pose1.orientation.x = 0.444;
    pose2.orientation.w = -0.666;

    EXPECT_DOUBLE_EQ(0.0, poseHelperPtr->calculateDistance(pose1, pose2));

    pose2.position.x = 13.33;
    pose2.position.y = -14.55;

    EXPECT_NEAR(19.733, poseHelperPtr->calculateDistance(pose1, pose2), EPSILON);

    pose1.position.x = -13.33;
    pose1.position.y = -14.55;

    EXPECT_DOUBLE_EQ(26.66, poseHelperPtr->calculateDistance(pose1, pose2));
}

TEST_F(PoseHelperSetup, CalcAngularDistanceInRad) {
    PoseHelperPtr poseHelperPtr = PoseHelper::getInstance();

    geometry_msgs::Pose pose1;
    pose1.orientation.w = 1.0;
    geometry_msgs::Pose pose2;
    pose2.orientation.w = 1.0;
    EXPECT_NEAR(0.0, poseHelperPtr->calcAngularDistanceInRad(pose1, pose2), EPSILON);

    // position should not change anything
    pose1.position.x = 13.33;
    pose2.position.y = -14.55;
    EXPECT_NEAR(0.0, poseHelperPtr->calcAngularDistanceInRad(pose1, pose2), EPSILON);

    // rotate x: 180°
    pose2.orientation.w = 0.0;
    pose2.orientation.x = 1.0;
    EXPECT_NEAR(180.0 * M_PI / 180.0, poseHelperPtr->calcAngularDistanceInRad(pose1, pose2), EPSILON);

    // rotate x: 90° and y: -90°
    pose2.orientation.w = 0.5;
    pose2.orientation.x = 0.5;
    pose2.orientation.y = -0.5;
    pose2.orientation.z = -0.5;
    EXPECT_NEAR(120.0 * M_PI / 180.0, poseHelperPtr->calcAngularDistanceInRad(pose1, pose2), EPSILON);
}

TEST_F(PoseHelperSetup, CheckViewCenterPointsAreApproxEquale) {
    PoseHelperPtr poseHelperPtr = PoseHelper::getInstance();

    geometry_msgs::Pose pose1;
    pose1.orientation.w = 1.0;
    geometry_msgs::Pose pose2;
    pose2.orientation.w = 1.0;
    EXPECT_TRUE(poseHelperPtr->checkViewCenterPointsAreApproxEquale(pose1, pose2));

    // 0.95 is viewCenterDistance and 0.16 is distance between viewpoints < viewCenterPositionDistanceThreshold ~= 0.176
    // rotate z: arctan(0.16/0.95) == 9.56° < 60° == mHypothesisUpdaterAngleThreshold
    pose2.orientation.w = 0.997;
    pose2.orientation.z = 0.083;
    EXPECT_TRUE(poseHelperPtr->checkViewCenterPointsAreApproxEquale(pose1, pose2));

    // 0.95 is viewCenterDistance and 0.19 is distance between viewpoints > viewCenterPositionDistanceThreshold ~= 0.176
    // rotate z: arctan(0.19/0.95) == 11.31° < 60° == mHypothesisUpdaterAngleThreshold
    pose2.orientation.w = 0.995;
    pose2.orientation.z = 0.099;
    EXPECT_FALSE(poseHelperPtr->checkViewCenterPointsAreApproxEquale(pose1, pose2));

    // 0.95 is viewCenterDistance
    pose2.position.x = 0.95;
    pose2.position.y = -0.95;
    // rotate z: 90° > 60° == mHypothesisUpdaterAngleThreshold
    pose2.orientation.w = 0.707;
    pose2.orientation.z = 0.707;
    EXPECT_FALSE(poseHelperPtr->checkViewCenterPointsAreApproxEquale(pose1, pose2));

    // 0.95 is viewCenterDistance
    pose2.position.x = 0.95 * 2;
    pose2.position.y = 0.0;
    // rotate z: 180°
    pose2.orientation.w = 0.0;
    pose2.orientation.z = 1.0;
    EXPECT_FALSE(poseHelperPtr->checkViewCenterPointsAreApproxEquale(pose1, pose2));
}

bool getDistance(asr_robot_model_services::GetDistance::Request &req, asr_robot_model_services::GetDistance::Response &res) {
    return true;
}

bool hasAdvertised;

int advertiseService() {
    ros::NodeHandle n(ros::this_node::getName());
    ros::ServiceServer service_GetDistance = n.advertiseService("/asr_robot_model_services/GetDistance", getDistance);
    while (true) {
        ros::spinOnce();
        hasAdvertised = true;
        boost::this_thread::sleep(boost::posix_time::milliseconds(10));
    }
    return 0;
}

TEST_F(PoseHelperSetup, ResetInstance) {
    n.setParam("distanceFunc", 2);
    PoseHelper::resetInstance();
    PoseHelperPtr poseHelperPtr = PoseHelper::getInstance();

    geometry_msgs::Pose pose1;
    geometry_msgs::Pose pose2;

    pose2.position.x = 1.0;
    pose2.position.y = 0.0;

    EXPECT_NEAR(1.0, poseHelperPtr->calculateDistance(pose1, pose2), EPSILON);

    n.setParam("distanceFunc", 1);
    PoseHelper::resetInstance();

    hasAdvertised = false;
    boost::thread t = boost::thread(advertiseService);
    while (!hasAdvertised) {}

    EXPECT_NEAR(0.0, poseHelperPtr->calculateDistance(pose1, pose2), EPSILON);
    t.interrupt();
    t.join();

}


/*
 dfc2.cpp

Copyright (c) 2016 Terumasa Tadano

This file is distributed under the terms of the MIT license.
Please see the file 'LICENCE.txt' in the root directory
or http://opensource.org/licenses/mit-license.php for information.
*/

#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <map>
#include <vector>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
#include <boost/version.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>

#include "memory.h"
#include "xml_parser.h"
#include "dfc2.h"
#include "constants.h"
#include "mathfunctions.h"

using namespace std;

int main()
{
    cout << " DFC2 -- a generator of renormalized harmonic FCs from SCPH outputs." << endl;
    cout << " XML file containing original FC2 : ";
    cin >> original_xml;
    cout << " Output xml filename with anharmonic correction : ";
    cin >> new_xml;
    cout << " FC2 correction file from SCPH calculation : ";
    cin >> file_fc2_correction;
    cout << " Target temperature : ";
    cin >> temp;

    // Load original harmonic force constants and structure data of the supercell
    load_fc2_xml(original_xml);

    // Load anharmonic correction and structure data of the primitive lattice
    load_delta_fc2(file_fc2_correction, temp);

    // Initialize new fc2 by the original values
    fc2_new.clear();
    copy(fc2_orig.begin(), fc2_orig.end(), back_inserter(fc2_new));

    // Add delta_fc2 to fc2_new
    calculate_new_fc2(fc2_orig, delta_fc2, fc2_new);

    write_new_xml(fc2_new, new_xml);

    cout << endl << " New XML file " << new_xml << " was created successfully." << endl;

    deallocate(xr_s);
    deallocate(kd);
    deallocate(kd_symbol);
    deallocate(map_p2s);
    deallocate(map_s2p);
    deallocate(xr_p);
    deallocate(kd_p);
}

void load_fc2_xml(const std::string file_in)
{
    int i;
    using namespace boost::property_tree;

    ptree pt;
    int atm1, atm2, xyz1, xyz2, cell_s;
    stringstream ss1, ss2;
    FcsClassExtent fcext_tmp;

    map<string, int> dict_atomic_kind;

    try {
        read_xml(file_in, pt);
    }
    catch (exception &e) {
        cout << "Cannot open file " + file_in << endl;
        exit(EXIT_FAILURE);
    }

    nat = boost::lexical_cast<unsigned int>(
        get_value_from_xml(pt,
                           "Data.Structure.NumberOfAtoms"));
    nkd = boost::lexical_cast<unsigned int>(
        get_value_from_xml(pt,
                           "Data.Structure.NumberOfElements"));

    ntran = boost::lexical_cast<unsigned int>(
        get_value_from_xml(pt,
                           "Data.Symmetry.NumberOfTranslations"));

    natmin = nat / ntran;

    for (i = 0; i < 3; ++i) {
        ss1.str("");
        ss1.clear();
        ss1 << get_value_from_xml(pt,
                                  "Data.Structure.LatticeVector.a"
                                  + boost::lexical_cast<string>(i + 1));
        ss1 >> lavec_s[0][i] >> lavec_s[1][i] >> lavec_s[2][i];
    }

    // Parse atomic elements and coordinates

    allocate(xr_s, nat, 3);
    allocate(kd, nat);
    allocate(kd_symbol, nkd);

    i = 0;

    BOOST_FOREACH(const ptree::value_type& child_, pt.get_child("Data.Structure.AtomicElements")) {
        const ptree &child = child_.second;
        const unsigned int icount_kd = child.get<unsigned int>("<xmlattr>.number");
        dict_atomic_kind[boost::lexical_cast<string>(child_.second.data())] = icount_kd - 1;
        kd_symbol[i++] = boost::lexical_cast<string>(child_.second.data());
    }

    unsigned int index;

    BOOST_FOREACH(const ptree::value_type& child_, pt.get_child("Data.Structure.Position")) {
        const ptree &child = child_.second;
        const string str_index = child.get<string>("<xmlattr>.index");
        const string str_element = child.get<string>("<xmlattr>.element");

        ss1.str("");
        ss1.clear();
        ss1 << child.data();

        index = boost::lexical_cast<unsigned int>(str_index) - 1;

        if (index >= nat) {
            cout << "index is out of range" << endl;
            exit(EXIT_FAILURE);
        }

        kd[index] = dict_atomic_kind[str_element];
        ss1 >> xr_s[index][0] >> xr_s[index][1] >> xr_s[index][2];
    }

    dict_atomic_kind.clear();

    // Parse mapping information

    allocate(map_p2s, natmin, ntran);
    allocate(map_s2p, nat);

    unsigned int tran, atom_p, atom_s;

    BOOST_FOREACH(const ptree::value_type& child_, pt.get_child("Data.Symmetry.Translations")) {
        const ptree &child = child_.second;
        const string str_tran = child.get<string>("<xmlattr>.tran");
        const string str_atom = child.get<string>("<xmlattr>.atom");

        tran = boost::lexical_cast<unsigned int>(str_tran) - 1;
        atom_p = boost::lexical_cast<unsigned int>(str_atom) - 1;
        atom_s = boost::lexical_cast<unsigned int>(child.data()) - 1;

        if (tran >= ntran || atom_p >= natmin || atom_s >= nat) {
            cout << "index is out of range" << endl;
            exit(EXIT_FAILURE);
        }

        map_p2s[atom_p][tran] = atom_s;
        map_s2p[atom_s].atom_num = atom_p;
        map_s2p[atom_s].tran_num = tran;
    }

    BOOST_FOREACH(const ptree::value_type& child_, pt.get_child("Data.ForceConstants.HARMONIC")) {
        const ptree &child = child_.second;
        const string str_p1 = child.get<string>("<xmlattr>.pair1");
        const string str_p2 = child.get<string>("<xmlattr>.pair2");

        ss1.str("");
        ss2.str("");
        ss1.clear();
        ss2.clear();

        ss1 << str_p1;
        ss2 << str_p2;

        ss1 >> atm1 >> xyz1;
        ss2 >> atm2 >> xyz2 >> cell_s;

        fcext_tmp.atm1 = atm1 - 1;
        fcext_tmp.xyz1 = xyz1 - 1;
        fcext_tmp.atm2 = atm2 - 1;
        fcext_tmp.xyz2 = xyz2 - 1;
        fcext_tmp.cell_s = cell_s - 1;
        fcext_tmp.fcs_val = boost::lexical_cast<double>(child.data());

        fc2_orig.push_back(fcext_tmp);
    }
}

void load_delta_fc2(const std::string file_in, const double temp)
{
    int i;
    ifstream ifs_in;
    stringstream ss;

    // Restart
    ifs_in.open(file_in.c_str(), ios::in);
    if (!ifs_in) {
        cout << "Could not open " + file_in << endl;
        exit(EXIT_FAILURE);
    }

    // Check the consistency

    string line_tmp, str_tmp;
    vector<string> str_vec;

    int sx, sy, sz;
    int atm1, atm2, xyz1, xyz2;
    double dfc2_tmp;
    bool found_tag = false;

    // Get lattice vectors
    for (i = 0; i < 3; ++i) {
        ifs_in >> lavec_p[0][i] >> lavec_p[1][i] >> lavec_p[2][i];
    }
    recips(lavec_p, rlavec_p);

    ifs_in >> nat_p >> nkd_p;
    ifs_in.ignore();
    getline(ifs_in, line_tmp);

    allocate(xr_p, nat_p, 3);
    allocate(kd_p, nat_p);
    for (i = 0; i < nat_p; ++i) {
        ifs_in >> xr_p[i][0] >> xr_p[i][1] >> xr_p[i][2] >> kd_p[i];
    }
    ifs_in.ignore();

    while (getline(ifs_in, line_tmp)) {
        if (line_tmp[0] == '#') {
            boost::split(str_vec, line_tmp, boost::is_space());
            if (abs(boost::lexical_cast<double>(str_vec[3]) - temp) < eps) {
                found_tag = true;
                break;
            }
        }
    }
    if (!found_tag) {
        cout << "Could not find the # Temp tag for the target temperature" << endl;
        exit(EXIT_FAILURE);
    }

    delta_fc2.clear();

    while (getline(ifs_in, line_tmp)) {

        if (line_tmp[0] == '#') break;

        if (!line_tmp.empty()) {
            stringstream ss1;

            ss1 << line_tmp;
            ss1 >> sx >> sy >> sz >> atm1 >> xyz1 >> atm2 >> xyz2 >> dfc2_tmp;

            delta_fc2.push_back(DeltaFcs(sx, sy, sz, atm1, xyz1, atm2, xyz2, dfc2_tmp));
        }

    }

    ifs_in.close();
}

void calculate_new_fc2(std::vector<FcsClassExtent> fc2_in,
                       std::vector<DeltaFcs> delta_fc2,
                       std::vector<FcsClassExtent> &fc2_out)
{
    int i, j, k;
    int ix, iy, iz;
    int icell;
    double **xshift_s;

    allocate(xshift_s, 27, 3);

    for (i = 0; i < 3; ++i) xshift_s[0][i] = 0.0;

    icell = 0;

    for (ix = -1; ix <= 1; ++ix) {
        for (iy = -1; iy <= 1; ++iy) {
            for (iz = -1; iz <= 1; ++iz) {
                if (ix == 0 && iy == 0 && iz == 0) continue;

                ++icell;

                xshift_s[icell][0] = static_cast<double>(ix);
                xshift_s[icell][1] = static_cast<double>(iy);
                xshift_s[icell][2] = static_cast<double>(iz);
            }
        }
    }

    double mat_convert[3][3];

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            mat_convert[i][j] = 0.0;
            for (k = 0; k < 3; ++k) {
                mat_convert[i][j] += rlavec_p[i][k] * lavec_s[k][j] / (2.0 * pi);
            }

            mat_convert[i][j] = static_cast<double>(nint(mat_convert[i][j]));
        }
    }

    //
    //    cout << "lavec super:" << endl;
    //    for (i = 0; i < 3; ++i) {
    //        for (j = 0; j < 3; ++j) {
    //            cout << setw(15) << lavec_s[i][j];
    //        }
    //        cout << endl;
    //    }
    //    cout << endl;
    //
    //    cout << "lavec primitive:" << endl;
    //    for (i = 0; i < 3; ++i) {
    //        for (j = 0; j < 3; ++j) {
    //            cout << setw(15) << lavec_p[i][j];
    //        }
    //        cout << endl;
    //    }
    //    cout << endl;
    //
    //
    //    cout << "Mat convert:" << endl;
    //    for (i = 0; i < 3; ++i) {
    //        for (j = 0; j < 3; ++j) {
    //            cout << setw(15) << mat_convert[i][j];
    //        }
    //        cout << endl;
    //    }
    //    cout << endl;

    double vec[3];

    int icount = 0;

    vector<int> arr_tmp;
    vector<FcsTrans> fc2_data;

    fc2_data.clear();

    for (auto it = fc2_in.cbegin(); it != fc2_in.cend(); ++it) {

        arr_tmp.clear();

        for (i = 0; i < 3; ++i) {
            vec[i] = xr_s[(*it).atm2][i] + xshift_s[(*it).cell_s][i]
                - xr_s[map_p2s[map_s2p[(*it).atm2].atom_num][0]][i];
        }

        rotvec(vec, vec, mat_convert);

        arr_tmp.push_back((*it).atm1);
        arr_tmp.push_back((*it).xyz1);
        arr_tmp.push_back(map_s2p[(*it).atm2].atom_num);
        arr_tmp.push_back((*it).xyz2);
        for (i = 0; i < 3; ++i) arr_tmp.push_back(nint(vec[i]));

        fc2_data.push_back(FcsTrans(arr_tmp, icount));
        ++icount;
    }

    std::sort(fc2_data.begin(), fc2_data.end());

    vector<FcsTrans>::iterator iter_found;
    int index_tmp = 0;

    for (auto it = delta_fc2.begin(); it != delta_fc2.end(); ++it) {

        if (abs((*it).dfc2) > eps10) {
            arr_tmp.clear();
            arr_tmp.push_back((*it).atm1);
            arr_tmp.push_back((*it).xyz1);
            arr_tmp.push_back((*it).atm2);
            arr_tmp.push_back((*it).xyz2);
            arr_tmp.push_back((*it).sx);
            arr_tmp.push_back((*it).sy);
            arr_tmp.push_back((*it).sz);

            iter_found = lower_bound(fc2_data.begin(), fc2_data.end(), FcsTrans(arr_tmp, index_tmp));

            if (iter_found != fc2_data.end() && arr_tmp == (*iter_found).arr) {
                fc2_new[(*iter_found).fcs_index].fcs_val += (*it).dfc2;
            } else {
                cout << "Warning: The following force constant doesn't exist in the original file:" << endl;
                cout << setw(5) << (*it).sx << setw(5) << (*it).sy << setw(5) << (*it).sz;
                cout << setw(5) << (*it).atm1 << setw(5) << (*it).xyz1;
                cout << setw(5) << (*it).atm2 << setw(5) << (*it).xyz2;
                cout << setw(15) << (*it).dfc2 << endl;
            }
        }
        ++index_tmp;

    }
}

void recips(double vec[3][3], double inverse[3][3])
{
    double det;
    det = vec[0][0] * vec[1][1] * vec[2][2]
        + vec[1][0] * vec[2][1] * vec[0][2]
        + vec[2][0] * vec[0][1] * vec[1][2]
        - vec[0][0] * vec[2][1] * vec[1][2]
        - vec[2][0] * vec[1][1] * vec[0][2]
        - vec[1][0] * vec[0][1] * vec[2][2];

    if (abs(det) < eps12) {
        cout << "Lattice vector is singular" << endl;
        exit(EXIT_FAILURE);
    }

    double factor = 2.0 * pi / det;

    inverse[0][0] = (vec[1][1] * vec[2][2] - vec[1][2] * vec[2][1]) * factor;
    inverse[0][1] = (vec[0][2] * vec[2][1] - vec[0][1] * vec[2][2]) * factor;
    inverse[0][2] = (vec[0][1] * vec[1][2] - vec[0][2] * vec[1][1]) * factor;

    inverse[1][0] = (vec[1][2] * vec[2][0] - vec[1][0] * vec[2][2]) * factor;
    inverse[1][1] = (vec[0][0] * vec[2][2] - vec[0][2] * vec[2][0]) * factor;
    inverse[1][2] = (vec[0][2] * vec[1][0] - vec[0][0] * vec[1][2]) * factor;

    inverse[2][0] = (vec[1][0] * vec[2][1] - vec[1][1] * vec[2][0]) * factor;
    inverse[2][1] = (vec[0][1] * vec[2][0] - vec[0][0] * vec[2][1]) * factor;
    inverse[2][2] = (vec[0][0] * vec[1][1] - vec[0][1] * vec[1][0]) * factor;
}

void write_new_xml(const std::vector<FcsClassExtent> fc2_in,
                   const std::string xml_out)
{
    // Write to XML file

    int i, j;
    using boost::property_tree::ptree;

    ptree pt;
    string str_pos[3], str_tmp;

    str_tmp.clear();

    pt.put("Data.OriginalFC2", original_xml);
    pt.put("Data.SCPH_file", file_fc2_correction);
    pt.put("Data.SCPH_Temperature", double2string(temp, 5));
    pt.put("Data.Structure.NumberOfAtoms", nat);
    pt.put("Data.Structure.NumberOfElements", nkd);

    for (i = 0; i < nkd; ++i) {
        ptree &child = pt.add("Data.Structure.AtomicElements.element", kd_symbol[i]);
        child.put("<xmlattr>.number", i + 1);
    }

    for (i = 0; i < 3; ++i) {
        str_pos[i].clear();
        for (j = 0; j < 3; ++j) {
            str_pos[i] += " " + double2string(lavec_s[j][i]);
        }
    }
    pt.put("Data.Structure.LatticeVector", "");
    pt.put("Data.Structure.LatticeVector.a1", str_pos[0]);
    pt.put("Data.Structure.LatticeVector.a2", str_pos[1]);
    pt.put("Data.Structure.LatticeVector.a3", str_pos[2]);

    pt.put("Data.Structure.Position", "");

    for (i = 0; i < nat; ++i) {
        str_tmp.clear();
        for (j = 0; j < 3; ++j) str_tmp += " " + double2string(xr_s[i][j]);
        ptree &child = pt.add("Data.Structure.Position.pos", str_tmp);
        child.put("<xmlattr>.index", i + 1);
        child.put("<xmlattr>.element", kd_symbol[kd[i]]);
    }

    pt.put("Data.Symmetry.NumberOfTranslations", ntran);
    for (i = 0; i < ntran; ++i) {
        for (j = 0; j < natmin; ++j) {
            ptree &child = pt.add("Data.Symmetry.Translations.map", map_p2s[j][i] + 1);
            child.put("<xmlattr>.tran", i + 1);
            child.put("<xmlattr>.atom", j + 1);
        }
    }

    pt.put("Data.ForceConstants", "");
    str_tmp.clear();

    for (auto it = fc2_in.begin(); it != fc2_in.end(); ++it) {
        ptree &child = pt.add("Data.ForceConstants.HARMONIC.FC2", double2string((*it).fcs_val));

        child.put("<xmlattr>.pair1", boost::lexical_cast<std::string>((*it).atm1 + 1)
                  + " " + boost::lexical_cast<std::string>((*it).xyz1 + 1));
        child.put("<xmlattr>.pair2", boost::lexical_cast<std::string>((*it).atm2 + 1)
                  + " " + boost::lexical_cast<std::string>((*it).xyz2 + 1)
                  + " " + boost::lexical_cast<std::string>((*it).cell_s + 1));
    }

    using namespace boost::property_tree::xml_parser;
    const int indent = 2;

#if BOOST_VERSION >= 105600
    write_xml(xml_out, pt, std::locale(),
              xml_writer_make_settings<ptree::key_type>(' ', indent, widen<std::string>("utf-8")));
#else
    write_xml(xml_out, pt, std::locale(),
        xml_writer_make_settings(' ', indent, widen<char>("utf-8")));
#endif
}

string double2string(const double d, const int nprec)
{
    std::string rt;
    std::stringstream ss;

    ss << std::scientific << std::setprecision(nprec) << d;
    ss >> rt;
    return rt;
}


//
// The MIT License(MIT)
//
// Copyright(c) 2014 Demonsaw LLC
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/format.hpp>

#include "transfer_component.h"
#include "component/timer_component.h"
#include "component/io/file_component.h"
#include "utility/io/file_util.h"

namespace eja
{
	// Utility
	bool transfer_component::done() const
	{
		if (!m_done)
		{
			const auto owner = get_entity();
			const auto file = owner->get<file_component>();

			// v2.0
			////////////////////////////////////////////////////////
			//assert(m_size <= file->get_size());
			////////////////////////////////////////////////////////

			return file && m_size >= file->get_size();
		}

		return true;
	}

	// Accessor
	std::string transfer_component::get_elapsed() const
	{
		const auto owner = get_entity();
		const auto timer = owner->get<timer_component>();
		return timer ? timer->str() : "";
	}

	std::string transfer_component::get_estimate() const
	{
		if (m_size)
		{
			const auto owner = get_entity();
			const auto timer = owner->get<timer_component>();
			const auto file = owner->get<file_component>();

			if (timer && file)
			{
				const auto ratio = static_cast<double>(timer->milliseconds()) / static_cast<double>(m_size);
				const auto remaining = static_cast<size_t>(ratio * (file->get_size() - m_size));

				const auto hour = ms_to_hour(remaining);
				const auto min = ms_to_min(remaining);
				const auto sec = ms_to_sec(remaining) + 1;
				return boost::str(boost::format("%02d:%02d:%02d") % hour % min % sec);
			}
		}

		return "00:00:00";
	}

	size_t transfer_component::get_speed() const
	{
		const auto owner = get_entity();
		const auto timer = owner->get<timer_component>();
		if (timer)
		{
			const auto size = m_size - m_offset;
			const auto seconds = timer->seconds();
			return seconds ? static_cast<size_t>(size / seconds) : static_cast<size_t>(size);
		}

		return 0;
	}

	double transfer_component::get_ratio() const
	{
		const auto owner = get_entity();
		const auto file = owner->get<file_component>();

		if (file && !file->empty())
		{
			const auto value = (static_cast<double>(m_size) / static_cast<double>(file->get_size()));

			// v1.0
			////////////////////////////////////////////////////////
			return (value <= 1.0) ? value : 1.0;
			////////////////////////////////////////////////////////

			// v2.0
			////////////////////////////////////////////////////////
			//assert(value <= 100.0);
			//return value;
			////////////////////////////////////////////////////////
		}

		return 0.0;
	}
}


#include "ScanRegistration.h"
#include "common/math_utils.h"
#include "common/pcl_util.h"
#include <pcl/filters/voxel_grid.h>
#include <tf/transform_datatypes.h>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace lidar_slam {

RegistrationParams::RegistrationParams(
    float scanPeriod_, int imuHistorySize_, int nFeatureRegions_,
    int curvatureRegion_, int maxCornerSharp_, int maxSurfaceFlat_,
    float lessFlatFilterSize_, float surfaceCurvatureThreshold_,
    float cornerCurvatureThreshold_, bool cornerCheckEnable_,
    float blindDegreeThreshold_, std::string curvatureEstimateMethod_)
    : scanPeriod(scanPeriod_), imuHistorySize(imuHistorySize_),
      nFeatureRegions(nFeatureRegions_), curvatureRegion(curvatureRegion_),
      maxCornerSharp(maxCornerSharp_), maxCornerLessSharp(10 * maxCornerSharp_),
      maxSurfaceFlat(maxSurfaceFlat_), lessFlatFilterSize(lessFlatFilterSize_),
      surfaceCurvatureThreshold(surfaceCurvatureThreshold_),
      cornerCurvatureThreshold(cornerCurvatureThreshold_),
      cornerCheckEnable(cornerCheckEnable_),
      blindDegreeThreshold(blindDegreeThreshold_),
      blindThreshold(cos(deg2rad(blindDegreeThreshold))),
      curvatureEstimateMethod(curvatureEstimateMethod_){

      };

bool RegistrationParams::initialize_params(ros::NodeHandle &nh) {
  scanPeriod = nh.param<float>("scanPeriod", 0.1);
  imuHistorySize = nh.param<int>("imuHistorySize", 200);
  nFeatureRegions = nh.param<int>("nFeatureRegions", 6);
  curvatureRegion = nh.param<int>("curvatureRegion", 5);
  maxCornerSharp = nh.param<int>("maxCornerSharp", 2);
  maxCornerLessSharp = nh.param<int>("maxCornerLessSharp", 20);
  maxSurfaceFlat = nh.param<int>("maxSurfaceFlat", 4);
  surfaceCurvatureThreshold =
      nh.param<float>("surfaceCurvatureThreshold", 0.02);
  cornerCurvatureThreshold = nh.param<float>("cornerCurvatureThreshold", 1.0);
  lessFlatFilterSize = nh.param<float>("lessFlatFilterSize", 0.2);
  cornerCheckEnable = nh.param<bool>("cornerCheckEnable", true);
  blindDegreeThreshold = nh.param<float>("blindDegreeThreshold", 0.5);
  blindThreshold = (cos(deg2rad(blindDegreeThreshold))), param_print();

  return true;
}

ScanRegistration::ScanRegistration(const RegistrationParams &config)
    : _config(config), _sweepStart(), _scanTime(), _imuStart(), _imuCur(),
      _imuIdx(0), _imuHistory(_config.imuHistorySize), _laserCloud(),
      _cornerPointsSharp(), _cornerPointsLessSharp(), _surfacePointsFlat(),
      _surfacePointsLessFlat(), _imuTrans(4, 1), _regionCurvature(),
      _regionLabel(), _regionSortIndices(), _scanNeighborPicked() {}

bool ScanRegistration::setup(ros::NodeHandle &node,
                             ros::NodeHandle &privateNode) {
  if (!_config.initialize_params(privateNode)) {
    return false;
  }
  _imuHistory.ensureCapacity(_config.imuHistorySize);

  // subscribe to IMU topic
  _subImu = node.subscribe<sensor_msgs::Imu>(
      "/imu/data", 50, &ScanRegistration::handleIMUMessage, this);

  // advertise scan registration topics
  _pubLaserCloud =
      node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);
  _pubCornerPointsSharp =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);
  _pubCornerPointsLessSharp =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);
  _pubSurfPointsFlat =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);
  _pubSurfPointsLessFlat =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);
  _pubImuTrans = node.advertise<sensor_msgs::PointCloud2>("/imu_trans", 5);
  _pubPointsBlind = node.advertise<sensor_msgs::PointCloud2>("/point_blind", 2);
  _pubPointsBlock = node.advertise<sensor_msgs::PointCloud2>("/point_block", 2);
  _pubPointsSlop = node.advertise<sensor_msgs::PointCloud2>("/point_slop", 2);
  _pubCurvature =
      node.advertise<sensor_msgs::PointCloud2>("/point_curvature", 2);
  return true;
}

void ScanRegistration::handleIMUMessage(
    const sensor_msgs::Imu::ConstPtr &imuIn) {
  double roll, pitch, yaw;
  tf::Quaternion orientation;
  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

  Vector3 acc;
  acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81);
  acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81);
  acc.z() = float(imuIn->linear_acceleration.x + sin(pitch) * 9.81);

  IMUState newState;
  newState.stamp = imuIn->header.stamp;
  newState.roll = roll;
  newState.pitch = pitch;
  newState.yaw = yaw;
  newState.acceleration = acc;

  if (_imuHistory.size() > 0) {
    // accumulate IMU position and velocity over time
    rotateZXY(acc, newState.roll, newState.pitch, newState.yaw);

    const IMUState &prevState = _imuHistory.last();
    float timeDiff = float((newState.stamp - prevState.stamp).toSec());
    newState.position = prevState.position + (prevState.velocity * timeDiff) +
                        (0.5 * acc * timeDiff * timeDiff);
    newState.velocity = prevState.velocity + acc * timeDiff;
  }

  _imuHistory.push(newState);
}

void ScanRegistration::reset(const ros::Time &scanTime, const bool &newSweep) {
  _scanTime = scanTime;

  // re-initialize IMU start index and state
  _imuIdx = 0;
  if (hasIMUData()) {
    interpolateIMUStateFor(0, _imuStart);
  }

  // clear internal cloud buffers at the beginning of a sweep
  if (newSweep) {
    _sweepStart = scanTime;

    // clear cloud buffers
    _laserCloud.clear();
    _cornerPointsSharp.clear();
    _cornerPointsLessSharp.clear();
    _surfacePointsFlat.clear();
    _surfacePointsLessFlat.clear();
    _pointsBlind.clear();
    _pointsBlock.clear();
    _pointsSlop.clear();
    _pointsCurvature.clear();
    // clear scan indices vector
    _scanIndices.clear();
  }
}

void ScanRegistration::setIMUTransformFor(const float &relTime) {
  interpolateIMUStateFor(relTime, _imuCur);

  float relSweepTime = (_scanTime - _sweepStart).toSec() + relTime;
  _imuPositionShift =
      _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;
}

void ScanRegistration::transformToStartIMU(PointIN &point) {
  // rotate point to global IMU system
  rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);

  // add global IMU position shift
  point.x += _imuPositionShift.x();
  point.y += _imuPositionShift.y();
  point.z += _imuPositionShift.z();

  // rotate point back to local IMU system relative to the start IMU state
  rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
}

void ScanRegistration::interpolateIMUStateFor(const float &relTime,
                                              IMUState &outputState) {
  double timeDiff = (_scanTime - _imuHistory[_imuIdx].stamp).toSec() + relTime;
  while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {
    _imuIdx++;
    timeDiff = (_scanTime - _imuHistory[_imuIdx].stamp).toSec() + relTime;
  }

  if (_imuIdx == 0 || timeDiff > 0) {
    outputState = _imuHistory[_imuIdx];
  } else {
    float ratio =
        -timeDiff /
        (_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp).toSec();
    IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio,
                          outputState);
  }
}

void ScanRegistration::extractFeatures(const uint16_t &beginIdx) {
  // extract features from individual scans

  int blind_points = 0;
  int slope_points = 0;
  int block_points = 0;
  int lessFlat_points = 0;
  int mindis_points = 0;
  size_t nScans = _scanIndices.size();
  for (size_t i = beginIdx; i < nScans; i++) {
    CloudI::Ptr surfPointsLessFlatScan(new CloudI);
    size_t scanStartIdx = _scanIndices[i].first;
    size_t scanEndIdx = _scanIndices[i].second;

    // skip empty scans
    if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) {
      continue;
    }

    // reset scan buffers
    setScanBuffersFor(scanStartIdx, scanEndIdx);

    // for debug
    /*
    for (int i = scanStartIdx; i <= scanEndIdx; i++) {
      switch (_scanNeighborPicked[i]) {
      case 4: {
        blind_points++;
        break;
      }
      case 3: {
        blind_points++;
        break;
      }
      case 2: {
        blind_points++;
        break;
      }
      case 1: {
        slope_points++;
        break;
      }
      }
    }
    */
    // for debug visulization
    /*
    for (int ii = scanStartIdx; ii <= scanEndIdx; ii++) {
      if (_scanNeighborPicked[ii - scanStartIdx] == 4) {
        _pointsBlind.push_back(toXYZI(_laserCloud[ii]));
      } else if (_scanNeighborPicked[ii - scanStartIdx] > 1) {
        _pointsBlock.push_back(toXYZI(_laserCloud[ii]));
      } else if (_scanNeighborPicked[ii - scanStartIdx] == 1) {
        _pointsSlop.push_back(toXYZI(_laserCloud[ii]));
      }
    }*/

    // extract features from equally sized scan regions
    for (int j = 0; j < _config.nFeatureRegions; j++) {
      size_t sp = ((scanStartIdx + _config.curvatureRegion) *
                       (_config.nFeatureRegions - j) +
                   (scanEndIdx - _config.curvatureRegion) * j) /
                  _config.nFeatureRegions;
      size_t ep = ((scanStartIdx + _config.curvatureRegion) *
                       (_config.nFeatureRegions - 1 - j) +
                   (scanEndIdx - _config.curvatureRegion) * (j + 1)) /
                      _config.nFeatureRegions -
                  1;

      // skip empty regions
      if (ep <= sp) {
        continue;
      }

      size_t regionSize = ep - sp + 1;
      setRegionBuffersFor(sp, ep);

      // extract flat surface features
      int surfPickedNum = 0;
      for (int k = 0; k < regionSize && surfPickedNum < _config.maxSurfaceFlat;
           k++) {
        size_t idx = _regionSortIndices[k];
        size_t scanIdx = idx - scanStartIdx;
        size_t regionIdx = idx - sp;

        if (_scanNeighborPicked[scanIdx] != SURF_PICKED_NEAR &&
            _regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) {

          surfPickedNum++;
          _regionLabel[regionIdx] = SURFACE_FLAT;
          _surfacePointsFlat.push_back(toXYZI(_laserCloud[idx]));

          markAsPicked(idx, scanIdx, SURF_PICKED_NEAR);
        }
      }

      // extract less flat surface features and edge_broken
      for (int k = 0; k < regionSize; k++) {
        size_t idx = sp + k;
        size_t scanIdx = idx - scanStartIdx;

        if (_regionCurvature[k] < _config.surfaceCurvatureThreshold) {
          surfPointsLessFlatScan->push_back(toXYZI(_laserCloud[idx]));
          mindis_points++;
          if (_regionLabel[k] != SURFACE_FLAT)
            _regionLabel[k] = SURFACE_LESS_FLAT;
        }
        if (_scanNeighborPicked[scanIdx] == EDGE_BROKEN) {
          _cornerPointsSharp.push_back(toXYZI(_laserCloud[idx]));
          _cornerPointsLessSharp.push_back(toXYZI(_laserCloud[idx]));
          _regionLabel[k] = CORNER_SHARP;
          _pointsBlock.push_back(toXYZI(_laserCloud[idx]));
        }
      }

      // extract  features
      int cornerPickedNum = 0;
      surfPickedNum = 0;
      for (size_t k = regionSize; k > 0;) {
        size_t idx = _regionSortIndices[--k];
        size_t scanIdx = idx - scanStartIdx;
        size_t regionIdx = idx - sp;

        if (_regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold)
          break;

        int point_label = pointClassify(idx);
        switch (point_label) {
        case MESSY: {
          //_pointsBlock.push_back(toXYZI(_laserCloud[idx]));
          break;
        }
        case SURFACE_FLAT: {
          _regionLabel[regionIdx] = SURFACE_FLAT;
          if (surfPickedNum < _config.maxSurfaceFlat) {
            surfPickedNum++;
            //_surfacePointsFlat.push_back(toXYZI(_laserCloud[idx]));
          }
          surfPointsLessFlatScan->push_back(toXYZI(_laserCloud[idx]));
          _pointsBlind.push_back(toXYZI(_laserCloud[idx]));
          break;
        }
        case CORNER_SHARP: {
          if (_scanNeighborPicked[scanIdx] > EDGE_BROKEN) {
            _regionLabel[regionIdx] = CORNER_SHARP;
            if (cornerPickedNum < _config.maxCornerSharp) {
              cornerPickedNum++;
              _cornerPointsSharp.push_back(toXYZI(_laserCloud[idx]));
            }
            _cornerPointsLessSharp.push_back(toXYZI(_laserCloud[idx]));
            _pointsSlop.push_back(toXYZI(_laserCloud[idx]));
          }
          break;
        }
        case ONESIDE_FLAT: {
          _regionLabel[regionIdx] = ONESIDE_FLAT;
          if (surfPickedNum < _config.maxSurfaceFlat) {
            surfPickedNum++;
            _surfacePointsFlat.push_back(toXYZI(_laserCloud[idx]));
          }
          surfPointsLessFlatScan->push_back(toXYZI(_laserCloud[idx]));
          _pointsCurvature.push_back(toXYZI(_laserCloud[idx]));
          break;
        }
        }
        /*
        if (point_label == 0) {
          _pointsBlock.push_back(toXYZI(_laserCloud[idx]));
          continue;

        } else if (point_label == 1) {
          _regionLabel[regionIdx] = SURFACE_FLAT;
          //_surfacePointsFlat.push_back(toXYZI(_laserCloud[idx]));
          surfPointsLessFlatScan->push_back(toXYZI(_laserCloud[idx]));
          _pointsBlind.push_back(toXYZI(_laserCloud[idx]));

          // markAsPicked(idx, scanIdx, 12);
        } else if (point_label == 2 && _scanNeighborPicked[scanIdx] == 0) {
          _regionLabel[regionIdx] = CORNER_SHARP;
          _cornerPointsSharp.push_back(toXYZI(_laserCloud[idx]));
          _cornerPointsLessSharp.push_back(toXYZI(_laserCloud[idx]));
          _pointsSlop.push_back(toXYZI(_laserCloud[idx]));

        } else if (point_label == 3) {
          _regionLabel[regionIdx] = SURFACE_FLAT;
          //_surfacePointsFlat.push_back(toXYZI(_laserCloud[idx]));
          surfPointsLessFlatScan->push_back(toXYZI(_laserCloud[idx]));

          if (_scanNeighborPicked[scanIdx] == 0 &&
              _regionCurvature[regionIdx] > 9.0) {
            _regionLabel[regionIdx] = CORNER_SHARP;
            _cornerPointsSharp.push_back(toXYZI(_laserCloud[idx]));
            _cornerPointsLessSharp.push_back(toXYZI(_laserCloud[idx]));
            // markAsPicked(idx, scanIdx, 13);
            _pointsCurvature.push_back(toXYZI(_laserCloud[idx]));
          }
        }*/
      }
    }

    // down size less flat surface point cloud of current scan
    CloudI surfPointsLessFlatScanDS;
    pcl::VoxelGrid<PointI> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(_config.lessFlatFilterSize,
                               _config.lessFlatFilterSize,
                               _config.lessFlatFilterSize);
    downSizeFilter.filter(surfPointsLessFlatScanDS);
    lessFlat_points += surfPointsLessFlatScan->points.size();
    _surfacePointsLessFlat += surfPointsLessFlatScanDS;
  }
/*
  std::cout << "[Features:]\n"
            << " ,blind_points:" << blind_points
            << " ,slope_points:" << slope_points
            << " ,block_points:" << block_points
            << " ,totoal_points:" << _scanIndices[nScans - 1].second
            << " ,lessFlat_points:" << lessFlat_points
            << " ,mindis_points:" << mindis_points << std::endl;

  ROS_WARN("sharp:%zu, less sharp:%zu", _cornerPointsSharp.points.size(),
           _cornerPointsLessSharp.points.size());
  ROS_WARN("surf:%zu, less surf:%zu", _surfacePointsFlat.points.size(),
           _surfacePointsLessFlat.points.size());
  ROS_WARN("blind:%zu, slop:%zu", _pointsBlind.points.size(),
           _pointsSlop.points.size());
  ROS_WARN("block:%zu", _pointsBlock.points.size());
  ROS_WARN("Curv:%zu", _pointsCurvature.points.size());*/
}

void ScanRegistration::setRegionBuffersFor(const size_t &startIdx,
                                           const size_t &endIdx) {
  // resize buffers
  size_t regionSize = endIdx - startIdx + 1;
  _regionCurvature.resize(regionSize);
  _regionSortIndices.resize(regionSize);
  _swapRegionSortIndices.resize(regionSize);
  _regionLabel.assign(regionSize, UNKNOW);

  // calculate point curvatures and reset sort indices
  float pointWeight = -2 * _config.curvatureRegion;

  for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) {
    float diffX = pointWeight * _laserCloud[i].x;
    float diffY = pointWeight * _laserCloud[i].y;
    float diffZ = pointWeight * _laserCloud[i].z;

    for (int j = 1; j <= _config.curvatureRegion; j++) {
      diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;
      diffY += _laserCloud[i + j].y + _laserCloud[i - j].y;
      diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z;
    }

    _regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ;
    _regionSortIndices[regionIdx] = i - startIdx;
  }

  // printf("40 * %d * %d * 8.5 = %d\n", _config.nFeatureRegions, regionSize, 40 * _config.nFeatureRegions * regionSize * 8.5);
  // sort point curvatures
  mergeSort(_regionSortIndices, 0, regionSize - 1, _swapRegionSortIndices);
  for(int i = 0; i < regionSize; i ++)
    _regionSortIndices[i] = _regionSortIndices[i] + startIdx;
  // for (size_t i = 1; i < regionSize; i++) {
  //   for (size_t j = i; j >= 1; j--) {
  //     if (_regionCurvature[_regionSortIndices[j] - startIdx] <
  //         _regionCurvature[_regionSortIndices[j - 1] - startIdx]) {
  //       std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]);
  //     }
  //   }
  // }
}

void ScanRegistration::setScanBuffersFor(const size_t &startIdx,
                                         const size_t &endIdx) {
  // resize buffers
  size_t scanSize = endIdx - startIdx + 1;
  _scanNeighborPicked.assign(scanSize, 0);

  for (int i = 0; i < _config.curvatureRegion; ++i) {
    const PointIN &point = (_laserCloud[startIdx + i]);
    const PointIN &nextPoint = (_laserCloud[startIdx + i + 1]);
    if (calcCosAngleDiff(point, nextPoint) < _config.blindThreshold) {
      std::fill_n(&_scanNeighborPicked[i], _config.curvatureRegion + 1,
                  BLIND_BLOCK);
    }
  }
  for (int i = 0; i < _config.curvatureRegion; ++i) {
    const PointIN &point = (_laserCloud[endIdx - i]);
    const PointIN &previousPoint = (_laserCloud[endIdx - i - 1]);
    if (calcCosAngleDiff(point, previousPoint) < _config.blindThreshold) {
      std::fill_n(
          &_scanNeighborPicked[endIdx - i - startIdx - _config.curvatureRegion],
          _config.curvatureRegion + 1, BLIND_BLOCK);
    }
  }

  // mark unreliable points as picked
  for (size_t i = startIdx + _config.curvatureRegion;
       i < endIdx - _config.curvatureRegion; i++) {
    const PointIN &previousPoint = (_laserCloud[i - 1]);
    const PointIN &point = (_laserCloud[i]);
    const PointIN &nextPoint = (_laserCloud[i + 1]);

    float diffNext = calcSquaredDiff(nextPoint, point);
    if (calcCosAngleDiff(point, nextPoint) < _config.blindThreshold) {
      std::fill_n(
          &_scanNeighborPicked[i - startIdx - _config.curvatureRegion + 1],
          _config.curvatureRegion * 2, BLIND_BLOCK);
      continue;
    }

    if (diffNext > 1.0) {
      float depth1 = calcPointDistance(point);
      float depth2 = calcPointDistance(nextPoint);
      float diffPrev = calcSquaredDiff(previousPoint, point);
      if (depth1 > depth2) {
        if (_scanNeighborPicked[i - startIdx + 1] > NEAR_BLOCK &&
            diffPrev / diffNext < 0.2)
          _scanNeighborPicked[i - startIdx + 1] = EDGE_BROKEN;
        std::fill_n(
            &_scanNeighborPicked[i - startIdx - _config.curvatureRegion + 1],
            _config.curvatureRegion, NEAR_BLOCK);

      } else {
        if (_scanNeighborPicked[i - startIdx] > NEAR_BLOCK &&
            diffPrev / diffNext < 0.2)
          _scanNeighborPicked[i - startIdx] = EDGE_BROKEN;
        std::fill_n(&_scanNeighborPicked[i - startIdx + 1],
                    _config.curvatureRegion, NEAR_BLOCK);
      }
    }
  }
}

void ScanRegistration::markAsPicked(const size_t &cloudIdx,
                                    const size_t &scanIdx, int label) {
  _scanNeighborPicked[scanIdx] = label;

  for (int i = 1; i <= _config.curvatureRegion; i++) {
    // if (calcSquaredDiff(_laserCloud[cloudIdx + i],
    //                    _laserCloud[cloudIdx + i - 1]) > 0.05) {
    //  break;
    //}

    _scanNeighborPicked[scanIdx + i] = label;
  }

  for (int i = 1; i <= _config.curvatureRegion; i++) {
    // if (calcSquaredDiff(_laserCloud[cloudIdx - i],
    //                    _laserCloud[cloudIdx - i + 1]) > 0.05) {
    //  break;
    //}

    _scanNeighborPicked[scanIdx - i] = label;
  }
}

int ScanRegistration::pointClassify(const size_t &cloudIdx) {

  Eigen::Matrix3f _lineMatA;
  Eigen::Matrix<float, 1, 3> _lineMatD;
  Eigen::Matrix3f _lineMatV;
  Eigen::Vector3f _lineCentroid;

  Eigen::Vector3f v1, v2;
  bool line1 = false;
  bool line2 = false;
  if (1) {
    // compute mean
    _lineCentroid.setZero();
    for (int j = 0; j <= _config.curvatureRegion; j++) {
      _lineCentroid += _laserCloud[cloudIdx - j].getVector3fMap();
    }
    _lineCentroid /= (_config.curvatureRegion + 1);

    // compute CovarianceMatrix
    _lineMatA.setZero();
    for (int j = 0; j <= _config.curvatureRegion; j++) {
      Eigen::Vector3f a =
          _laserCloud[cloudIdx - j].getVector3fMap() - _lineCentroid;
      _lineMatA(0, 0) += a(0) * a(0);
      _lineMatA(1, 0) += a(0) * a(1);
      _lineMatA(2, 0) += a(0) * a(2);
      _lineMatA(1, 1) += a(1) * a(1);
      _lineMatA(2, 1) += a(1) * a(2);
      _lineMatA(2, 2) += a(2) * a(2);
    }
    _lineMatA /= (_config.curvatureRegion + 1);

    // compute eigenvalues and eigenvectors
    _lineMatD.setZero();
    _lineMatV.setZero();
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(_lineMatA);
    _lineMatD = esolver.eigenvalues().real();
    _lineMatV = esolver.eigenvectors().real();

    // compute corner line and coefficients
    if (_lineMatD(0, 2) > 100 * _lineMatD(0, 1) &&
        _lineMatD(0, 2) > 10000 * _lineMatD(0, 0)) {
      v1 = _lineMatV.col(2);
      line1 = true;
      for (int j = 0; j <= _config.curvatureRegion; j++) {
        Eigen::Vector3f a =
            _laserCloud[cloudIdx - j].getVector3fMap() - _lineCentroid;

        float distance = (a.cross(v1)).norm() / v1.norm();
        if (fabs(distance) > 0.08) {
          line1 = false;
          break;
        }
      }
    }
  }
  if (1) {
    _lineCentroid.setZero();
    for (int j = -1.0 * _config.curvatureRegion; j <= 0; j++) {
      _lineCentroid += _laserCloud[cloudIdx - j].getVector3fMap();
    }
    _lineCentroid /= (_config.curvatureRegion + 1);

    // compute CovarianceMatrix
    _lineMatA.setZero();
    for (int j = -1.0 * _config.curvatureRegion; j <= 0; j++) {
      Eigen::Vector3f a =
          _laserCloud[cloudIdx - j].getVector3fMap() - _lineCentroid;

      _lineMatA(0, 0) += a(0) * a(0);
      _lineMatA(1, 0) += a(0) * a(1);
      _lineMatA(2, 0) += a(0) * a(2);
      _lineMatA(1, 1) += a(1) * a(1);
      _lineMatA(2, 1) += a(1) * a(2);
      _lineMatA(2, 2) += a(2) * a(2);
    }
    _lineMatA /= (_config.curvatureRegion + 1);

    // compute eigenvalues and eigenvectors
    _lineMatD.setZero();
    _lineMatV.setZero();
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(_lineMatA);
    _lineMatD = esolver.eigenvalues().real();
    _lineMatV = esolver.eigenvectors().real();

    // compute corner line and coefficients
    if (_lineMatD(0, 2) > 100 * _lineMatD(0, 1) &&
        _lineMatD(0, 2) > 10000 * _lineMatD(0, 0)) {

      v2 = _lineMatV.col(2);
      line2 = true;
      for (int j = -1.0 * _config.curvatureRegion; j <= 0; j++) {
        Eigen::Vector3f a =
            _laserCloud[cloudIdx - j].getVector3fMap() - _lineCentroid;

        float distance = (a.cross(v2)).norm() / v2.norm();
        if (fabs(distance) > 0.08) {
          line2 = false;
          break;
        }
      }
    }
  }

  if (line1 && line2) {
    float diff = calcCosAngleDiff(v1, v2);
    if (diff < cos(deg2rad(175.0)) || diff > cos(deg2rad(5.0))) {
      return SURFACE_FLAT;
    } else if (diff > cos(deg2rad(135.0)) && diff < cos(deg2rad(45.0))) {
      return CORNER_SHARP;
    }
  }
  if (line1 || line2) {
    return ONESIDE_FLAT;
  } else {
    return MESSY;
  }
  std::cout << "pointClassify error \n";
  return 0;
}

void ScanRegistration::publishResult() {
  // publish full resolution and feature point clouds
  publishCloudMsg(_pubLaserCloud, _laserCloud, _sweepStart, "/lidar");
  publishCloudMsg(_pubCornerPointsSharp, _cornerPointsSharp, _sweepStart,
                  "/lidar");
  publishCloudMsg(_pubCornerPointsLessSharp, _cornerPointsLessSharp,
                  _sweepStart, "/lidar");
  publishCloudMsg(_pubSurfPointsFlat, _surfacePointsFlat, _sweepStart,
                  "/lidar");
  publishCloudMsg(_pubSurfPointsLessFlat, _surfacePointsLessFlat, _sweepStart,
                  "/lidar");
  publishCloudMsg(_pubPointsBlind, _pointsBlind, _sweepStart, "/lidar");
  publishCloudMsg(_pubPointsBlock, _pointsBlock, _sweepStart, "/lidar");
  publishCloudMsg(_pubPointsSlop, _pointsSlop, _sweepStart, "/lidar");
  publishCloudMsg(_pubCurvature, _pointsCurvature, _sweepStart, "/lidar");

  // publish corresponding IMU transformation information
  _imuTrans[0].x = _imuStart.pitch.rad();
  _imuTrans[0].y = _imuStart.yaw.rad();
  _imuTrans[0].z = _imuStart.roll.rad();

  _imuTrans[1].x = _imuCur.pitch.rad();
  _imuTrans[1].y = _imuCur.yaw.rad();
  _imuTrans[1].z = _imuCur.roll.rad();

  Vector3 imuShiftFromStart = _imuPositionShift;
  rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch,
            -_imuStart.roll);

  _imuTrans[2].x = imuShiftFromStart.x();
  _imuTrans[2].y = imuShiftFromStart.y();
  _imuTrans[2].z = imuShiftFromStart.z();

  Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;
  rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch,
            -_imuStart.roll);

  _imuTrans[3].x = imuVelocityFromStart.x();
  _imuTrans[3].y = imuVelocityFromStart.y();
  _imuTrans[3].z = imuVelocityFromStart.z();

  publishCloudMsg(_pubImuTrans, _imuTrans, _sweepStart, "/lidar");
}

} // end namespace lidar_slam