1、更新相机SDK；

2、更新默认配置；
3、添加定平代码；

.gitignore

@@ -10,7 +10,6 @@
 *.pch
 
 # Compiled Dynamic libraries
-*.so
 *.dylib
 *.dll
 
@@ -47,8 +46,6 @@
 
 # Shared objects (inc. Windows DLLs)
 *.dll
-*.so
-*.so.*
 *.dylib
 
 # Executables

detect/ceres_detect/detect_wheel_ceres.cpp

@@ -42,7 +42,7 @@ public:
       T theta_front = variable[3];
 
       //整车旋转
-      Eigen::Rotation2D<T> rotation(theta);
+      Eigen::Rotation2D<T> rotation(-theta);
       Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
       //左前偏移
@@ -70,8 +70,8 @@ public:
       //左前轮
       int left_front_num = m_left_front_cloud->size();
       for (int i = 0; i < m_left_front_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) + cx),
-                                           (T(m_left_front_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) - cx),
+                                           (T(m_left_front_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
         //旋转
@@ -83,8 +83,8 @@ public:
       //右前轮
       int right_front_num = m_right_front_cloud->size();
       for (int i = 0; i < m_right_front_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) + cx),
-                                           (T(m_right_front_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) - cx),
+                                           (T(m_right_front_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
         //旋转
@@ -98,8 +98,8 @@ public:
       int left_rear_num = m_left_rear_cloud->size();
 
       for (int i = 0; i < m_left_rear_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x) + cx),
-                                           (T(m_left_rear_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x) - cx),
+                                           (T(m_left_rear_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
         interpolator_l.Evaluate(tanslate_point[0]*T(1000.)+T(200.),&residual[2*(left_front_num + right_front_num + i)]);
@@ -110,8 +110,8 @@ public:
 
       //右后轮
       for (int i = 0; i < m_right_rear_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x) + cx),
-                                           (T(m_right_rear_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x) - cx),
+                                           (T(m_right_rear_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
 
@@ -155,7 +155,7 @@ public:
       T length = variable[2];
 
       //整车旋转
-      Eigen::Rotation2D<T> rotation(theta);
+      Eigen::Rotation2D<T> rotation(-theta);
       Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
       //左前偏移
@@ -177,7 +177,7 @@ public:
       for (int i = 0; i < m_left_front_cloud->size(); ++i) {
         const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x)),
                                            (T(m_left_front_cloud->points[i].y)));
-        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        const Eigen::Matrix<T, 2, 1> tp(T(0),-cy);
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_left_front;
         interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[i]);
@@ -189,7 +189,7 @@ public:
       for (int i = 0; i < m_right_front_cloud->size(); ++i) {
         const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) ),
                                            (T(m_right_front_cloud->points[i].y)));
-        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        const Eigen::Matrix<T, 2, 1> tp(T(0),-cy);
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_right_front;
 
@@ -202,7 +202,7 @@ public:
       for (int i = 0; i < m_left_rear_cloud->size(); ++i) {
         const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x)),
                                            (T(m_left_rear_cloud->points[i].y)));
-        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        const Eigen::Matrix<T, 2, 1> tp(T(0),-cy);
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point +tp- wheel_center_normal_left_rear;
         interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
@@ -215,7 +215,7 @@ public:
       for (int i = 0; i < m_right_rear_cloud->size(); ++i) {
         const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x)),
                                            (T(m_right_rear_cloud->points[i].y)));
-        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        const Eigen::Matrix<T, 2, 1> tp(T(0),-cy);
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point +tp- wheel_center_normal_right_rear;
 
@@ -270,7 +270,7 @@ public:
       T theta=T(theta_);
 
       //整车旋转
-      Eigen::Rotation2D<T> rotation(theta);
+      Eigen::Rotation2D<T> rotation(-theta);
       Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
       //左前偏移
@@ -298,8 +298,8 @@ public:
       //左前轮
       int left_front_num = m_left_front_cloud->size();
       for (int i = 0; i < m_left_front_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) + cx),
-                                           (T(m_left_front_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) -cx),
+                                           (T(m_left_front_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
         //旋转
@@ -313,8 +313,8 @@ public:
       //右前轮
       int right_front_num = m_right_front_cloud->size();
       for (int i = 0; i < m_right_front_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) + cx),
-                                           (T(m_right_front_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) - cx),
+                                           (T(m_right_front_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
         //旋转
@@ -329,8 +329,8 @@ public:
       int left_rear_num = m_left_rear_cloud->size();
 
       for (int i = 0; i < m_left_rear_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x) + cx),
-                                           (T(m_left_rear_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x) - cx),
+                                           (T(m_left_rear_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
         interpolator_l.Evaluate(tanslate_point[0]*T(1000.)+T(200.),&residual[2*(left_front_num + right_front_num + i)]);
@@ -342,8 +342,8 @@ public:
       //右后轮
 
       for (int i = 0; i < m_right_rear_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x) + cx),
-                                           (T(m_right_rear_cloud->points[i].y) + cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x) - cx),
+                                           (T(m_right_rear_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
 
@@ -576,4 +576,4 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr detect_wheel_ceres::filter(pcl::PointCloud<p
       out->push_back(cloud->points[i]);
   }
   return out;
-}
+}

detect/ceres_detect/detect_wheel_ceres.h

@@ -14,7 +14,14 @@
 
 class detect_wheel_ceres {
 public:
-    detect_wheel_ceres();
+    static detect_wheel_ceres *iter() {
+        static detect_wheel_ceres *instance = nullptr;
+        if (instance == nullptr) {
+            instance = new detect_wheel_ceres();
+        }
+        return instance;
+    }
+
     ~detect_wheel_ceres();
 
     bool detect_2Step(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
@@ -29,6 +36,7 @@ public:
                 float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
 
 private:
+    detect_wheel_ceres();
     bool detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                     float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);

detect/detect_manager.cpp

@@ -33,6 +33,8 @@ void DetectManager::run() {
     while (m_condit.is_alive()) {
         m_condit.wait();
         // 参数初始化
+        cost = std::chrono::steady_clock::now() - t_start_time;
+        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(99, cost.count() * 1000)));
         t_start_time = std::chrono::steady_clock::now();
 
         if (m_condit.is_alive()) {
@@ -100,7 +102,7 @@ void DetectManager::run() {
                 DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
 
                 bool ceres_detect_ret = false;
-                detect_wheel_ceres wheel_detect;
+                auto wheel_detect = detect_wheel_ceres::iter();
                 // 采用上次测量结果的数据
                 if (!m_detect_result_time_lock_data.timeout(0.5)) {
                     detect_result = m_detect_result_time_lock_data();
@@ -113,7 +115,7 @@ void DetectManager::run() {
                 // 根据车轮数采用不同的优化方式
                 if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
                     objs.size() > 2) {
-                    ceres_detect_ret = wheel_detect.detect_2Step(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
+                    ceres_detect_ret = wheel_detect->detect_2Step(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                     vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                     detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
                                     detect_result.car.theta, detect_result.car.wheel_base,
@@ -136,7 +138,7 @@ void DetectManager::run() {
                         }
                     }
                 } else {
-                    ceres_detect_ret = wheel_detect.detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
+                    ceres_detect_ret = wheel_detect->detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                          vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                          detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
                                          detect_result.car.theta, m_detect_result_record.car.wheel_base,
@@ -165,20 +167,8 @@ void DetectManager::run() {
             }
             cost = std::chrono::steady_clock::now() - t_start_time;
             DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
-
-            // 算法结果处理
-//            static float min_wheel_width = 1e8, max_wheel_width = -1e8, min_wheel_base = 1e8, max_wheel_base = -1e8;
-//            if (m_detect_result_record.car.wheel_width > 1.0) {
-//                min_wheel_width = std::min(min_wheel_width, m_detect_result_record.car.wheel_width);
-//                max_wheel_width = std::max(max_wheel_width, m_detect_result_record.car.wheel_width);
-//            }
-//            if (m_detect_result_record.car.wheel_base > 1.0) {
-//                min_wheel_base = std::min(min_wheel_base, m_detect_result_record.car.wheel_base);
-//                max_wheel_base = std::max(max_wheel_base, m_detect_result_record.car.wheel_base);
-//            }
-
-//            LOG(INFO) << "\n" << m_detect_result_record.info();
-            usleep(1000 * 100);
+        } else {
+            cv::destroyAllWindows();
         }
     }
     LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";

vzense/NebulaSDK/AArch64/include/VzenseNebula_types.h

@@ -4,6 +4,10 @@
 #include <stdint.h>
 #include "VzenseNebula_enums.h"
 
+#ifndef __cplusplus
+#include <stdbool.h>
+#endif
+
 typedef uint16_t VzDepthPixel;  //!< Depth image pixel type in 16-bit
 typedef uint16_t VzGray16Pixel; //!< Gray image pixel type in 16-bit
 typedef uint8_t VzGray8Pixel;   //!< Gray image pixel type in 8-bit
@@ -165,9 +169,6 @@ typedef struct
     int	exposureTime;              //When the control mode is AE,  exposureTime represents the maximum exposure time.
                                    //When the control mode is Manual, exposureTime represents the current exposure time.
 } VzExposureTimeParams;
-
-
-
 /**
  * @brief Error informations about the device
  */

vzense/NebulaSDK/x86_64/include/VzenseNebula_types.h

@@ -4,6 +4,10 @@
 #include <stdint.h>
 #include "VzenseNebula_enums.h"
 
+#ifndef __cplusplus
+#include <stdbool.h>
+#endif
+
 typedef uint16_t VzDepthPixel;  //!< Depth image pixel type in 16-bit
 typedef uint16_t VzGray16Pixel; //!< Gray image pixel type in 16-bit
 typedef uint8_t VzGray8Pixel;   //!< Gray image pixel type in 8-bit
@@ -165,9 +169,6 @@ typedef struct
     int	exposureTime;              //When the control mode is AE,  exposureTime represents the maximum exposure time.
                                    //When the control mode is Manual, exposureTime represents the current exposure time.
 } VzExposureTimeParams;
-
-
-
 /**
  * @brief Error informations about the device
  */

vzense/NebulaSDK/x86_64/lib/libNebula_api.so

@@ -0,0 +1 @@
+libNebula_api.so.1.2

vzense/NebulaSDK/x86_64/lib/libNebula_api.so.1.2

@@ -0,0 +1 @@
+libNebula_api.so.1.2.7

vzense/device_tof3d.cpp

@@ -483,10 +483,17 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
     cv::Mat t_ir_img;
     cv::Mat t_point_img;
 
+    auto t_start_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
+
     LOG(INFO) << device.etc->ip() << ": " << azimuth << " in thread " << std::this_thread::get_id();
     updateTrans(azimuth, device.etc->trans());
     while (device.condit->is_alive()) {
         device.condit->wait();
+        cost = std::chrono::steady_clock::now() - t_start_time;
+        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(99, cost.count() * 1000)));
+        t_start_time = std::chrono::steady_clock::now();
+
         if (device.condit->is_alive()) {
             Error_manager ret;
             device.handle_mutex->lock();
@@ -542,7 +549,6 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
             if (!ret.get_error_description().empty()) {
                 LOG(WARNING) << ret.get_error_description();
             }
-            std::this_thread::sleep_for(std::chrono::milliseconds(1));
         }
     }