From bc5fbd32084179a073b3f47913536d82bf9269d3 Mon Sep 17 00:00:00 2001
From: 6440e7113916fa54ba548fe6 <6440e7113916fa54ba548fe6@devide>
Date: Fri, 21 Jul 2023 10:11:00 +0800
Subject: [PATCH] Fri Jul 21 10:11:00 CST 2023 inscode

---
 demo.cpp | 40 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)
 create mode 100644 demo.cpp

diff --git a/demo.cpp b/demo.cpp
new file mode 100644
index 0000000..d0ff0b5
--- /dev/null
+++ b/demo.cpp
@@ -0,0 +1,40 @@
+
+
+std::vector<Box> getBoxes(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr &msg_boxes) { std::vector<Box> result;
+
+#pragma omp parallel for
+// 遍历消息中的所有bounding box
+for (int i = 0; i < msg_boxes->boxes.size(); ++i)
+{
+    const auto& bbox = msg_boxes->boxes[i];
+
+    // 判断是否创建
+    if (bbox.pose.position.x < POINT_RANGE[0] || bbox.pose.position.x > POINT_RANGE[3] )
+        continue;
+    if (bbox.pose.position.y < POINT_RANGE[1] || bbox.pose.position.y > POINT_RANGE[4] )
+        continue;
+
+    // 得到中心点
+    float center[3] = {(float)bbox.pose.position.x, (float)bbox.pose.position.y, (float)bbox.pose.position.z};
+
+    // 得到尺寸
+    float wlh[3]    = {(float)bbox.dimensions.x, (float)bbox.dimensions.y, (float)bbox.dimensions.z};
+
+    // 计算yaw
+    Eigen::Quaternionf bboxQuaternion(bbox.pose.orientation.w, bbox.pose.orientation.x, bbox.pose.orientation.y, bbox.pose.orientation.z);
+    Eigen::Matrix3f rotMat = bboxQuaternion.toRotationMatrix();
+    float yaw_tem = atan2(rotMat(1, 0), rotMat(0, 0));
+
+    // 创建实例
+    Box box(center, wlh, yaw_tem);
+
+    // 进行互斥锁保护，避免多个线程同时对result进行访问
+    #pragma omp critical
+    {
+        // 将box入栈
+        result.push_back(box);
+    }
+}
+return result;
+
+}
\ No newline at end of file
-- 
GitLab