/*
 * Copyright (c) 2011, Ivan Dryanovski, William Morris
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the CCNY Robotics Lab 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 OWNER 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.
 */

/*  This package uses Canonical Scan Matcher [1], written by
 *  Andrea Censi
 *
 *  [1] A. Censi, "An ICP variant using a point-to-line metric"
 *  Proceedings of the IEEE International Conference
 *  on Robotics and Automation (ICRA), 2008
 */

#include "laser_scan_matcher.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>

#include <pcl/registration/ndt.h>
#include <pcl/point_types.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <../include/pclomp/ndt_omp.h>

#include <boost/assign.hpp>

namespace scan_tools
{

LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private):
  nh_(nh),
  nh_private_(nh_private),
  initialized_(false)
{
  ROS_INFO("Starting LaserScanMatcher");

  // **** init parameters

  initParams();

  // **** state variables

  f2b_.setIdentity();
  f2b_kf_.setIdentity();
  input_.laser[0] = 0.0;
  input_.laser[1] = 0.0;
  input_.laser[2] = 0.0;

  // Initialize output_ vectors as Null for error-checking
  output_.cov_x_m = 0;
  output_.dx_dy1_m = 0;
  output_.dx_dy2_m = 0;

  // *** subscribers
  scan_subscriber_ = nh_.subscribe(
      "scan", 1, &LaserScanMatcher::scanCallback, this);

  // *** publishers
  origin_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("origin_cloud", 10);
  current_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("current_cloud", 10);
  current_pcl_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("current_cloud_pcl", 10);
}

LaserScanMatcher::~LaserScanMatcher()
{
  ROS_INFO("Destroying LaserScanMatcher");
}

void LaserScanMatcher::initParams()
{
  if (!nh_private_.getParam ("base_frame", base_frame_))
    base_frame_ = "base_link";
  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
    fixed_frame_ = "world";

  // **** keyframe params: when to generate the keyframe scan
  // if either is set to 0, reduces to frame-to-frame matching
  // 更新关键帧粒度，超过10cm或10度
  if (!nh_private_.getParam ("kf_dist_linear", kf_dist_linear_))
    kf_dist_linear_ = 5.10;
  if (!nh_private_.getParam ("kf_dist_angular", kf_dist_angular_))
    kf_dist_angular_ = 310.0 * (M_PI / 180.0);

  kf_dist_linear_sq_ = kf_dist_linear_ * kf_dist_linear_;

  // **** How to publish the output?
  // tf (fixed_frame->base_frame),
  // pose message (pose of base frame in the fixed frame)

  if (!nh_private_.getParam ("publish_tf", publish_tf_))
    publish_tf_ = true;

  // **** CSM parameters - comments copied from algos.h (by Andrea Censi)

  // Maximum angular displacement between scans
  if (!nh_private_.getParam ("max_angular_correction_deg", input_.max_angular_correction_deg))
    input_.max_angular_correction_deg = 45.0;

  // Maximum translation between scans (m)
  if (!nh_private_.getParam ("max_linear_correction", input_.max_linear_correction))
    input_.max_linear_correction = 0.50;

  // Maximum ICP cycle iterations
  if (!nh_private_.getParam ("max_iterations", input_.max_iterations))
    input_.max_iterations = 10;

  // A threshold for stopping (m)
  if (!nh_private_.getParam ("epsilon_xy", input_.epsilon_xy))
    input_.epsilon_xy = 0.000001;

  // A threshold for stopping (rad)
  if (!nh_private_.getParam ("epsilon_theta", input_.epsilon_theta))
    input_.epsilon_theta = 0.000001;

  // Maximum distance for a correspondence to be valid
  if (!nh_private_.getParam ("max_correspondence_dist", input_.max_correspondence_dist))
    input_.max_correspondence_dist = 0.3;

  // Noise in the scan (m)
  if (!nh_private_.getParam ("sigma", input_.sigma))
    input_.sigma = 0.010;

  // Use smart tricks for finding correspondences.
  if (!nh_private_.getParam ("use_corr_tricks", input_.use_corr_tricks))
    input_.use_corr_tricks = 1;

  // Restart: Restart if error is over threshold
  if (!nh_private_.getParam ("restart", input_.restart))
    input_.restart = 0;

  // Restart: Threshold for restarting
  if (!nh_private_.getParam ("restart_threshold_mean_error", input_.restart_threshold_mean_error))
    input_.restart_threshold_mean_error = 0.01;

  // Restart: displacement for restarting. (m)
  if (!nh_private_.getParam ("restart_dt", input_.restart_dt))
    input_.restart_dt = 1.0;

  // Restart: displacement for restarting. (rad)
  if (!nh_private_.getParam ("restart_dtheta", input_.restart_dtheta))
    input_.restart_dtheta = 0.1;

  // Max distance for staying in the same clustering
  if (!nh_private_.getParam ("clustering_threshold", input_.clustering_threshold))
    input_.clustering_threshold = 0.25;

  // Number of neighbour rays used to estimate the orientation
  if (!nh_private_.getParam ("orientation_neighbourhood", input_.orientation_neighbourhood))
    input_.orientation_neighbourhood = 20;

  // If 0, it's vanilla ICP
  if (!nh_private_.getParam ("use_point_to_line_distance", input_.use_point_to_line_distance))
    input_.use_point_to_line_distance = 1;

  // Discard correspondences based on the angles
  if (!nh_private_.getParam ("do_alpha_test", input_.do_alpha_test))
    input_.do_alpha_test = 0;

  // Discard correspondences based on the angles - threshold angle, in degrees
  if (!nh_private_.getParam ("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
    input_.do_alpha_test_thresholdDeg = 20.0;

  // Percentage of correspondences to consider: if 0.9,
  // always discard the top 10% of correspondences with more error
  if (!nh_private_.getParam ("outliers_maxPerc", input_.outliers_maxPerc))
    input_.outliers_maxPerc = 0.90;

  // Parameters describing a simple adaptive algorithm for discarding.
  //  1) Order the errors.
  //  2) Choose the percentile according to outliers_adaptive_order.
  //     (if it is 0.7, get the 70% percentile)
  //  3) Define an adaptive threshold multiplying outliers_adaptive_multinput_
  //     with the value of the error at the chosen percentile.
  //  4) Discard correspondences over the threshold.
  //  This is useful to be conservative; yet remove the biggest errors.
  if (!nh_private_.getParam ("outliers_adaptive_order", input_.outliers_adaptive_order))
    input_.outliers_adaptive_order = 0.7;

  if (!nh_private_.getParam ("outliers_adaptive_mult", input_.outliers_adaptive_mult))
    input_.outliers_adaptive_mult = 2.0;

  // If you already have a guess of the solution, you can compute the polar angle
  // of the points of one scan in the new position. If the polar angle is not a monotone
  // function of the readings index, it means that the surface is not visible in the
  // next position. If it is not visible, then we don't use it for matching.
  if (!nh_private_.getParam ("do_visibility_test", input_.do_visibility_test))
    input_.do_visibility_test = 0;

  // no two points in laser_sens can have the same corr.
  if (!nh_private_.getParam ("outliers_remove_doubles", input_.outliers_remove_doubles))
    input_.outliers_remove_doubles = 1;

  // If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
  if (!nh_private_.getParam ("do_compute_covariance", input_.do_compute_covariance))
    input_.do_compute_covariance = 0;

  // Checks that find_correspondences_tricks gives the right answer
  if (!nh_private_.getParam ("debug_verify_tricks", input_.debug_verify_tricks))
    input_.debug_verify_tricks = 0;

  // If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
  // incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
  if (!nh_private_.getParam ("use_ml_weights", input_.use_ml_weights))
    input_.use_ml_weights = 0;

  // If 1, the field 'readings_sigma' in the second scan is used to weight the
  // correspondence by 1/sigma^2
  if (!nh_private_.getParam ("use_sigma_weights", input_.use_sigma_weights))
    input_.use_sigma_weights = 0;
}

void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
  double dist = 0, theta = 0;
  // **** if first scan, cache the tf from base to the scanner
  // std::cout<<"received scan msgs: "<<scan_msg->ranges.size()<<std::endl;
  if (!initialized_)
  {
    createCache(scan_msg);    // caches the sin and cos of all angles

    // cache the static tf from base to laser
    if (!getBaseToLaserTf(scan_msg->header.frame_id))
    {
      ROS_WARN("Skipping scan");
      return;
    }

    laserScanToLDP(scan_msg, prev_ldp_scan_, scan_msg->angle_min, scan_msg->angle_max);
    last_icp_time_ = scan_msg->header.stamp;
    initialized_ = true;

    //for cloud display
    pc_origin_.width = prev_ldp_scan_->nrays;
    pc_origin_.height = 1;
    pc_origin_.resize(prev_ldp_scan_->nrays);

    // pc_origin_.header.stamp = ros::Time::now();
    // pc_origin_.header.frame_id = base_frame_;
    // pc_origin_.points.resize(prev_ldp_scan_->nrays);
    // std::cout<<base_frame_<<std::endl;
    for (int i = 0; i < prev_ldp_scan_->nrays; i++)
    {
      // printf("prev i, valid[i]: %d, %d\n",i,prev_ldp_scan_->valid[i]);
      if (prev_ldp_scan_->valid[i])
      {
        // std::cout<<"000"<<std::endl;
        dist = prev_ldp_scan_->readings[i];
        theta = prev_ldp_scan_->theta[i];
        // std::cout<<"dist, theta: "<<dist<<", "<<theta<<std::endl;
        // std::cout<<"cos, sin: "<<cos(theta)<<", "<<sin(theta)<<std::endl;
        pc_origin_.points[i].x = dist * cos(theta);
        pc_origin_.points[i].y = dist * sin(theta);
        pc_origin_.points[i].z = 0;
        
      }else{
        pc_origin_.points[i].x = 0;
        pc_origin_.points[i].y = 0;
        pc_origin_.points[i].z = 0;
      }
      // printf("curr x,y,z: %.3f, %.3f, %.3f\n", pc_origin_.points[i].x, pc_origin_.points[i].y, pc_origin_.points[i].z);
    }

    //printf("min_ang, max_ang, ang_inc: %.3f %.3f %.3f\n",scan_msg->angle_min, scan_msg->angle_max, scan_msg->angle_increment);
  }
  laserScanToLDP(scan_msg, curr_ldp_scan_, -M_PI_2, M_PI_2);
 
  //for cloud display
  pc_curr_.width = curr_ldp_scan_->nrays;
  pc_curr_.height = 1;
  pc_curr_.resize(curr_ldp_scan_->nrays);
  for (int i = 0; i < curr_ldp_scan_->nrays; i++)
  {
    // printf("curr i, valid[i]: %d, %d\n",i,curr_ldp_scan_->valid[i]);
    if (curr_ldp_scan_->valid[i])
    {
      dist = curr_ldp_scan_->readings[i];
      theta = curr_ldp_scan_->theta[i];
      pc_curr_.points[i].x = dist * cos(theta);
      pc_curr_.points[i].y = dist * sin(theta);
      pc_curr_.points[i].z = 0;
    }
    else
    {
      pc_curr_.points[i].x = 0;
      pc_curr_.points[i].y = 0;
      pc_curr_.points[i].z = 0;
    }
    // printf("curr x,y,z: %.3f, %.3f, %.3f\n", pc_curr_.points[i].x, pc_curr_.points[i].y, pc_curr_.points[i].z);
  }
  // tf::Transform t;
  // icp_process(pc_origin_, pc_curr_, t);
  processScan(curr_ldp_scan_, scan_msg->header.stamp);
  
}
void LaserScanMatcher::icp_process(const pcl::PointCloud<pcl::PointXYZ> frame_left, const pcl::PointCloud<pcl::PointXYZ> frame_right, tf::Transform transform)
{
  ros::WallTime start = ros::WallTime::now();
  LDP prev, curr;
  pointCloudToLDP(frame_left.makeShared(), prev, -M_PI, M_PI);
  pointCloudToLDP(frame_right.makeShared(), curr, -M_PI/2.0, M_PI/2.0);
  prev->odometry[0] = 0.0;
  prev->odometry[1] = 0.0;
  prev->odometry[2] = 0.0;

  prev->estimate[0] = 0.0;
  prev->estimate[1] = 0.0;
  prev->estimate[2] = 0.0;

  prev->true_pose[0] = 0.0;
  prev->true_pose[1] = 0.0;
  prev->true_pose[2] = 0.0;

  input_.laser_ref  = prev;
  input_.laser_sens = curr;

  input_.first_guess[0] = f2b_.getOrigin().getX();
  input_.first_guess[1] = f2b_.getOrigin().getY();
  input_.first_guess[2] = tf::getYaw(f2b_.getRotation());

  // If they are non-Null, free covariance gsl matrices to avoid leaking memory
  //清空GNU科学计算库协方差矩阵
  if (output_.cov_x_m)
  {
    gsl_matrix_free(output_.cov_x_m);
    output_.cov_x_m = 0;
  }
  if (output_.dx_dy1_m)
  {
    gsl_matrix_free(output_.dx_dy1_m);
    output_.dx_dy1_m = 0;
  }
  if (output_.dx_dy2_m)
  {
    gsl_matrix_free(output_.dx_dy2_m);
    output_.dx_dy2_m = 0;
  }

  ros::WallTime end0 = ros::WallTime::now();
  // ******************** csm ICP配准 ********************
  sm_icp(&input_, &output_);

  ros::WallTime end1 = ros::WallTime::now();
  double dur1 = (end1 - end0).toSec() * 1e3;
  ROS_INFO("Scan matcher total duration: %.2f ms", dur1);
  printf("icp_process transform x,y,theta: %.3f, %.3f, %.3f\n",output_.x[0],output_.x[1],output_.x[2]);
  transform.setOrigin(tf::Vector3(output_.x[0], output_.x[1], 0.0));
  tf::Quaternion q;
  q.setRPY(0.0, 0.0, output_.x[2]);
  transform.setRotation(q);
}
void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
{
  // // 发布tf模板
  // tf::TransformBroadcaster tf_broadcaster_;
  // tf::Transform transform;
  // transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0));
  // transform.setRotation( tf::Quaternion(0, 0, 0, 1));
  // tf::StampedTransform transform_msg (transform, ros::Time::now(), "map", "livox_frame");
  // tf_broadcaster_.sendTransform(transform_msg);

  ros::WallTime start = ros::WallTime::now();

  prev_ldp_scan_->odometry[0] = 0.0;
  prev_ldp_scan_->odometry[1] = 0.0;
  prev_ldp_scan_->odometry[2] = 0.0;

  prev_ldp_scan_->estimate[0] = 0.0;
  prev_ldp_scan_->estimate[1] = 0.0;
  prev_ldp_scan_->estimate[2] = 0.0;

  prev_ldp_scan_->true_pose[0] = 0.0;
  prev_ldp_scan_->true_pose[1] = 0.0;
  prev_ldp_scan_->true_pose[2] = 0.0;

  input_.laser_ref  = prev_ldp_scan_;
  input_.laser_sens = curr_ldp_scan;

  input_.first_guess[0] = f2b_.getOrigin().getX();
  input_.first_guess[1] = f2b_.getOrigin().getY();
  input_.first_guess[2] = tf::getYaw(f2b_.getRotation());

  // If they are non-Null, free covariance gsl matrices to avoid leaking memory
  //清空GNU科学计算库协方差矩阵
  if (output_.cov_x_m)
  {
    gsl_matrix_free(output_.cov_x_m);
    output_.cov_x_m = 0;
  }
  if (output_.dx_dy1_m)
  {
    gsl_matrix_free(output_.dx_dy1_m);
    output_.dx_dy1_m = 0;
  }
  if (output_.dx_dy2_m)
  {
    gsl_matrix_free(output_.dx_dy2_m);
    output_.dx_dy2_m = 0;
  }

  // *** scan match - using point to line icp from CSM， CSM is C(anonical) Scan Matcher
    // printf("first_guess: %.4f, %.4f, %.4f.\n",input_.first_guess[0],input_.first_guess[1],input_.first_guess[2]);
    // printf("max_ang, max_lin, max_iter: %.4f, %.4f %d\n",input_.max_angular_correction_deg, input_.max_linear_correction, input_.max_iterations);
    // printf("ep_xy, ep_the: %.4f, %.4f\n",input_.epsilon_xy, input_.epsilon_theta);
    // printf("max_corr, use_corr, restart, re_thre, re_dt, re_dth: %.4f, %d, %d, %.4f, %.4f %.4f\n",input_.max_correspondence_dist, input_.use_corr_tricks, input_.restart, input_.restart_threshold_mean_error, input_.restart_dt, input_.restart_dtheta);
    // printf("out_maxP, out_ada_or, out_ada_mu, out_rem_do: %.4f, %.4f, %.4f %d\n",input_.outliers_maxPerc, input_.outliers_adaptive_order, input_.outliers_adaptive_mult, input_.outliers_remove_doubles);
    // printf("clust_thre, orien_nei, do_al_te, do_al_te_thr: %.4f, %d, %d, %.4f\n",input_.clustering_threshold, input_.orientation_neighbourhood, input_.do_alpha_test, input_.do_alpha_test_thresholdDeg);
    // printf("do_visi, use_point, use_ml, use_sig, do_com_conv, debug_veri: %d, %d, %d, %d, %d, %d\n",input_.do_visibility_test, input_.use_point_to_line_distance, input_.use_ml_weights, input_.use_sigma_weights, input_.do_compute_covariance, input_.debug_verify_tricks);
    // printf("laser[3]: %.4f, %.4f, %.4f\n",input_.laser[0],input_.laser[1],input_.laser[2]);
    // printf("sigma, min_r, max_r: %.4f, %.4f, %.4f\n",input_.sigma, input_.min_reading, input_.max_reading);
    // printf("gpm_the, gpm_ex, gpm_int: %.4f, %.4f, %d\n",input_.gpm_theta_bin_size_deg, input_.gpm_extend_range_deg, input_.gpm_interval);
    // printf("--------------- hsm -------------\n");
    // printf("max_norm, lin_cel, ang_cel, num_ang, ang_hyp: %.4f, %.4f, %.4f, %d, %.4f\n", input_.hsm.max_norm, input_.hsm.linear_cell_size, input_.hsm.angular_cell_size_deg, input_.hsm.num_angular_hypotheses, input_.hsm.angular_hyp_min_distance_deg);
    // printf("xc_ndi, xc_dir_min, lin_xc_max, lin_xc_pea, max_trans, debug_true: %d, %.4f, %d, %.4f, %.4f, %d\n",input_.hsm.xc_ndirections,input_.hsm.xc_directions_min_distance_deg, input_.hsm.linear_xc_max_npeaks,input_.hsm.linear_xc_peaks_min_distance, input_.hsm.max_translation, input_.hsm.debug_true_x_valid);
    // printf("debug_true_x[3]: %.4f, %.4f, %.4f\n",input_.hsm.debug_true_x[0],input_.hsm.debug_true_x[1],input_.hsm.debug_true_x[2]);
  
  ros::WallTime end0 = ros::WallTime::now();
  // ******************** csm ICP配准 ********************
  sm_icp(&input_, &output_);

  ros::WallTime end1 = ros::WallTime::now();
  double dur1 = (end1 - end0).toSec() * 1e3;
  //ROS_INFO("Scan matcher total duration: %.2f ms", dur1);

  // // ******************** pcl ICP配准 ********************
	// pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  // pcl::PointCloud<pcl::PointXYZ>::Ptr ori_ptr = pc_origin_.makeShared();
  // pcl::PointCloud<pcl::PointXYZ>::Ptr cur_ptr = pc_curr_.makeShared();
	// icp.setInputSource(ori_ptr);
	// icp.setInputTarget(cur_ptr);
	// pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);
  // ros::WallTime end1_1 = ros::WallTime::now();
  // double dur1_1 = (end1_1 - end1).toSec() * 1e3;
	// icp.align(*Final);
  // ros::WallTime end1_2 = ros::WallTime::now();
  // double dur1_2 = (end1_2 - end1_1).toSec() * 1e3;
  // Eigen::Matrix4f pcl_trans= icp.getFinalTransformation();
	// //输出最终的变换矩阵（4x4）
	// printf("pcl transform x,y,theta: %.3f, %.3f, %.3f\n",pcl_trans(0,3),pcl_trans(1,3),acos(pcl_trans(0,0)));
  // // std::cout << pcl_trans << std::endl;
  // ros::WallTime end2 = ros::WallTime::now();
  // double dur2 = (end2 - end1).toSec() * 1e3;

  // ******************** NDT配准 ********************
  pcl::PointCloud<pcl::PointXYZ>::Ptr ori_ptr = pc_origin_.makeShared();
  pcl::PointCloud<pcl::PointXYZ>::Ptr cur_ptr = pc_curr_.makeShared();
  // // Filtering input scan to roughly 10% of original size to increase speed of registration.
  // pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (0.05, 0.05, 0.05);
  approximate_voxel_filter.setInputCloud (ori_ptr);
  approximate_voxel_filter.filter (*ori_ptr);

  // std::cout << "Filtered cloud contains " << filtered_cloud->size ()
  //           << " data points from room_scan2.pcd" << std::endl;

  // Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
  pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.00001);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (0.1);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (0.5);
  // Setting max number of registration iterations.
  ndt.setMaximumIterations (50);
  // Setting point cloud to be aligned.
  ndt.setInputSource (cur_ptr);
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (ori_ptr);

  int n = 3;
  ndt_omp->setInputTarget(ori_ptr);
  ndt_omp->setInputSource(cur_ptr);
  ndt_omp->setNumThreads(n);
  ndt_omp->setTransformationEpsilon(0.00001);
  ndt_omp->setStepSize(0.1);
  ndt_omp->setResolution(0.5);
  ndt_omp->setMaximumIterations(40);
  ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);

  // Set initial alignment estimate found using robot odometry.
  Eigen::AngleAxisf init_rotation (trans[2], Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (trans[0], trans[1], 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  ros::WallTime end1_1 = ros::WallTime::now();
  double dur1_1 = (end1_1 - end1).toSec() * 1e3;

  // Calculating required rigid transform to align the input cloud to the target cloud.
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt_omp->align (*output_cloud, init_guess);
  // std::cout << "Normal Distributions Transform has converged:" << ndt_omp->hasConverged ()
  //           << " score: " << ndt_omp->getFitnessScore () << std::endl;

  ros::WallTime end1_2 = ros::WallTime::now();
  double dur1_2 = (end1_2 - end1_1).toSec() * 1e3;

  Eigen::Matrix4f pcl_trans= ndt_omp->getFinalTransformation();
	//输出最终的变换矩阵（4x4）
	// printf("pcl transform x,y,theta: %.3f, %.3f, %.3f\n",pcl_trans(0,3),pcl_trans(1,3),atan2(-pcl_trans(0,1), pcl_trans(0,0)));
  // std::cout << pcl_trans << std::endl;
  ros::WallTime end2 = ros::WallTime::now();
  double dur2 = (end2 - end1).toSec() * 1e3;

  // // Saving transformed input cloud.
  // pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud)
  // pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  tf::Transform corr_ch;
  // int valid = output_.valid;
  int valid = 1;
  if (valid)
  {

    // the correction of the laser's position, in the laser frame
    tf::Transform corr_ch_l;
    createTfFromXYTheta(pcl_trans(0,3),pcl_trans(1,3), atan2(-pcl_trans(0,1), pcl_trans(0,0)), corr_ch_l);
    // trans[0] = pcl_trans(0,3);
    // trans[1] = pcl_trans(1,3);
    // trans[2] = atan2(-pcl_trans(0,1), pcl_trans(0,0));
    // createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
    trans[0] = output_.x[0];
    trans[1] = output_.x[1];
    trans[2] = output_.x[2];
    // printf("csm transform x,y,theta: %.3f, %.3f, %.3f\n",trans[0],trans[1],trans[2]);

    //theta [-pi,pi]
    //std::cout<<"theta: "<<trans[2]<<std::endl;
    // the correction of the base's position, in the base frame
    //corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
    corr_ch = laser_to_base_ * corr_ch_l * base_to_laser_;

    // update the pose in the world frame
    f2b_ = f2b_kf_ * corr_ch;

    if (publish_tf_)
    {
      tf::StampedTransform transform_msg (f2b_, time, fixed_frame_, base_frame_);
      tf_broadcaster_.sendTransform(transform_msg);

      //定义发布的消息
      sensor_msgs::PointCloud2 output_origin;
      //把点云转化为ros消息
      pcl::toROSMsg(pc_origin_, output_origin);
      output_origin.header.frame_id = fixed_frame_;
      origin_cloud_publisher_.publish(output_origin);

      //定义发布的消息
      sensor_msgs::PointCloud2 output_curr;
      // //点云旋转
      // Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); //单位矩阵
      // float theta = M_PI / 20;
      // //给矩阵赋值
      // transform_1(0, 0) = cos(theta);
      // transform_1(0, 1) = -sin(theta);
      // transform_1(1, 0) = sin(theta);
      // transform_1(1, 1) = cos(theta);
      // //沿x轴平移0.025
      // transform_1(0, 3) = 0.01;
      // printf("Method #1: using a Matrix4f\n");
      // std::cout << transform_1 << std::endl;
      pcl::PointCloud<pcl::PointXYZ> curr1;
      Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
      transform_1.translation() << trans[0], trans[1], 0.0;
      transform_1.rotate (Eigen::AngleAxisf (trans[2], Eigen::Vector3f::UnitZ()));
      pcl::transformPointCloud(pc_curr_, curr1, transform_1);
      //把点云转化为ros消息
      pcl::toROSMsg(curr1, output_curr);
      output_curr.header.frame_id = fixed_frame_;
      current_cloud_publisher_.publish(output_curr);
      // std::cout<<"----"<<std::endl;

      pcl::PointCloud<pcl::PointXYZ> curr2;
      sensor_msgs::PointCloud2 output_curr2;
      Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
      transform_2.translation() << pcl_trans(0,3),pcl_trans(1,3), 0.0;
      transform_2.rotate (Eigen::AngleAxisf (atan2(-pcl_trans(0,1), pcl_trans(0,0)), Eigen::Vector3f::UnitZ()));
      pcl::transformPointCloud(pc_curr_, curr2, transform_2);
      //把点云转化为ros消息
      pcl::toROSMsg(curr2, output_curr2);
      output_curr2.header.frame_id = fixed_frame_;
      current_pcl_cloud_publisher_.publish(output_curr2);
      // std::cout<<"****"<<std::endl;
    }
  }
  else
  {
    corr_ch.setIdentity();
    ROS_WARN("Error in scan matching");
  }

  // **** swap old and new

  // if (newKeyframeNeeded(corr_ch))
  // {
  //   // generate a keyframe
  //   ld_free(prev_ldp_scan_);
  //   prev_ldp_scan_ = curr_ldp_scan;
  //   f2b_kf_ = f2b_;
  //   std::cout<<"new key generated.............."<<std::endl;
  // }
  // else
  // {
    ld_free(curr_ldp_scan);
    // ld_free(ldp_prev);
  // }

  last_icp_time_ = time;

  // **** statistics
  ros::WallTime end3 = ros::WallTime::now();
  double dur3 = (end3 - start).toSec() * 1e3;
  // std::cout<<"time consumption(csm, p1 p2 pcl and total): "<<dur1<<", "/* <<dur1_1<<", "<<dur1_2<<", "*/<<dur2<<", "<<dur3<<std::endl;
}

bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d)
{
  if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_) return true;

  double x = d.getOrigin().getX();
  double y = d.getOrigin().getY();
  if (x*x + y*y > kf_dist_linear_sq_) return true;

  return false;
}

void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
                                            LDP& ldp, double angle_min, double angle_max)
{
  unsigned int n = scan_msg->ranges.size();
  int left=0,right=0;
  if(angle_min>=-M_PI-1e04 && angle_min<=M_PI*2+1e04 && angle_max>=-M_PI-1e04 && angle_max<=M_PI*2+1e04 && angle_min<=angle_max){
    if (angle_min >= scan_msg->angle_min && angle_max <= scan_msg->angle_max)
    {
      left = int((angle_min - scan_msg->angle_min) / scan_msg->angle_increment);
      right = int((angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
      n = right - left + 1;
    }else if((angle_min+M_PI*2) <= scan_msg->angle_max){
      left = int((angle_min+M_PI*2 - scan_msg->angle_min) / scan_msg->angle_increment);
      right = int((angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
      n = right + scan_msg->ranges.size()-left;
    }else{
      std::cout<<"angle limit"<<std::endl;
      ldp = ld_alloc_new(0);
      return;
    }
  }
  else
  {
    std::cout<<"angle not included."<<angle_min<<","<<angle_max<<std::endl;
    ldp = ld_alloc_new(0);
    return;
  }
  // if(count>n || count<0)
  //   return;
  ldp = ld_alloc_new(n);
  int index = 0;
  //printf("n, ang_lim: %d, %.3f\n",n,scan_msg->angle_min + n * scan_msg->angle_increment);
  for (unsigned int i = 0; i < n; i++)
  {
    if (left <= right)
    {
      index = left + i;
    }
    else if (left > right)
    {
      if (i <= right)
        index = i;
      else
        index = left + i - right;
    }
    else
    {
      return;
    }
    // calculate position in laser frame
    double r = scan_msg->ranges[index];
    if (r > scan_msg->range_min && r < scan_msg->range_max)
    {
      // fill in laser scan data
      ldp->valid[i] = 1;
      ldp->readings[i] = r;
    }
    else
    {
      ldp->valid[i] = 0;
      ldp->readings[i] = -1; // for invalid range -1
    }
    ldp->theta[i] = scan_msg->angle_min + index * scan_msg->angle_increment;
    ldp->cluster[i] = -1;
  }

  ldp->min_theta = ldp->theta[0];
  ldp->max_theta = ldp->theta[n - 1];

  ldp->odometry[0] = 0.0;
  ldp->odometry[1] = 0.0;
  ldp->odometry[2] = 0.0;

  ldp->true_pose[0] = 0.0;
  ldp->true_pose[1] = 0.0;
  ldp->true_pose[2] = 0.0;
}
void LaserScanMatcher::pointCloudToLDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud_msg,
                     LDP &ldp, double angle_min, double angle_max)
{
  pcl::PointCloud<pcl::PointXYZ> laserCloudIn = *point_cloud_msg;
  // pcl::fromROSMsg(*point_cloud_msg, laserCloudIn);
  // std::vector<int> indices;
  // pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
  double theta_min = atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
  unsigned int n = laserCloudIn.points.size();
  double theta_max = atan2(laserCloudIn.points[n-1].y, laserCloudIn.points[n-1].x);
  std::cout<<laserCloudIn.points[0].x<<", "<<laserCloudIn.points[0].y<<std::endl;
  std::cout<<laserCloudIn.points[n-1].x<<", "<<laserCloudIn.points[n-1].y<<std::endl;
  if (theta_max <= 1e-4)
    theta_max += M_PI * 2;
  double angle_increment = (theta_max - theta_min) / n;
  int left=0,right=0;

  if(angle_min>=-M_PI-1e04 && angle_min<=M_PI*2+1e04 && angle_max>=-M_PI-1e04 && angle_max<=M_PI*2+1e04 && angle_min<=angle_max){
    if (angle_min >= theta_min && angle_max <= theta_max)
    {
      left = int((angle_min - theta_min) / angle_increment);
      right = int((angle_max - theta_min) / angle_increment);
      n = right - left + 1;
    }else if((angle_min+M_PI*2) <= theta_max){
      left = int((angle_min+M_PI*2 - theta_min) / angle_increment);
      right = int((angle_max - theta_min) / angle_increment);
      n = right + n-left;
    }else{
      printf("a min max, theta min max: %.3f, %.3f, %.3f, %.3f",angle_min, angle_max, theta_min, theta_max);
      std::cout<<"cloud angle limit"<<std::endl;
      ldp = ld_alloc_new(0);
      return;
    }
  }
  else
  {
    std::cout<<"cloud angle not included."<<angle_min<<","<<angle_max<<std::endl;
    ldp = ld_alloc_new(0);
    return;
  }
  ldp = ld_alloc_new(n);
  int index = 0;
  //printf("n, ang_lim: %d, %.3f\n",n,scan_msg->angle_min + n * scan_msg->angle_increment);
  for (unsigned int i = 0; i < n; i++)
  {
    if (left <= right)
    {
      index = left + i;
    }
    else if (left > right)
    {
      if (i <= right)
        index = i;
      else
        index = left + i - right;
    }
    else
    {
      ldp = ld_alloc_new(0);
      return;
    }
    // calculate position in laser frame
    double r = sqrt(pow(laserCloudIn.points[index].x,2)+pow(laserCloudIn.points[index].y,2));
    // fill in laser scan data
    ldp->valid[i] = 1;
    ldp->readings[i] = r;
    ldp->theta[i] = theta_min + index * angle_increment;
    ldp->cluster[i] = -1;
  }

  ldp->min_theta = ldp->theta[0];
  ldp->max_theta = ldp->theta[n - 1];

  ldp->odometry[0] = 0.0;
  ldp->odometry[1] = 0.0;
  ldp->odometry[2] = 0.0;

  ldp->true_pose[0] = 0.0;
  ldp->true_pose[1] = 0.0;
  ldp->true_pose[2] = 0.0;
}
void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
  a_cos_.clear();
  a_sin_.clear();

  for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
  {
    double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
    a_cos_.push_back(cos(angle));
    a_sin_.push_back(sin(angle));
  }

  input_.min_reading = scan_msg->range_min;
  input_.max_reading = scan_msg->range_max;
}

bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id)
{
  ros::Time t = ros::Time::now();

  tf::StampedTransform base_to_laser_tf;
  try
  {
    tf_listener_.waitForTransform(
      base_frame_, frame_id, t, ros::Duration(1.0));
    tf_listener_.lookupTransform (
      base_frame_, frame_id, t, base_to_laser_tf);
  }
  catch (tf::TransformException ex)
  {
    ROS_WARN("Could not get initial transform from base to laser frame, %s", ex.what());
    return false;
  }
  base_to_laser_ = base_to_laser_tf;
  laser_to_base_ = base_to_laser_.inverse();

  return true;
}

void LaserScanMatcher::createTfFromXYTheta(
  double x, double y, double theta, tf::Transform& t)
{
  t.setOrigin(tf::Vector3(x, y, 0.0));
  tf::Quaternion q;
  q.setRPY(0.0, 0.0, theta);
  t.setRotation(q);
}

} // namespace scan_tools
