combination

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include "svnet.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <opencv2/core/core.hpp>
#include <unistd.h>
#include <sys/time.h>

#define NUM_PROCESS_ITEM 10
using namespace std;
using namespace cv;

static const string OPENCV_WINDOW = "window";

Mat cameraMatrix(3, 3, DataType<double>::type);
Mat distCoeffs(4, 1, DataType<double>::type);
Mat rvec(3, 1, DataType<double>::type);
Mat tvec(3, 1, DataType<double>::type);
Mat image, drawImg, undistort_img;
vector<Point2f> save2Dpoint;
double x, y;


vector<Point2f> get2Dpoint();
Mat display_image(Mat image);
void param_init();

class cloud
{
  ros::NodeHandle nh;
  ros::Subscriber pcl_sub;
  ros::Publisher pcl_pub;
  int cloud_count = 0;

public:
  cloud()
 {
    pcl_sub = nh.subscribe("Sensor/points", 10, &cloud::cloudCB, this);
    pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_filtered", 1);
 }

  ~cloud()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }
  
  void cloudCB(const sensor_msgs::PointCloud2& input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);
	
	std::vector<cv::Point3f> points2;
	double x, y, z;
	for (size_t i = 0; i < cloud.points.size(); ++i) {

		x = cloud.points[i].x;
		y = cloud.points[i].y;
		z = cloud.points[i].z;
		points2.push_back(cv::Point3f(x, y, z));
	}

	vector<Point2f> projectedPoints2;
	projectPoints(points2, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints2);
	save2Dpoint = projectedPoints2;
	
        cout << "cloud num: "<< cloud_count << endl;
	cloud_count++;	

        pcl::toROSMsg(cloud_filtered, output);
    	output.header.frame_id = "point_cloud";
        pcl_pub.publish(output);
    }	
};


class fusion
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  int img_count = 0;
  
  int loop_count = 0;
  int cloud_count = 0;

public:
  fusion()
    : it_(nh_)
  {
    image_sub_ = it_.subscribe("/undistort_image", 1, &fusion::imageCb, this);
    image_pub_ = it_.advertise("/edge_detector/raw_image", 1);
    cv::namedWindow(OPENCV_WINDOW);
  }

  ~fusion()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {

    cv_bridge::CvImagePtr cv_ptr;
    namespace enc = sensor_msgs::image_encodings;

    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    // Draw an example circle on the video stream
    if (cv_ptr->image.rows > 400 && cv_ptr->image.cols > 600){

        

        if(save2Dpoint.size()>1){
	for (int i = 0; i < save2Dpoint.size(); i++) {
		x = save2Dpoint[i].x;
		y = save2Dpoint[i].y;
		circle(cv_ptr->image, Point(x, y), 1, Scalar(0, 0, 255), -1);
	}


        cv_ptr->image = test_image(cv_ptr->image);
	imshow(OPENCV_WINDOW, cv_ptr->image);	
	waitKey(1);

	cout << "image num: " << img_count << endl;
	img_count++;

 	image_pub_.publish(cv_ptr->toImageMsg());
	}
     }
  }

  Mat test_image(Mat img)
  {
    
    cv::Mat image;
    img.copyTo(image);

    SVNET_PROCESS_ITEM* item = NULL;
    SVNET_ERROR_T error = SVNET_SUCCESS;

    if ((error = SVNET_init(NUM_PROCESS_ITEM, image.cols, image.rows)) != SVNET_SUCCESS) {
        fprintf(stderr, "SVNet_init returned %d\n", error);
        exit(-1);
    }
    if ((item = SVNET_getAvailableProcessItem()) == NULL) {
        fprintf(stderr, "SVNET_getAvailableProcessItem() returned NULL\n");
        SVNET_release();
        exit(-1);
    }
    for (int i = 0; i < image.rows; i++) {
        memcpy(item->bgrData + i*image.cols*3, image.data + i*image.step.p[0], image.step.p[0]);
    }
    if ((error = SVNET_processImage(item)) != SVNET_SUCCESS) {
        fprintf(stderr, "SVNET_processImage returned %d\n", error);
        SVNET_release();
        exit(-1);
    }

    printf("# of Detected objects: %d\n", item->nDetectedObjects);
    for (int j = 0; j < item->nDetectedObjects; j++) {
        SVNET_OBJECT* object = &item->objects[j];
        printf(" object[%d]: (class: %d), (rect: (%f, %f, %f, %f)), (score: %f), (trackingId: %d)\n",
            j, object->type, object->x1, object->y1, object->x2, object->y2, object->score, object->trackingId);

        cv::Scalar color;
        switch (object->type) {
            case 0: color = cv::Scalar(0, 255, 0); break;
            case 1: color = cv::Scalar(0, 255, 255); break;
            case 2: color = cv::Scalar(0, 0, 255); break;
            case 3: color = cv::Scalar(0, 0, 200); break;
            case 4: color = cv::Scalar(0, 0, 170); break;
            case 5: color = cv::Scalar(0, 0, 150); break;
            default: color = cv::Scalar(255 % object->type, 255 % (object->type + object->type / 2), 255 % (object->type + object->type / 3)); break;
        }
        cv::rectangle(image, cv::Rect(object->x1, object->y1, object->x2 - object->x1 + 1, object->y2 - object->y1 + 1), color, 2);
    }
    printf("Latency: %d ms\n", (int)(item->elapsedTime*1000 + 0.5f));

    //cv::imshow("SVNet", image);
    //cvWaitKey(1);
    
    return image;
    SVNET_returnProcessItem(item);
  }
};



main(int argc, char** argv)
{
    param_init();
    ros::init(argc, argv, "pcl_and_opencv");
    ROS_INFO("Started two Node");
    fusion ic;
    cloud jc;

    ros::spin();

    return 0;
}

void param_init() {
	cameraMatrix.at<double>(0, 0) = 1173.861610;   // fx
	cameraMatrix.at<double>(0, 1) = 0;
	cameraMatrix.at<double>(0, 2) = 665.778565;    // cx

	cameraMatrix.at<double>(1, 0) = 0;
	cameraMatrix.at<double>(1, 1) = 1171.727336;    // fy
	cameraMatrix.at<double>(1, 2) = 352.757616;     // cy

	cameraMatrix.at<double>(2, 0) = 0;
	cameraMatrix.at<double>(2, 1) = 0;
	cameraMatrix.at<double>(2, 2) = 1;

	distCoeffs.at<double>(0) = -0.281792;
	distCoeffs.at<double>(1) = 0.115447;
	distCoeffs.at<double>(2) = 0.000875;
	distCoeffs.at<double>(3) = -0.001135;

	rvec.at<double>(0) = 1.285994831387268;
	rvec.at<double>(1) = -1.202461389136498;
	rvec.at<double>(2) = 1.180007709399751;

	tvec.at<double>(0) = -0.0003491853220207097;
	tvec.at<double>(1) = 0.2041162796497789;
	tvec.at<double>(2) = -0.03691034276389026;
}