0

我创建了一个订阅 ROS msg 的节点（到 darknet_ros 边界框 msg）并接收框的坐标点。我喜欢使用这 8 个点来创建质心并使用区域增长算法执行聚类，以获得对象（四元数）的方向。这是我的 ROS 节点，用于获取 Box 坐标

#include "ros/ros.h"
#include "darknet_ros_3d_msgs/BoundingBoxes3d.h"
#include "darknet_ros_3d_msgs/BoundingBox3d.h"
#include "darknet_ros_msgs/BoundingBoxes.h"
#include "darknet_ros_msgs/BoundingBox.h"
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float64MultiArray.h>
#include "geometric_shapes/shape_operations.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <stdlib.h>
#include <iostream>
#include <string>
using namespace std;

void chatterCallback(const darknet_ros_3d_msgs::BoundingBoxes3d::ConstPtr& msg)
    { 
            if (!msg->bounding_boxes.empty())
                     {
                        int inc = 1;
                    if(msg->bounding_boxes[0].Class == "box")
                        {
                            float *array_xmin = new float[inc];
                            float *array_xmax = new float[inc];
                            float *array_ymin = new float[inc];
                            float *array_ymax = new float[inc];
                            float *array_zmin = new float[inc];
                            float *array_zmax = new float[inc];

                            array_xmin[inc] = msg->bounding_boxes[0].xmin; 
                            array_xmax[inc] = msg->bounding_boxes[0].xmax; 
                            array_ymin[inc] = msg->bounding_boxes[0].ymin;
                            array_ymax[inc] = msg->bounding_boxes[0].ymax; 
                            array_zmin[inc] = msg->bounding_boxes[0].zmin; 
                            array_zmax[inc] = msg->bounding_boxes[0].zmax;  

                            inc++;
                            delete[] array_xmin;
                            delete[] array_xmax;
                            delete[] array_ymin;
                            delete[] array_ymax;
                            delete[] array_zmin;
                            delete[] array_zmax;                

                       }            

                    }
    }

int main(int argc,  char** argv)
{
  ros::init(argc, argv, "cpp_subscriber");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/darknet_ros_3d/bounding_boxes", 20, chatterCallback);
  ros::spin();  
  return 0;
}

这里是 ROS msg 定义：

std_msgs/Header header
BoundingBox3d[] bounding_boxes

而 BoundingBox3d 的内容如下：

string Class
float64 probability
float64 xmin
float64 ymin
float64 xmax
float64 ymax
float64 zmin
float64 zmax

以及发布的 ROS msg(boundig_boxes)

header: 
  seq: 211
  stamp: 
    secs: 1586856134
    nsecs: 753424600
  frame_id: "zed_left_camera_frame"
bounding_boxes: 
  - 
    Class: "box"
    probability: 0.355243891478
    xmin: 0.635941922665
    ymin: -0.46644961834
    xmax: 0.735007822514
    ymax: -0.11608736217
    zmin: -0.150878965855
    zmax: 0.156425699592

那么如何在点云中输入这个xmin，xmax，ymin，ymax，zmin，zmax来创建Centroid，然后分割，然后得到对象的方向呢？谢谢

4

0 回答 0