icp map to aviod cross thread access.

velodyne_lidar/icp_svd_registration.cpp

@@ -79,7 +79,7 @@ bool ICPSVDRegistration::ScanMatch(
     pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);
 
     // init estimation:
-    transformation_ = Eigen::Matrix4f::Identity();
+    Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
     //
     // TODO: first option -- implement all computing logic on your own
     //
@@ -87,7 +87,7 @@ bool ICPSVDRegistration::ScanMatch(
     int curr_iter = 0;
     while (curr_iter < max_iter_) {
         // TODO: apply current estimation:
-        Eigen::Matrix4f t_current_pose = transformation_ * predict_pose;
+        Eigen::Matrix4f t_current_pose = transformation * predict_pose;
         pcl::transformPointCloud(*input_source_, *transformed_input_source, t_current_pose);
 
         // TODO: get correspondence:
@@ -120,31 +120,34 @@ bool ICPSVDRegistration::ScanMatch(
             // std::cout<<"correspond avg dist sq too large: " << t_avg_correspond_dist_sq <<std::endl;
             return false;
         }
-
+// std::cout<<"eeeee"<<std::endl;
         // TODO: whether the transformation update is significant:
         // break if convergent
         if (!IsSignificant(t_iter_transform, trans_eps_)) {
-            // transformation_ = t_iter_transform * transformation_;
+            // transformation = t_iter_transform * transformation;
             break;
         }
-
+// std::cout<<"fffff     \n"<<t_iter_transform<<"\n----------------\n"<<transformation<<std::endl;
         // TODO: update transformation:
-        transformation_ = t_iter_transform * transformation_;
+        Eigen::Matrix4f t_trans_mat = t_iter_transform * transformation;
+        // std::cout<<"ffffff"<<std::endl;
+        transformation = t_trans_mat;
+        // std::cout<<"fffffff"<<std::endl;
 
         ++curr_iter;
     }
-
+// std::cout<<"ggggg     \n"<<std::endl;
     if(curr_iter>=max_iter_)
     {
-        transformation_ = Eigen::Matrix4f::Identity();
+        transformation = Eigen::Matrix4f::Identity();
         return false;
     }
 
     // set output:
-    result_pose = transformation_ * predict_pose;
+    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<<"quat: "<<Eigen::Quaternionf(transformation.block<3,3>(0,0)).coeffs().transpose()<<std::endl;
     // std::cout << "iter count:" << curr_iter << std::endl;
     return true;
 }

velodyne_lidar/icp_svd_registration.hpp

@@ -63,7 +63,7 @@ class ICPSVDRegistration {
     pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr input_target_kdtree_;
     pcl::PointCloud<pcl::PointXYZ>::Ptr input_source_;
 
-    Eigen::Matrix4f transformation_;
+    // Eigen::Matrix4f transformation_;
 };
 
 #endif

velodyne_lidar/region_status_checker.h

@@ -66,7 +66,8 @@ public:
     void Region_status_checker_init()
     {
         mb_exit = false;
-        mp_icp_svd = new ICPSVDRegistration(0.1, 0.0018, 0.036, 20);
+        // mp_icp_svd = new ICPSVDRegistration(0.1, 0.0018, 0.036, 20);
+        // mp_icp_svd = new ICPSVDRegistration(0.1, 0.0002, 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);
@@ -87,11 +88,20 @@ public:
             delete m_manager_thread;
             m_manager_thread = nullptr;
         }
-        if(mp_icp_svd!=nullptr)
+        for (auto iter = m_icp_svd_map.begin(); iter != m_icp_svd_map.end(); iter++)
         {
-            delete mp_icp_svd;
-            mp_icp_svd = nullptr;
+            if(iter->second!= nullptr)
+            {
+                delete iter->second;
+                iter->second = nullptr;
+            }
         }
+        
+        // if(mp_icp_svd!=nullptr)
+        // {
+        //     delete mp_icp_svd;
+        //     mp_icp_svd = nullptr;
+        // }
     }
 
     // 更新测量数据回调
@@ -113,6 +123,7 @@ public:
             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));
+            m_icp_svd_map.insert(std::pair<int, ICPSVDRegistration*>(terminal_id, new ICPSVDRegistration(0.1, 0.0018, 0.036, 20)));
         }
         else//已创建
         {
@@ -130,10 +141,11 @@ public:
             return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("当前空点云 终端") + std::to_string(terminal_id)).c_str());
         }
 
-        if(mp_icp_svd == nullptr)
+        if(m_icp_svd_map.find(terminal_id) == m_icp_svd_map.end())
         {
             return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("当前icp空指针 终端") + std::to_string(terminal_id)).c_str());
         }
+        ICPSVDRegistration* tp_icp_svd = m_icp_svd_map[terminal_id];
 
         if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end() || m_measure_results_map[terminal_id].size() == 0)
         {
@@ -242,9 +254,8 @@ public:
         vox.setLeafSize(0.03f, 0.03f, 0.03f); //设置滤波时创建的体素体积为1cm的立方体
         vox.filter(*t_curr_cloud_inside_filtered);             //执行滤波处理，存储输出
 
-        mp_icp_svd->SetInputTarget(t_prev_cloud_inside_filtered);
+        tp_icp_svd->SetInputTarget(t_prev_cloud_inside_filtered);
         
-
         // static int count = 0;
         // if(count++ == 20)
         // {
@@ -256,12 +267,13 @@ public:
         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 = Eigen::Matrix4f::Identity();
-        if (!mp_icp_svd->ScanMatch(t_curr_cloud_inside_filtered, t_pred_pose, t_result_cloud, t_res_pose))
+        // std::cout << "222" << std::endl;
+        if (!tp_icp_svd->ScanMatch(t_curr_cloud_inside_filtered, 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());
         }
+        // std::cout << "2222" << std::endl;
 
-        // std::cout << "222" << std::endl;
         if(is_significant(t_res_pose, dtime / 1000.0))
         {
             mb_is_stable = false;
@@ -383,7 +395,8 @@ private:
     // double m_max_queue_size;
 
     // icp匹配
-    ICPSVDRegistration *mp_icp_svd;
+    // ICPSVDRegistration *mp_icp_svd;
+    std::map<int, ICPSVDRegistration*> m_icp_svd_map;
     // 是否已经稳定
     bool mb_is_stable;
     // 最初稳定时刻