Fri Jul 21 10:11:00 CST 2023 inscode

demo.cpp

+
+
+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