add some interface to get cloud with rich info.

add a calib tool in test folder to find lidar rotation.
use degree rather than radian  in setting files.

CMakeLists.txt

@@ -134,3 +134,9 @@ ${PCL_LIBRARIES}
 ${CERES_LIBRARIES}
 ${YAML_CPP_LIBRARIES}
 )
+
+add_executable(lidar_calib_tool ./tests/lidar_calib_tools.cpp ./tool/point_tool.cpp)
+target_link_libraries(lidar_calib_tool
+  ${PCL_LIBRARIES}
+  ${OpenCV_LIBS}
+)

tests/lidar_calib_tools.cpp

@@ -0,0 +1,167 @@
+/*
+ * @Description: lidar calib tools
+ * @Author: yct
+ * @Date: 2021-09-02 11:02:00
+ * @LastEditTime: 2021-09-02 17:31:42
+ * @LastEditors: yct
+ */
+
+#include "../tool/point_tool.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+/**
+ * @description: used to find vlp16 intrinsic
+ * @param  {*}
+ * @return {*}
+ * @param {Quaterniond} ground_quat  quaternion to align lidar coords with ground plane
+ * @param {double} angle_z_axis  rotation angle along z axis to align lidar coords with PLC coords
+ */
+void find_intrinsic(Eigen::Vector3d ground_norm, Eigen::Vector3d y_axis)
+{
+    Eigen::Quaterniond ground_quat = Eigen::Quaterniond::FromTwoVectors(ground_norm, Eigen::Vector3d::UnitZ());
+    Eigen::Quaterniond xy_plane_quat = Eigen::Quaterniond::FromTwoVectors(y_axis, Eigen::Vector3d::UnitY());
+    // std::cout << "y euler: " << xy_plane_quat.toRotationMatrix().eulerAngles(2, 1, 0).transpose() << std::endl;
+    Eigen::Matrix3d combined_rotation = ground_quat.toRotationMatrix() * xy_plane_quat.toRotationMatrix();
+    Eigen::Vector3d euler_angles = combined_rotation.eulerAngles(2, 1, 0);
+    for (size_t i = 0; i < euler_angles.rows(); i++)
+    {
+        if(fabs(euler_angles[i]) > M_PI_2)
+        {
+            if(euler_angles[i] > 0)
+            {
+                euler_angles[i] = euler_angles[i] - M_PI;
+            }else{
+                euler_angles[i] = M_PI + euler_angles[i];
+            }
+        }
+    }
+    std::cout << "lidar intrinsic y p r: " << euler_angles.transpose()*180.0/M_PI << std::endl;
+}
+
+/**
+ * @description: fit ground norm from two poles, then find y axis direction from front to back
+ * @return {*}
+ * @param {Ptr} front_barrier
+ * @param {Ptr} back_barrier
+ */
+void find_rotation_from_points(pcl::PointCloud<pcl::PointXYZ>::Ptr front_barrier, pcl::PointCloud<pcl::PointXYZ>::Ptr back_barrier,
+                                Eigen::Vector3d &front_direction, Eigen::Vector3d &back_direction, Eigen::Vector3d &y_direction)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr direction_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+
+    Eigen::Vector3d point_front, point_back;
+    // 根据点云拟合直线
+    //创建一个模型参数对象，用于记录拟合结果
+    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
+	//inliers通过点云序号来记录模型内点
+	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
+	//创建一个分割对象
+	pcl::SACSegmentation<pcl::PointXYZ> seg;
+	//Optional,设置结果直线展示的点是分割掉的点还是分割剩下的点
+	seg.setOptimizeCoefficients(true);//flase 展示的是分割剩下的点
+	//Mandatory-设置目标几何形状
+	seg.setModelType(pcl::SACMODEL_LINE);
+	//分割方法：随机采样法
+	seg.setMethodType(pcl::SAC_RANSAC);
+	//设置误差容忍范围，也就是阈值
+	seg.setDistanceThreshold(0.01);
+	//输入点云
+	seg.setInputCloud(front_barrier);
+	//分割点云
+	seg.segment(*inliers, *coefficients);
+
+    if(coefficients->values.size() < 6)
+    {
+        std::cout << "fatal, front line coeff size not enough." << std::endl;
+        return;
+    }
+    else
+    {
+        point_front << coefficients->values[0], coefficients->values[1], coefficients->values[2];
+        front_direction << coefficients->values[3], coefficients->values[4], coefficients->values[5];
+        std::cout << "front direction: " << front_direction.transpose() << std::endl;
+    }
+
+    // 保存前杆方向点云，调试用
+    for (size_t i = 0; i < 200; i++)
+    {
+        Eigen::Vector3d t_point = point_front + front_direction * 0.01 * i;
+        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
+    }
+
+    seg.setInputCloud(back_barrier);
+    seg.segment(*inliers, *coefficients);
+    if(coefficients->values.size() < 6)
+    {
+        std::cout << "fatal, back line coeff size not enough." << std::endl;
+        return;
+    }
+    else
+    {
+        point_back << coefficients->values[0], coefficients->values[1], coefficients->values[2];
+        back_direction << coefficients->values[3], coefficients->values[4], coefficients->values[5];
+        std::cout << "back direction: " << back_direction.transpose() << std::endl;
+    }
+
+    // 保存后杆方向点云，调试用
+    for (size_t i = 0; i < 200; i++)
+    {
+        Eigen::Vector3d t_point = point_back + back_direction * 0.01 * i;
+        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
+    }
+
+    std::cout << "point front to back : " << point_front.transpose() << " -----> " << point_back.transpose() << std::endl;
+
+    // 注意y方向需要清除z分量
+    y_direction = point_front - point_back;
+    y_direction.z() = 0.0;
+    std::cout << "y direction: " << y_direction.transpose() << std::endl;
+
+    // 保存y方向点云，调试用
+    for (size_t i = 0; i < 200; i++)
+    {
+        Eigen::Vector3d t_point = point_back + y_direction * 0.01 * i;
+        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
+    }
+    save_cloud_txt(direction_cloud, "./direction_cloud.txt");
+}
+
+int main()
+{
+    // cloud 0 calib
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_front, t_back;
+    t_front = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    t_front->push_back(pcl::PointXYZ(3.158999919891,-0.365000009537,0.851999998093));
+    t_front->push_back(pcl::PointXYZ(3.158999919891,-0.363999992609,0.734000027180));
+    t_front->push_back(pcl::PointXYZ(3.154000043869,-0.363000005484,0.616999983788));
+    t_front->push_back(pcl::PointXYZ(3.154000043869,-0.361999988556,0.503000020981));
+    t_front->push_back(pcl::PointXYZ(3.155999898911,-0.361000001431,0.389999985695));
+    t_front->push_back(pcl::PointXYZ(3.140000104904,-0.368000000715,0.165999993682));
+    t_front->push_back(pcl::PointXYZ(3.148000001907,-0.368000000715,0.054999999702));
+    t_front->push_back(pcl::PointXYZ(3.142999887466,-0.363000005484,-0.054999999702));
+    t_back = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+
+    Eigen::Vector3d front_direction, back_direction, y_direction;
+
+    if(!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/back_pole.txt", t_back))
+    {
+        std::cout << "read failed." << std::endl;
+        return -1;
+    }
+
+    // front direction: 0.0191082  0.002411  0.999815
+    // back direction: 0.0190939 0.0143106  0.999715
+    // 000.00802904 -0.000469358 00000.999968 0000.0897851
+    find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);
+    // 经过测试，地面法向量不适宜使用杆子获取，还是使用地面环点云拟合获得
+    // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
+    find_intrinsic(Eigen::Vector3d(0.00802904, -0.000469358, 0.999968), y_direction);
+
+
+    // cloud 1
+    // 000.0104679 -0.00822843 0000.999911 000.0986272
+    find_intrinsic(Eigen::Vector3d(0.0104679, -0.00822843, 0.999911), Eigen::Vector3d::UnitY());
+
+    return 0;
+}

velodyne_lidar/ground_region.h

@@ -13,6 +13,7 @@
 #include "match3d/detect_wheel_ceres3d.h"
 #include "velodyne_config.pb.h"
 #include "../message/measure_message.pb.h"
+#include "../verify/Verify_result.h"
 
 #include <opencv2/opencv.hpp>
 #include <glog/logging.h>
@@ -23,16 +24,23 @@
 class Ground_region
 {
 public:
+   enum Region_cloud_type
+   {
+      total = 0,
+      left_front = 1,
+      left_back = 2,
+      right_front = 3,
+      right_back = 4,
+   };
+   enum Ground_region_status
+   {
+      E_UNKNOWN = 0, //未知
+      E_READY = 1,   //准备，待机
+      E_BUSY = 2,    //工作正忙
 
-	enum Ground_region_status
-	{
-		E_UNKNOWN = 0, //未知
-		E_READY = 1,   //准备，待机
-		E_BUSY = 2,	   //工作正忙
-
-		E_FAULT = 10, //故障
-	};
-   #define GROUND_REGION_DETECT_CYCLE_MS 150
+      E_FAULT = 10, //故障
+   };
+#define GROUND_REGION_DETECT_CYCLE_MS 150
 
    Ground_region();
    ~Ground_region();
@@ -49,6 +57,31 @@ public:
 	Error_manager get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
 											 std::chrono::system_clock::time_point command_time);
 
+   void get_region_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud, Region_cloud_type type)
+   {
+      std::lock_guard<std::mutex> lck(m_cloud_collection_mutex);
+      out_cloud->clear();
+      if(m_detector==nullptr)
+         return;
+      switch (type)
+      {
+      case Region_cloud_type::total:
+         out_cloud->operator+=(*mp_cloud_collection);
+         break;
+      case Region_cloud_type::left_front:
+         out_cloud->operator+=(m_detector->m_left_front_cloud);
+         break;
+      case Region_cloud_type::left_back:
+         out_cloud->operator+=(m_detector->m_left_rear_cloud);
+         break;
+      case Region_cloud_type::right_front:
+         out_cloud->operator+=(m_detector->m_right_front_cloud);
+         break;
+      case Region_cloud_type::right_back:
+         out_cloud->operator+=(m_detector->m_right_rear_cloud);
+         break;
+      }
+   }
    // 获取区域终端号, 注意！未初始化调用将返回-1
    int get_terminal_id()
    {

velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp

@@ -85,9 +85,9 @@ Error_manager Velodyne_lidar_device::init(velodyne::velodyneLidarParams params)
 	m_lidar_id = params.lidar_id();
 
 	// 设置雷达内参矩阵
-	Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(params.calib().r(), Eigen::Vector3d::UnitX());
-	Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(params.calib().p(), Eigen::Vector3d::UnitY());
-	Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(params.calib().y(), Eigen::Vector3d::UnitZ());
+	Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(params.calib().r()*M_PI/180.0, Eigen::Vector3d::UnitX());
+	Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(params.calib().p()*M_PI/180.0, Eigen::Vector3d::UnitY());
+	Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(params.calib().y()*M_PI/180.0, Eigen::Vector3d::UnitZ());
 	Eigen::Vector3d trans(params.calib().cx(), params.calib().cy(), params.calib().cz());
 	m_calib_matrix << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() * rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
 
@@ -300,9 +300,9 @@ Error_manager Velodyne_lidar_device::end_task()
 }
 
 //取消任务单，由发送方提前取消任务单
-Error_manager Velodyne_lidar_device::cancel_task(Task_Base *p_wanji_lidar_scan_task)
+Error_manager Velodyne_lidar_device::cancel_task(Task_Base *p_velodyne_lidar_scan_task)
 {
-	if (p_wanji_lidar_scan_task == mp_velodyne_lidar_scan_task)
+	if (p_velodyne_lidar_scan_task == mp_velodyne_lidar_scan_task)
 	{
 		m_execute_condition.notify_all(false);
 
@@ -331,6 +331,43 @@ bool Velodyne_lidar_device::is_ready()
 	return m_velodyne_lidar_device_status == E_READY;
 }
 
+Error_manager Velodyne_lidar_device::get_new_ring_cloud_and_wait(std::mutex *p_mutex, std::vector<Lidar_point> &cloud_vec,
+																 std::chrono::steady_clock::time_point command_time, int timeout_ms)
+{
+	if(p_mutex == nullptr)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	//判断是否超时
+	while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
+	{
+		//获取指令时间之后的点云, 如果没有就会循环
+		if (m_parse_ring_time > command_time)
+		{
+			std::unique_lock<std::mutex> lck(*p_mutex);
+			{
+				cloud_vec.clear();
+				std::lock_guard<std::mutex> lck(m_cloud_mutex);
+				if (m_ring_cloud.size() <= 0)
+					return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
+				for (size_t i = 0; i < m_ring_cloud.size(); i++)
+				{
+					cloud_vec.assign(m_ring_cloud.begin(), m_ring_cloud.end());
+				}
+			}
+			return SUCCESS;
+		}
+		//else 等待下一次数据更新
+
+		//等1ms
+		std::this_thread::sleep_for(std::chrono::milliseconds(1));
+	}
+	//超时退出就报错
+	return Error_manager(Error_code::VELODYNE_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
+						 " Velodyne_lidar_device::get_new_cloud_and_wait error ");
+}
+
 //外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
 Error_manager Velodyne_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
 															std::chrono::steady_clock::time_point command_time, int timeout_ms)
@@ -364,7 +401,7 @@ Error_manager Velodyne_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex,
 		std::this_thread::sleep_for(std::chrono::milliseconds(1));
 	}
 	//超时退出就报错
-	return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
+	return Error_manager(Error_code::VELODYNE_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
 						 " Velodyne_lidar_device::get_new_cloud_and_wait error ");
 }
 

velodyne_lidar/velodyne_driver/velodyne_lidar_device.h

@@ -55,6 +55,10 @@ public:
 
 	int get_lidar_id() { return m_lidar_id; }
 
+	// 外部调用获取最新带环信息点云，通常用于标定
+	Error_manager get_new_ring_cloud_and_wait(std::mutex* p_mutex, std::vector<Lidar_point> &cloud_vec,
+							std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);
+
 	//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
 	Error_manager get_new_cloud_and_wait(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
 							std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);

velodyne_lidar/velodyne_manager.h

@@ -4,7 +4,7 @@
  * 分布式模式下，程序各自独立，分别只实例化一个设定的区域，以及相应雷达
  * @Author: yct
  * @Date: 2021-07-23 16:37:33
- * @LastEditTime: 2021-08-31 17:34:39
+ * @LastEditTime: 2021-09-02 17:27:03
  * @LastEditors: yct
  */
 
@@ -64,9 +64,9 @@ struct Timestamped_cloud
 		// 更新外参矩阵
 		void updateMatrix()
 		{
-			Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(m_extrinsic.r(), Eigen::Vector3d::UnitX());
-			Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(m_extrinsic.p(), Eigen::Vector3d::UnitY());
-			Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(m_extrinsic.y(), Eigen::Vector3d::UnitZ());
+			Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(m_extrinsic.r()*M_PI/180.0, Eigen::Vector3d::UnitX());
+			Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(m_extrinsic.p()*M_PI/180.0, Eigen::Vector3d::UnitY());
+			Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(m_extrinsic.y()*M_PI/180.0, Eigen::Vector3d::UnitZ());
 			Eigen::Vector3d trans(m_extrinsic.cx(), m_extrinsic.cy(), m_extrinsic.cz());
 			m_extrin_matrix << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() * rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
 		}