I have create a node that subscribe to ROS msg (to darknet_ros bounding box msg) and receive a the coordinate points of the box. I like to use this 8 points to create a Centroid and perform clustering with region growing algorithm in order to get the orientation of the object(quaternion). Here is my ROS node to get the Box coordinate
#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;
}
Here the ROS msg definition:
std_msgs/Header header
BoundingBox3d[] bounding_boxes
And the BoundingBox3d has the following content:
string Class
float64 probability
float64 xmin
float64 ymin
float64 xmax
float64 ymax
float64 zmin
float64 zmax
And the ROS msg that is published(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
So how to feed this xmin, xmax, ymin, ymax, zmin, zmax in the point cloud to create Centroid and then segmentation and then get the orientation of the object? Thanks