for()
{
	// 残差对优化变量 R t 的雅可比矩阵
	Eigen::Matrix<float, 3, 6> Jacobian = Eigen::Matrix<float, 3, 6>::Zero();
	// 构建雅克比矩阵
	Jacobian.leftCols(3) = Eigen::Matrix3f::Identity();
	// 右扰动模型
	Jacobian.rightCols(3) = -T.block<3, 3>(0, 0) * Hat(origin_point_eigen);
	// 构建海森矩阵
	Hessian += Jacobian.transpose() * Jacobian;
	B += -Jacobian.transpose() * error;
}

// 可以改用QR分解：Eigen::Matrix<float, 6, 1> delta_x = Hessian.householderQr().solve(B);
Eigen::Matrix<float, 6, 1> delta_x = Hessian.inverse() * B;

T.block<3, 1>(0, 3) = T.block<3, 1>(0, 3) + delta_x.head(3);
// 由于使用的是右扰动模型求解，因此更新旋转时也应该右乘迭代量
T.block<3, 3>(0, 0) *= SO3Exp(delta_x.tail(3)).matrix();

class Parent
{
public:
  virtual std::string getName1(int x)
  {
    return "Parent";
  }

  virtual std::string getName2(int x)
  {
    return "Parent";
  }
};

class Child : public Parent
{
public:
  virtual std::string getName1(double x) override
  {
    return "Sub";
  }
  virtual std::string getName2(int x) const override
  {
    return "Sub";
  }
};

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/transforms.h>
#include <pcl/console/time.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>      //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>   //滤波类头文件  （使用体素网格过滤器处理的效果比较好）
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <boost/thread/thread.hpp>

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> PointCloud;
typedef pcl::PointCloud<PointType>::Ptr PointCloudPtr;
typedef pcl::PointCloud<PointType>::ConstPtr PointCloudConstPtr;

int main (int argc, char** argv)
{
    ros::init(argc, argv, "test_ndt");
    ros::NodeHandle nh;

    PointCloudPtr target_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/user/test_nodes/src/test_pcl/room_scan1.pcd", *target_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << target_cloud->size() << " points" << endl; 

    PointCloudPtr input_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/user/test_nodes/src/test_pcl/room_scan2.pcd", *input_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << input_cloud->size() << " points" << endl;

    PointCloudPtr filtered_cloud(new PointCloud);
    pcl::VoxelGrid<PointType> filter;
    filter.setLeafSize(0.1, 0.1, 0.1);
    filter.setInputCloud(input_cloud);
    filter.filter(*filtered_cloud);
    cout << "after filtered, input cloud has "<< filtered_cloud->size() << " points" << endl;
    
    pcl::NormalDistributionsTransform<PointType, PointType>::Ptr ndt_ptr(new pcl::NormalDistributionsTransform<PointType, PointType>() );
    ndt_ptr->setMaximumIterations(35);
    ndt_ptr->setTransformationEpsilon(0.01);
    ndt_ptr->setStepSize(0.1);
    ndt_ptr->setResolution(0.1);
    ndt_ptr->setInputSource(filtered_cloud);
    ndt_ptr->setInputTarget(target_cloud);

    Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

    PointCloudPtr output_cloud(new PointCloud);
    ndt_ptr->align(*output_cloud, init_guess);
    cout << "output cloud points size: "<< output_cloud->size() << endl;
    cout << "ndt transform: "<< endl << ndt_ptr->getFinalTransformation() << endl;
    cout << "score: "<< ndt_ptr->getFitnessScore() << endl;
    
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt_ptr->getFinalTransformation() );
    pcl::io::savePCDFileASCII("/home/user/test_nodes/src/test_pcl/final.pcd", *output_cloud );

    // 初始化点云可视化对象
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
    viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色

    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
    target_color (target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "target cloud");

    // 对转换后的源点云着色 (green)可视化.
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
    output_color(output_cloud, 0, 0, 255);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");
    // 启动可视化
    viewer_final->addCoordinateSystem (1.0);  //显示XYZ指示轴
    viewer_final->initCameraParameters ();   //初始化摄像头参数

    // 等待直到可视化窗口关闭
    while (!viewer_final->wasStopped ())
    {
      viewer_final->spinOnce (101);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

#include <GeographicLib/LocalCartesian.hpp>

GeographicLib::LocalCartesian  geo_converter;
double longitude = 0.0;
double latitude = 0.0;
double altitude = 0.0;
double local_E, local_N, local_U;

GeographicLib::LocalCartesian geo_converter;
// 这步获得了当前经纬高的值，初始化原点
geo_converter.Reset(latitude, longitude, altitude);
// 这里的意思是相对于原点，移动的经纬高的数值
latitude = 1.0;
longitude = 1.0;
altitude = 1.0;
// 将经纬高转换成东北天坐标[E,N,U]，前三个传入，后三个传出
geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
std::cout << "local_E: "<< local_E<< "  local_N: "<<local_N << "  local_U: "<< local_U << std::endl<<std::endl;

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

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

const auto trajectory_node_poses =
    map_builder_->pose_graph()->GetTrajectoryNodePoses();
const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
const auto constraints = map_builder_->pose_graph()->constraints();
for (const auto& constraint : constraints)
{
    const auto submap_it = submap_poses.find(constraint.submap_id);
    if (submap_it == submap_poses.end()) {
      continue;
    }
    const auto& submap_pose = submap_it->data.pose;
    const auto node_it = trajectory_node_poses.find(constraint.node_id);
    if (node_it == trajectory_node_poses.end()) {
      continue;
    }
    const auto& trajectory_node_pose = node_it->data.global_pose;
    const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;

    // Inter constraints, different trajectories  线段的两个端点
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(submap_pose.translation()));
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    // Inter residuals, different trajectories  线段的两个端点
    residual_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    residual_marker->points.push_back(
        ToGeometryMsgPoint(trajectory_node_pose.translation()));