Как создать облака точек Centroid в ROS для выполнения кластеризации из набора точек, полученных из ROS MSG? - PullRequest
Новая
       48

Как создать облака точек Centroid в ROS для выполнения кластеризации из набора точек, полученных из ROS MSG?

0 голосов
/ 14 апреля 2020

Я создал узел, который подписывается на ROS msg (на msg в ограничивающем поле darknet_ros) и получаю координаты точки окна. Мне нравится использовать эти 8 точек, чтобы создать Centroid и выполнить кластеризацию с алгоритмом роста региона, чтобы получить ориентацию объекта (кватерниона). Вот мой узел 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: 

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-сообщение, которое публикуется (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, а затем сегментации и затем получить ориентация объекта? Спасибо

...