add car movement check func.

still crash need to solve.

message/measure_message.proto

@@ -143,7 +143,8 @@ message Ground_status_msg
     required Locate_information         locate_information_realtime = 6;//地面雷达的 实时定位信息
     required Ground_statu               ground_status=7; // 电子围栏状态
     // 超界状态,为0表示正常，从末尾开始123456位分别代表前、后、左、右、底盘、车高超界
-    // 增加超宽、超轴距、左(逆时针)旋转超界，右(顺时针)旋转超界，前轮角超界判断，分别处于7 8 9 10 11四个位
+    // 增加超宽、超轴距、左(逆时针)旋转超界，右(顺时针)旋转超界，前轮角超界判断，分别处于7 8 9 10 11
+    // 增加 车辆移动 12位 静止为0，运动为1
     required int32               	border_status=8; 
 
     required Error_manager              error_manager = 9;

tool/common_data.h

@@ -101,6 +101,20 @@ public:
 			this->car_front_theta /= scalar;
 			return *this;
 		}
+		Car_wheel_information& operator=(const Car_wheel_information& other)
+		{
+			this->car_center_x = other.car_center_x;
+			this->car_center_y = other.car_center_y;
+			this->car_angle = other.car_angle;
+			this->car_wheel_base = other.car_wheel_base;
+			this->car_wheel_width = other.car_wheel_width;
+			this->car_front_theta = other.car_front_theta;
+			this->correctness = other.correctness;
+			this->uniform_car_x = other.uniform_car_x;
+			this->uniform_car_y = other.uniform_car_y;
+			this->range_status = other.range_status;
+			return *this;
+		}
 		// 定义评分规则, 
 		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.5
 		float calc_score()
@@ -169,6 +183,12 @@ public:
 			this->wheel_data /= scalar;
 			return *this;
 		}
+		Car_wheel_information_stamped& operator=(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data = other.wheel_data;
+			this->measure_time = other.measure_time;
+			return *this;
+		}
 		// 定义评分规则, 
 		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.1
 		float calc_score()

velodyne_lidar/ground_region.cpp

@@ -13,6 +13,8 @@
 // 测量结果滤波,不影响现有结构
 #include "../tool/measure_filter.h"
 
+// 增加车辆停止状态判断
+#include "region_status_checker.h"
 
 //欧式聚类*******************************************************
 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
@@ -304,11 +306,14 @@ Ground_region::~Ground_region()
         delete m_detector;
         m_detector = nullptr;
     }
+
+    Region_status_checker::get_instance_references().Region_status_checker_uninit();
     // LOG(WARNING) << "detector released";
 }
 
 Error_manager Ground_region::init(velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model, pcl::PointCloud<pcl::PointXYZ>::Ptr right_model)
 {
+    Region_status_checker::get_instance_references().Region_status_checker_init();
     m_region = region;
     m_detector = new detect_wheel_ceres3d(left_model,right_model);
     mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
@@ -740,10 +745,13 @@ void Ground_region::thread_measure_func()
             detect_wheel_ceres3d::Detect_result t_result;
 
             Error_manager ec = detect(mp_cloud_collection, t_result);
-      
+
             std::lock_guard<std::mutex> lck(m_car_result_mutex);
+            m_detect_update_time = std::chrono::system_clock::now();
             // 增加滤波轴距
             Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
+            // 车辆移动检测
+            Common_data::Car_wheel_information_stamped t_wheel_info_stamped_for_car_move;
             if (ec == SUCCESS)
             {
                 m_car_wheel_information.correctness = true;
@@ -771,6 +779,27 @@ void Ground_region::thread_measure_func()
                 t_wheel_info_stamped.measure_time = m_detect_update_time;
                 Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);
 
+                // 20211228 added by yct, car movement checking, human and door detection
+                t_wheel_info_stamped_for_car_move = t_wheel_info_stamped;
+                t_wheel_info_stamped_for_car_move.wheel_data.car_center_x -= m_region.plc_offsetx();
+                t_wheel_info_stamped_for_car_move.wheel_data.car_center_y -= m_region.plc_offsety();
+                t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_x -= m_region.plc_offsetx();
+                t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_y -= m_region.plc_offsety();
+                t_wheel_info_stamped_for_car_move.wheel_data.car_angle -= m_region.plc_offset_degree();
+                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);
+                // success means car stable
+                if(car_status_res == SUCCESS)
+                {
+                    m_car_wheel_information.range_status &= ~((int)Range_status::Range_car_moving);
+                }else
+                {
+                    m_car_wheel_information.range_status |= Range_status::Range_car_moving;
+                    // if(m_region.region_id()==4){
+                    //     std::cout<<"success: "<<car_status_res.to_string()<<std::endl;
+                    // }
+                }
+                Region_status_checker::get_instance_references().add_measure_data(m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);
+
                 ec = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region.region_id(), t_wheel_info_stamped.wheel_data);
                 if (ec == SUCCESS)
                 {
@@ -791,8 +820,21 @@ void Ground_region::thread_measure_func()
             {
                 m_car_wheel_information.correctness = false;
                 // LOG_IF(ERROR, m_region.region_id() == 0) << ec.to_string();
+
+                // 20211228 added by yct, car movement checking, human and door detection
+                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);
+                // success means car stable
+                if(car_status_res == SUCCESS)
+                {
+                    m_car_wheel_information.range_status &= ~((int)Range_status::Range_car_moving);
+                }else
+                {
+                    m_car_wheel_information.range_status |= Range_status::Range_car_moving;
+                    // if(m_region.region_id()==4){
+                    //     std::cout<<"failed: "<<car_status_res.to_string()<<std::endl;
+                    // }
+                }
             }
-            m_detect_update_time = std::chrono::system_clock::now();
         }
         m_region_status = E_READY;
     }

velodyne_lidar/ground_region.h

@@ -59,6 +59,7 @@ public:
       Range_angle_anti_clock = 0x0100, // 左(逆时针)旋转超界
       Range_angle_clock = 0x0200, // 右(顺时针)旋转超界
       Range_steering_wheel_nozero = 0x0400, // 方向盘未回正
+      Range_car_moving = 0x0800, // 车辆移动为1，静止为0
    };
 
 #define GROUND_REGION_DETECT_CYCLE_MS 150

velodyne_lidar/icp_svd_registration.cpp

@@ -0,0 +1,254 @@
+/*
+ * @Description: ICP SVD lidar odometry
+ * @Author: Ge Yao
+ * @Date: 2020-10-24 21:46:45
+ */
+
+#include <pcl/common/transforms.h>
+
+#include <cmath>
+#include <vector>
+
+#include <Eigen/Dense>
+#include <Eigen/SVD>
+
+#include "glog/logging.h"
+
+#include "icp_svd_registration.hpp"
+
+ICPSVDRegistration::ICPSVDRegistration(
+    const YAML::Node& node
+) : input_target_kdtree_(new pcl::KdTreeFLANN<pcl::PointXYZ>()) {
+    // parse params:
+    float max_corr_dist = node["max_corr_dist"].as<float>();
+    float trans_eps = node["trans_eps"].as<float>();
+    float euc_fitness_eps = node["euc_fitness_eps"].as<float>();
+    int max_iter = node["max_iter"].as<int>();
+
+    SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter);
+}
+
+ICPSVDRegistration::ICPSVDRegistration(
+    float max_corr_dist, 
+    float trans_eps, 
+    float euc_fitness_eps, 
+    int max_iter
+) : input_target_kdtree_(new pcl::KdTreeFLANN<pcl::PointXYZ>()) {
+    SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter);
+}
+
+bool ICPSVDRegistration::SetRegistrationParam(
+    float max_corr_dist, 
+    float trans_eps, 
+    float euc_fitness_eps, 
+    int max_iter
+) {
+    // set params:
+    max_corr_dist_ = max_corr_dist;
+    trans_eps_ = trans_eps;
+    euc_fitness_eps_ = euc_fitness_eps;
+    max_iter_ = max_iter;
+
+    // LOG(INFO) << "ICP SVD params:" << std::endl
+    //           << "max_corr_dist: " << max_corr_dist_ << ", "
+    //           << "trans_eps: " << trans_eps_ << ", "
+    //           << "euc_fitness_eps: " << euc_fitness_eps_ << ", "
+    //           << "max_iter: " << max_iter_ 
+    //           << std::endl << std::endl;
+
+    return true;
+}
+
+bool ICPSVDRegistration::SetInputTarget(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_target) {
+    input_target_ = input_target;
+    input_target_kdtree_->setInputCloud(input_target_);
+
+    return true;
+}
+
+bool ICPSVDRegistration::ScanMatch(
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_source, 
+    const Eigen::Matrix4f& predict_pose, 
+    pcl::PointCloud<pcl::PointXYZ>::Ptr& result_cloud_ptr,
+    Eigen::Matrix4f& result_pose
+) {
+    input_source_ = input_source;
+
+    // pre-process input source:
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_input_source(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);
+
+    // init estimation:
+    transformation_.setIdentity();
+    
+    //
+    // TODO: first option -- implement all computing logic on your own
+    //
+    // do estimation:
+    int curr_iter = 0;
+    while (curr_iter < max_iter_) {
+        // TODO: apply current estimation:
+        Eigen::Matrix4f t_current_pose = transformation_ * predict_pose;
+        pcl::transformPointCloud(*input_source_, *transformed_input_source, t_current_pose);
+
+        // TODO: get correspondence:
+        std::vector<Eigen::Vector3f> xs, ys;
+        size_t t_correspondence = GetCorrespondence(transformed_input_source, xs, ys);
+
+        // TODO: do not have enough correspondence -- break:
+        if(t_correspondence < 4 || xs.size() != ys.size() || xs.size()<=0)
+        {
+            std::cout<<"icp_svd not enough correspondence "<<std::endl;
+            return false;
+        }
+
+        // TODO: update current transform:
+        Eigen::Matrix4f t_iter_transform;
+        GetTransform(xs, ys, t_iter_transform);
+
+        // calculate whether match fitness epsilon
+        float t_avg_correspond_dist_sq = 0.0f;
+        for (int i = 0; i < xs.size(); i++)
+        {
+            float t_correspond_dist_sq = pow(xs[i].x()-ys[i].x(), 2) + 
+                                            pow(xs[i].y()-ys[i].y(), 2) + 
+                                            pow(xs[i].z()-ys[i].z(), 2);
+            t_avg_correspond_dist_sq += t_correspond_dist_sq;
+        }
+        t_avg_correspond_dist_sq /= xs.size();
+        if(t_avg_correspond_dist_sq > euc_fitness_eps_)
+        {
+            std::cout<<"correspond avg dist sq too large: " << t_avg_correspond_dist_sq <<std::endl;
+            return false;
+        }
+
+        // TODO: whether the transformation update is significant:
+        // break if convergent
+        if (!IsSignificant(t_iter_transform, trans_eps_)) {
+            // transformation_ = t_iter_transform * transformation_;
+            break;
+        }
+
+        // TODO: update transformation:
+        transformation_ = t_iter_transform * transformation_;
+
+        ++curr_iter;
+    }
+
+    if(curr_iter>=max_iter_)
+    {
+        transformation_.setIdentity();
+        return false;
+    }
+
+    // set output:
+    result_pose = transformation_ * predict_pose;
+    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);
+
+    // std::cout<<"quat: "<<Eigen::Quaternionf(transformation_.block<3,3>(0,0)).coeffs().transpose()<<std::endl;
+    // std::cout << "iter count:" << curr_iter << std::endl;
+    return true;
+}
+
+size_t ICPSVDRegistration::GetCorrespondence(
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_source, 
+    std::vector<Eigen::Vector3f> &xs,
+    std::vector<Eigen::Vector3f> &ys
+) {
+    const float MAX_CORR_DIST_SQR = max_corr_dist_ * max_corr_dist_;
+
+    size_t num_corr = 0;
+
+    // TODO: set up point correspondence
+    xs.clear();
+    ys.clear();
+    std::vector<int> t_indices;
+    std::vector<float> t_sqr_dists;
+    for (int i = 0; i < input_source->size(); i++)
+    {
+        int neighbors = input_target_kdtree_->nearestKSearch(input_source->points[i], 1, t_indices, t_sqr_dists);
+        if(neighbors>0 && t_sqr_dists[0] < MAX_CORR_DIST_SQR)
+        {
+            xs.push_back(Eigen::Vector3f(input_target_->points[t_indices[0]].x,
+                                        input_target_->points[t_indices[0]].y,
+                                        input_target_->points[t_indices[0]].z));
+            ys.push_back(Eigen::Vector3f(input_source->points[i].x,
+                                        input_source->points[i].y,
+                                        input_source->points[i].z));
+            num_corr++;
+        }
+    }
+
+    return num_corr;
+}
+
+void ICPSVDRegistration::GetTransform(
+    const std::vector<Eigen::Vector3f> &xs,
+    const std::vector<Eigen::Vector3f> &ys,
+    Eigen::Matrix4f &transformation
+) {
+    const size_t N = xs.size();
+
+    // TODO: find centroids of mu_x and mu_y:
+    Eigen::Vector3f ux=Eigen::Vector3f::Zero(), uy=Eigen::Vector3f::Zero();
+    for (size_t i = 0; i < N; i++)
+    {
+        ux += xs[i];
+        uy += ys[i];
+    }
+    ux /= N;
+    uy /= N;
+
+    // TODO: build H:
+    // H = sum((yi-uy) * (xi-ux)_t)
+    Eigen::Matrix3f H = Eigen::Matrix3f::Zero();
+    for (size_t i = 0; i < N; i++)
+    {
+        H += (ys[i]-uy) * (xs[i]-ux).transpose();
+    }
+
+    // TODO: solve R:
+    // H = U * S * V_t
+    // R = V * U_t
+    Eigen::JacobiSVD<Eigen::MatrixXf> svd_h(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    Eigen::Matrix3f u = svd_h.matrixU();
+    Eigen::Matrix3f v = svd_h.matrixV();
+    if(u.determinant() * v.determinant() <0)
+    {
+        for (size_t i = 0; i < 3; i++)
+        {
+            v(i, 2) *= -1;
+        }
+    }
+    Eigen::Matrix3f R = v * u.transpose();
+    Eigen::Quaternionf t_quat(R);
+    t_quat.normalize();
+
+    // TODO: solve t:
+    // e2 = || ux - R * uy -t ||^2
+    // t = ux - R * uy
+    Eigen::Vector3f t = ux - t_quat.toRotationMatrix() * uy;
+
+    // TODO: set output:
+    transformation << t_quat.toRotationMatrix(), t, 0.0f, 0.0f, 0.0f, 1.0f;
+}
+
+bool ICPSVDRegistration::IsSignificant(
+    const Eigen::Matrix4f &transformation,
+    const float trans_eps
+) {
+    // a. translation magnitude -- norm:
+    float translation_magnitude = transformation.block<3, 1>(0, 3).norm();
+    // b. rotation magnitude -- angle:
+    float rotation_magnitude = fabs(
+        acos(
+            (std::min(transformation.block<3, 3>(0, 0).trace(), 2.999999f) - 1.0f) / 2.0f
+        )
+    );
+    // std::cout << "two mag: " << translation_magnitude << ", " << rotation_magnitude << std::endl;
+
+    return (
+        (translation_magnitude > trans_eps) || 
+        (rotation_magnitude > trans_eps)
+    );
+}

velodyne_lidar/icp_svd_registration.hpp

@@ -0,0 +1,69 @@
+/*
+ * @Description: ICP SVD lidar odometry
+ * @Author: Ge Yao
+ * @Date: 2020-10-24 21:46:57
+ */
+#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_SVD_REGISTRATION_HPP_
+#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_SVD_REGISTRATION_HPP_
+
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <pcl/kdtree/kdtree_flann.h>
+
+class ICPSVDRegistration {
+  public:
+    ICPSVDRegistration(const YAML::Node& node);
+    ICPSVDRegistration(
+      float max_corr_dist, 
+      float trans_eps, 
+      float euc_fitness_eps, 
+      int max_iter
+    );
+
+    bool SetInputTarget(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_target);
+    bool ScanMatch(
+      const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_source, 
+      const Eigen::Matrix4f& predict_pose, 
+      pcl::PointCloud<pcl::PointXYZ>::Ptr& result_cloud_ptr,
+      Eigen::Matrix4f& result_pose
+    );
+  
+  private:
+    bool SetRegistrationParam(
+      float max_corr_dist, 
+      float trans_eps, 
+      float euc_fitness_eps, 
+      int max_iter
+    );
+
+  private:
+    size_t GetCorrespondence(
+      const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_source, 
+      std::vector<Eigen::Vector3f> &xs,
+      std::vector<Eigen::Vector3f> &ys
+    );
+
+    void GetTransform(
+      const std::vector<Eigen::Vector3f> &xs,
+      const std::vector<Eigen::Vector3f> &ys,
+      Eigen::Matrix4f &transformation_
+    );
+
+    bool IsSignificant(
+      const Eigen::Matrix4f &transformation,
+      const float trans_eps
+    );
+
+    float max_corr_dist_;
+    float trans_eps_; 
+    float euc_fitness_eps_; 
+    int max_iter_;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_target_;
+    pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr input_target_kdtree_;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_source_;
+
+    Eigen::Matrix4f transformation_;
+};
+
+#endif

velodyne_lidar/region_status_checker.h

@@ -0,0 +1,382 @@
+/*
+ * @Description: 区域状态检查类，用于判断当前各区域车辆状态(已停好、运动中)，以及周围是否有障碍物
+ * @Author: yct
+ * @Date: 2021-12-21 19:54:34
+ * @LastEditTime: 2021-12-28 11:53:11
+ * @LastEditors: yct
+ */
+
+#ifndef REGION_STATUS_CHECKER_HH
+#define REGION_STATUS_CHECKER_HH
+#include "../tool/singleton.h"
+#include "../tool/common_data.h"
+#include "../error_code/error_code.h"
+#include <map>
+#include <mutex>
+#include <deque>
+#include <chrono>
+#include <vector>
+#include <algorithm>
+#include <thread>
+#include "glog/logging.h"
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+// #include <pcl/registration/ndt.h>
+#include <pcl/filters/voxel_grid.h>
+
+#include "icp_svd_registration.hpp"
+
+// #include "opencv2/opencv.hpp"
+#include "../tool/point_tool.h"
+
+#define DEFAULT_REGION_STATUS_TIMEOUT_MILLI 30000
+#define DEFAULT_REGION_STATUS_MAX_QUEUE_SIZE 600
+// 检测到静止后稳定时间
+#define MIN_STABLE_TIME_MILLI 1000
+// 外点差异量
+#define OUTSIDE_POINT_LIMIT 30
+
+class Region_status_checker : public Singleton<Region_status_checker>
+{
+    // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+    friend class Singleton<Region_status_checker>;
+
+struct Region_measure_info_stamped
+{
+    Common_data::Car_wheel_information_stamped measure_result;
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+};
+
+public:
+    // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Region_status_checker(const Region_status_checker &) = delete;
+    Region_status_checker &operator=(const Region_status_checker &) = delete;
+    ~Region_status_checker() = default;
+
+    void Region_status_checker_init()
+    {
+        mb_exit = false;
+        mp_icp_svd = new ICPSVDRegistration(0.1, 0.0018, 0.036, 20);
+        // mp_ndt = pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>);
+        // mp_ndt->setMaxCorrespondenceDistance(0.2);
+        // mp_ndt->setTransformationEpsilon(0.01);
+        // mp_ndt->setEuclideanFitnessEpsilon(0.09);
+        // mp_ndt->setMaximumIterations(10);
+        m_manager_thread = new std::thread(&Region_status_checker::thread_func, this);
+    }
+
+    void Region_status_checker_uninit()
+    {
+        mb_exit = true;
+        if(m_manager_thread != nullptr)
+        {
+            if(m_manager_thread->joinable())
+            {
+                m_manager_thread->join();
+            }
+            delete m_manager_thread;
+            m_manager_thread = nullptr;
+        }
+        if(mp_icp_svd!=nullptr)
+        {
+            delete mp_icp_svd;
+            mp_icp_svd = nullptr;
+        }
+    }
+
+    // 更新测量数据回调
+    void add_measure_data(int terminal_id, Common_data::Car_wheel_information_stamped& measure_result, pcl::PointCloud<pcl::PointXYZ>& cloud)
+    {
+        Region_measure_info_stamped data;
+        data.measure_result = measure_result;
+        std::vector<int> index;
+        // cloud.is_dense = false;
+        // pcl::removeNaNFromPointCloud(cloud, data.cloud, index);
+        data.cloud = cloud;
+        if (!data.measure_result.wheel_data.correctness)
+            return;
+//        LOG(INFO) << data.wheel_data.to_string();
+        // 未创建队列
+        std::lock_guard<std::mutex> lck(m_mutex);
+        if (m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
+        {
+            std::deque<Region_measure_info_stamped> t_deque;
+            t_deque.push_back(data);
+            m_measure_results_map.insert(std::pair<int, std::deque<Region_measure_info_stamped>>(terminal_id, t_deque));
+        }
+        else//已创建
+        {
+            m_measure_results_map[terminal_id].push_back(data);
+        }
+    }
+
+    // 根据当前帧判断区域停车状态
+    // 当前帧不需要存在正确结果，利用点云判断是否车辆未移动即可
+    Error_manager get_region_parking_status(int terminal_id, Common_data::Car_wheel_information_stamped& measure_result, pcl::PointCloud<pcl::PointXYZ>& cloud)
+    {
+        std::lock_guard<std::mutex> lck(m_mutex);
+        if(cloud.size() <= 0)
+        {
+            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("当前空点云 终端") + std::to_string(terminal_id)).c_str());
+        }
+
+        if(mp_icp_svd == nullptr)
+        {
+            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("当前icp空指针 终端") + std::to_string(terminal_id)).c_str());
+        }
+
+        if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
+        {
+            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("暂无最近历史数据用于推测当前状态 终端") + std::to_string(terminal_id)).c_str());
+        }
+
+        // 时间检查, 当前队列中数据全为正确
+        // 计算最近一帧外框，x一边外扩4cm, y一边外扩100cm
+        auto last_data_ref = m_measure_results_map[terminal_id].back();
+        if(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now()-last_data_ref.measure_result.measure_time).count() > DEFAULT_REGION_STATUS_TIMEOUT_MILLI)
+        {
+            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("最近历史数据超时 终端") + std::to_string(terminal_id)).c_str());
+        }
+
+        // 截取当前帧框内与框外点云
+        Eigen::Vector2d t_last_center(
+            last_data_ref.measure_result.wheel_data.car_center_x,
+            last_data_ref.measure_result.wheel_data.car_center_y);
+        Eigen::Vector2d t_width_height(
+            last_data_ref.measure_result.wheel_data.car_wheel_width + 0.08,
+            last_data_ref.measure_result.wheel_data.car_wheel_base + 2.0);
+
+        // 历史正确数据整车旋转
+        Eigen::Rotation2Dd t_rot((90 - last_data_ref.measure_result.wheel_data.car_angle) * M_PI / 180.0);
+        Eigen::Matrix2d t_rot_mat = t_rot.toRotationMatrix();
+
+        // 计算历史(模板)框内点云与框外点云
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_inside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_outside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        for (size_t i = 0; i < last_data_ref.cloud.size(); i++)
+        {
+            Eigen::Vector2d t_point(last_data_ref.cloud[i].x - t_last_center.x(), last_data_ref.cloud[i].y - t_last_center.y());
+            Eigen::Vector2d t_rot_point = t_rot_mat * t_point;
+            if (t_rot_point.x() > -t_width_height.x() / 2.0 && t_rot_point.x() < t_width_height.x() / 2.0 && t_rot_point.y() > -t_width_height.y() / 2.0 && t_rot_point.y() < t_width_height.y() / 2.0)
+            {
+                t_prev_cloud_inside->push_back(last_data_ref.cloud[i]);
+            }
+            else
+            {
+                t_prev_cloud_outside->push_back(last_data_ref.cloud[i]);
+            }
+
+            // if(std::isfinite(last_data_ref.cloud[i].x) && std::isfinite(last_data_ref.cloud[i].y) && std::isfinite(last_data_ref.cloud[i].z))
+            // {
+            // }else
+            // {
+            //     std::cout << "!!!!!!!!!!!!!!!!!!!!!!!! prev nan point" << std::endl;
+
+            // }
+        }
+
+        // 计算当前帧在框内与框外点云
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_inside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_outside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        std::vector<int> index;
+        pcl::PointCloud<pcl::PointXYZ> t_cloud_valid;
+        t_cloud_valid = cloud;
+        // t_cloud_valid.is_dense = false;
+        // pcl::removeNaNFromPointCloud(t_cloud_valid, t_cloud_valid, index);
+        for (size_t i = 0; i < t_cloud_valid.size(); i++)
+        {
+            Eigen::Vector2d t_point(t_cloud_valid[i].x - t_last_center.x(), t_cloud_valid[i].y - t_last_center.y());
+            Eigen::Vector2d t_rot_point = t_rot_mat * t_point;
+            if (t_rot_point.x() > -t_width_height.x() / 2.0 && t_rot_point.x() < t_width_height.x() / 2.0 && t_rot_point.y() > -t_width_height.y() / 2.0 && t_rot_point.y() < t_width_height.y() / 2.0)
+            {
+                t_curr_cloud_inside->push_back(t_cloud_valid[i]);
+            }
+            else
+            {
+                t_curr_cloud_outside->push_back(t_cloud_valid[i]);
+            }
+            // if(std::isfinite(t_cloud_valid[i].x) && std::isfinite(t_cloud_valid[i].y) && std::isfinite(t_cloud_valid[i].z))
+            // {
+            // }else{
+            //     std::cout << "!!!!!!!!!!!!!!!!!!!!!!!! curr nan point" << std::endl;
+
+            // }
+        }
+        if(t_prev_cloud_inside->size() <= 30 || t_curr_cloud_inside->size() <= 30)
+        {
+            char buf[255];
+            sprintf(buf, ",, prev %d, curr %d;; last center(%.3f, %.3f)", t_prev_cloud_inside->size(), t_curr_cloud_inside->size(), t_last_center.x(), t_last_center.y());
+            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("区域剪切后无点可用于匹配 终端") + std::to_string(terminal_id)+std::string(buf)).c_str());
+        }
+        // std::cout << "000" << std::endl;
+
+        // 直接使用当前时间，测量时间传入存在变为0的情况!!!!!
+        measure_result.measure_time = std::chrono::system_clock::now();
+        double dtime = std::chrono::duration_cast<std::chrono::milliseconds>(measure_result.measure_time - last_data_ref.measure_result.measure_time).count();
+        // double curr_time = std::chrono::duration_cast<std::chrono::milliseconds>(measure_result.measure_time.time_since_epoch()).count();
+        // double last_time = std::chrono::duration_cast<std::chrono::milliseconds>(last_data_ref.measure_result.measure_time.time_since_epoch()).count();
+        // std::cout << "dtime " << dtime / 1000.0 << ",, curr, last: " << curr_time << ", " << last_time << ", diff: " << curr_time - last_time << std::endl;
+        
+        // icp计算前后内点匹配情况
+        //下采样
+        pcl::VoxelGrid<pcl::PointXYZ> vox;    //创建滤波对象
+        vox.setInputCloud(t_prev_cloud_inside);         //设置需要过滤的点云给滤波对象
+        vox.setLeafSize(0.03f, 0.03f, 0.03f); //设置滤波时创建的体素体积为1cm的立方体
+        vox.filter(*t_prev_cloud_inside);             //执行滤波处理，存储输出
+
+        vox.setInputCloud(t_curr_cloud_inside);         //设置需要过滤的点云给滤波对象
+        vox.setLeafSize(0.03f, 0.03f, 0.03f); //设置滤波时创建的体素体积为1cm的立方体
+        vox.filter(*t_curr_cloud_inside);             //执行滤波处理，存储输出
+
+        mp_icp_svd->SetInputTarget(t_prev_cloud_inside);
+        
+
+        // static int count = 0;
+        // if(count++ == 20)
+        // {
+        //     save_cloud_txt(t_prev_cloud_inside, "./t_prev_cloud.txt");
+        //     save_cloud_txt(t_curr_cloud_inside, "./t_curr_cloud.txt");
+        // }
+
+        // std::cout << "111" << std::endl;
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_result_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        Eigen::Matrix4f t_pred_pose, t_res_pose;
+        t_pred_pose.setIdentity();
+        if (!mp_icp_svd->ScanMatch(t_curr_cloud_inside, t_pred_pose, t_result_cloud, t_res_pose))
+        {
+            return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("icp匹配失败，终端") + std::to_string(terminal_id)).c_str());
+        }
+
+        if(is_significant(t_res_pose, dtime / 1000.0))
+        {
+            mb_is_stable = false;
+            return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("检测到车辆移动，终端") + std::to_string(terminal_id)).c_str());
+        }else{
+            if (!mb_is_stable)
+            {
+                mb_is_stable = true;
+                m_stable_time = std::chrono::system_clock::now();
+                return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("开始等待稳定，终端") + std::to_string(terminal_id)).c_str());
+            }else
+            {
+                auto current_time = std::chrono::system_clock::now();
+                double duration = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - m_stable_time).count();
+                if(duration > MIN_STABLE_TIME_MILLI)
+                {
+                    // 判断外点数量
+                    // LOG_IF(WARNING, terminal_id == 4) << "ouside point prev curr: " << t_prev_cloud_outside->size() << ", " << t_curr_cloud_outside->size();
+                    if (t_curr_cloud_outside->size() > (t_prev_cloud_outside->size() + OUTSIDE_POINT_LIMIT))
+                    {
+                        // debug提示用
+                        // LOG_IF(WARNING, terminal_id == 4) << "检测到车身外侧噪点存在！！！";
+                    }
+                    return SUCCESS;
+                }else
+                {
+                    return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("等待稳定中，终端") + std::to_string(terminal_id)).c_str());
+                }
+            }
+        }
+        // std::cout << "333" << std::endl;
+
+        return ERROR;
+    }
+
+    // 显著性判断，平移x方向速度0.02m/s,y方向速度0.02m/s误差，角度3deg/s误差
+    bool is_significant(Eigen::Matrix4f trans, double dtime, double vx_eps = 0.02, double vy_eps = 0.02, double omega_eps = 0.051)
+    {
+        // 间隔时间为负数，或间隔超过0.5s则手动赋值
+        if(dtime<0 || dtime>0.5)
+        {
+            dtime = 0.1;
+        }
+            // return false;
+        Eigen::Vector3f translation = trans.block<3, 1>(0, 3);
+        float rot_angle = fabs(acos((std::min(trans.block<3, 3>(0, 0).trace(), 2.999999f) - 1.0f) / 2.0f));
+        // 值足够小，可当做车辆未移动
+        Eigen::Vector3d velocity(fabs(translation.x() / dtime), fabs(translation.y() / dtime), fabs(rot_angle / dtime));
+
+        if (velocity.x() <= vx_eps && velocity.y() <= vy_eps && velocity.z() <= omega_eps)
+        {
+            return false;
+        }
+        // char buf[512] = {0};
+        // sprintf(buf, "显著性分析, 速度 %.3f %.3f %.3f", velocity.x(), velocity.y(), velocity.z());
+        // LOG(INFO) << buf << ", dtime: " << dtime;
+        // // LOG(WARNING) << "???????????????????????????????????显著？？？？？？";
+        // if (velocity.x() > vx_eps)
+        // {
+        //     LOG(WARNING) << "x:" << velocity.x();
+        // }
+        // if(velocity.y() > vy_eps)
+        // {
+        //     LOG(WARNING) << "y:" << velocity.y();
+        // }
+        // if(velocity.z() > omega_eps)
+        // {
+        //     LOG(WARNING) << "z:" << velocity.z();
+        // }
+        return true;
+    }
+
+    void thread_func()
+    {
+        while(!mb_exit)
+        {
+            {
+                std::lock_guard<std::mutex> lck(m_mutex);
+                for (auto iter = m_measure_results_map.begin(); iter != m_measure_results_map.end(); iter++)
+                {
+                    std::deque<Region_measure_info_stamped> *tp_data_queue = &(iter->second);
+                    for (size_t i = 0; i < tp_data_queue->size(); i++)
+                    {
+                        // 判断队列头部(最老历史数据)是否超时，超时则丢出
+                        double dtime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - tp_data_queue->front().measure_result.measure_time).count();
+                        if(dtime > DEFAULT_REGION_STATUS_TIMEOUT_MILLI)
+                        {
+                            tp_data_queue->pop_front();
+                        }
+                    }
+                    // 维持队列长度
+                    while (tp_data_queue->size() > DEFAULT_REGION_STATUS_MAX_QUEUE_SIZE)
+                    {
+                        tp_data_queue->pop_front();
+                    }
+                }
+            }
+            usleep(1000 * 100);
+        }
+    }
+
+private:
+    // 父类的构造函数必须保护，子类的构造函数必须私有。
+    Region_status_checker() = default;
+
+    // map访问锁
+    std::mutex m_mutex;
+    // 各终端测量数据队列
+    std::map<int, std::deque<Region_measure_info_stamped> > m_measure_results_map;
+
+    bool mb_exit;
+    // 内部管理线程
+    std::thread *m_manager_thread;
+
+    // // 配置参数
+    // // 合法时间长度，即过去多久时间内数据有效
+    // double m_valid_time_milli;
+    // // 队列极限长度
+    // double m_max_queue_size;
+
+    // icp匹配
+    ICPSVDRegistration *mp_icp_svd;
+    // 是否已经稳定
+    bool mb_is_stable;
+    // 最初稳定时刻
+    std::chrono::system_clock::time_point m_stable_time;
+
+    // // 点云高度截断，保留底层点云匹配
+    // double height_limit;
+};
+
+#endif // !REGION_STATUS_CHECKER_HH