wb init value, Matrix4f fault, model update

velodyne_lidar/icp_svd_registration.cpp

@@ -79,8 +79,7 @@ bool ICPSVDRegistration::ScanMatch(
     pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);
 
     // init estimation:
-    transformation_.setIdentity();
-    
+    transformation_ = Eigen::Matrix4f::Identity();
     //
     // TODO: first option -- implement all computing logic on your own
     //
@@ -137,7 +136,7 @@ bool ICPSVDRegistration::ScanMatch(
 
     if(curr_iter>=max_iter_)
     {
-        transformation_.setIdentity();
+        transformation_ = Eigen::Matrix4f::Identity();
         return false;
     }
 

velodyne_lidar/icp_svd_registration.hpp

@@ -46,7 +46,7 @@ class ICPSVDRegistration {
     void GetTransform(
       const std::vector<Eigen::Vector3f> &xs,
       const std::vector<Eigen::Vector3f> &ys,
-      Eigen::Matrix4f &transformation_
+      Eigen::Matrix4f &transformation
     );
 
     bool IsSignificant(

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -417,7 +417,7 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
     // detect_result.cx = center_x;
     detect_result.cx = detect_result.cx;
     detect_result.cy = (centroid_front.y()+centroid_rear.y())/2.0f;
-    detect_result.wheel_base=std::max(len1,len2);
+    detect_result.wheel_base = centroid_front.y() - centroid_rear.y() + 0.12; //std::max(len1,len2);
     // 已计算次数，初始传入值正常，之后solve函数会修改初始值，需要恢复角度单位为弧度
     int calc_count = 0;
     // double final_theta = 0;