void
	inputCallback(const sensor_msgs::Image::ConstPtr& color_image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg)
	{
		//ROS_INFO("Input Callback");

		//Elements needed for recognition




		// convert color image to cv::Mat
		cv_bridge::CvImageConstPtr color_image_ptr;
		cv::Mat color_image;
		convertColorImageMessageToMat(color_image_msg, color_image_ptr, color_image);

		// get color image from point cloud
		pcl::PointCloud<pcl::PointXYZRGB> point_cloud_src;
		pcl::fromROSMsg(*pointcloud_msg, point_cloud_src);

// cv::Mat color_image = cv::Mat::zeros(point_cloud_src.height, point_cloud_src.width, CV_8UC3);
// for (unsigned int v=0; v<point_cloud_src.height; v++)
// {
// for (unsigned int u=0; u<point_cloud_src.width; u++)
// {
// pcl::PointXYZRGB point = point_cloud_src(u,v);
// if (isnan_(point.z) == false)
// color_image.at<cv::Point3_<unsigned char> >(v,u) = cv::Point3_<unsigned char>(point.b, point.g, point.r);
// }
// }
		cv::Mat color_image_copy = color_image.clone();
		int half_side_length = 50;
		cv::rectangle(color_image_copy, cv::Point(color_image.cols/2-half_side_length, color_image.rows/2-half_side_length),
				cv::Point(color_image.cols/2+half_side_length, color_image.rows/2+half_side_length), CV_RGB(0, 255, 0), 3);
		cv::imshow("color image", color_image_copy);

		char key= cv::waitKey(20);
		//std::cout<<key;
		//char key = 'c';
		//std::cout<<"gut"<<std::endl;

		//std::cout<< "How do you want to name the image? (The name of the data cloud is the same as the one of the image"<<std::endl;


		if (key=='c')
		{
			//The image captured is in 'color_image_copy' and then classified
			Classifier C;
			C.imageExtractROI(color_image_copy);
			C.classify();

		}
		else if (key=='q')
			ros::shutdown();


	}
void CobKinectImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg)
{
//		Timer tim;
//		tim.start();

	// check camera link orientation and decide whether image must be turned around
	bool turnAround = false;
	tf::StampedTransform transform;
	try
	{
		transform_listener_->lookupTransform("/base_link", "/head_cam3d_link", ros::Time(0), transform);
		btScalar roll, pitch, yaw;
		transform.getBasis().getRPY(roll, pitch, yaw, 1);
		if (cob3Number_ == 2)
			roll *= -1.;
		if (roll > 0.0)
			turnAround = true;
		//      std::cout << "xyz: " << transform.getOrigin().getX() << " " << transform.getOrigin().getY() << " " << transform.getOrigin().getZ() << "\n";
		//      std::cout << "abcw: " << transform.getRotation().getX() << " " << transform.getRotation().getY() << " " << transform.getRotation().getZ() << " " << transform.getRotation().getW() << "\n";
		//      std::cout << "rpy: " << roll << " " << pitch << " " << yaw << "\n";
	} catch (tf::TransformException ex)
	{
		if (display_warnings_ == true)
			ROS_WARN("%s",ex.what());
	}

	// now turn if necessary
	if (turnAround == false)
	{
		// image upright
		sensor_msgs::Image color_image_turned_msg = *color_image_msg;
		color_camera_image_pub_.publish(color_image_turned_msg);
	}
	else
	{
		// image upside down
		cv_bridge::CvImageConstPtr color_image_ptr;
		cv::Mat color_image;
		convertColorImageMessageToMat(color_image_msg, color_image_ptr, color_image);

		// rotate images by 180 degrees
		cv::Mat color_image_turned(color_image.rows, color_image.cols, color_image.type());
		if (color_image.type() != CV_8UC3)
		{
			std::cout << "CobKinectImageFlipNodelet::inputCallback: Error: The image format of the color image is not CV_8UC3.\n";
			return;
		}
		for (int v = 0; v < color_image.rows; v++)
		{
			uchar* src = color_image.ptr(v);
			uchar* dst = color_image_turned.ptr(color_image.rows - v - 1);
			dst += 3 * (color_image.cols - 1);
			for (int u = 0; u < color_image.cols; u++)
			{
				for (int k = 0; k < 3; k++)
				{
					*dst = *src;
					src++;
					dst++;
				}
				dst -= 6;
			}
		}
		cv_bridge::CvImage cv_ptr;
		cv_ptr.image = color_image_turned;
		cv_ptr.encoding = "bgr8";
		sensor_msgs::Image::Ptr color_image_turned_msg = cv_ptr.toImageMsg();
		color_image_turned_msg->header = color_image_msg->header;
		color_camera_image_pub_.publish(color_image_turned_msg);
	}

	//ROS_INFO("Image callback in image flip took %f ms.", tim.getElapsedTimeInMilliSec());
}
    void inputCallback(const sensor_msgs::PointCloud2::ConstPtr& point_cloud_msg, const sensor_msgs::Image::ConstPtr& color_image_msg)
    {
      // check camera link orientation and decide whether image must be turned around
      bool turnAround = false;
      tf::StampedTransform transform;
      try
      {
        transform_listener_->lookupTransform("/base_link", "/head_cam3d_link", ros::Time(0), transform);
        btScalar roll, pitch, yaw;
        transform.getBasis().getRPY(roll, pitch, yaw, 1);
        if (roll > 0.0) turnAround = true;
  //      std::cout << "xyz: " << transform.getOrigin().getX() << " " << transform.getOrigin().getY() << " " << transform.getOrigin().getZ() << "\n";
  //      std::cout << "abcw: " << transform.getRotation().getX() << " " << transform.getRotation().getY() << " " << transform.getRotation().getZ() << " " << transform.getRotation().getW() << "\n";
  //      std::cout << "rpy: " << roll << " " << pitch << " " << yaw << "\n";
      }
      catch (tf::TransformException ex)
      {
        ROS_WARN("%s",ex.what());
      }
      
      if (turnAround==false)
      {
        // image upright, robot is watching backwards
        color_camera_image_pub_.publish(color_image_msg);
        point_cloud_pub_.publish(point_cloud_msg);
      }
      else
      {
        // image upside down, robot is watching forwards
        // read input
        // color
        cv_bridge::CvImageConstPtr color_image_ptr;
        cv::Mat color_image;
        convertColorImageMessageToMat(color_image_msg, color_image_ptr, color_image);
        
        // point cloud
        pcl::PointCloud<pcl::PointXYZ> point_cloud_src;
        pcl::fromROSMsg(*point_cloud_msg, point_cloud_src);

        // rotate images by 180 degrees
        // color
        cv::Mat color_image_turned(color_image.rows, color_image.cols, color_image.type());
        if (color_image.type() != CV_8UC3)
        {
          std::cout << "CobKinectImageFlipNodelet::inputCallback: Error: The image format of the color image is not CV_8UC3.\n";
          return;
        }
        for (int v=0; v<color_image.rows; v++)
        {
          uchar* src = color_image.ptr(v);
          uchar* dst = color_image_turned.ptr(color_image.rows-v-1);
          dst += 3*(color_image.cols-1);
          for (int u=0; u<color_image.cols; u++)
          {
            for (int k=0; k<3; k++)
            {
              *dst = *src;
              src++; dst++;
            }
            dst -= 6;
          }
        }
        
        // point cloud
        pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_turned(new pcl::PointCloud<pcl::PointXYZ>);
        for (int v=(int)point_cloud_src.height-1; v>=0; v--)
        {
          for (int u=(int)point_cloud_src.width-1; u>=0; u--)
          {
            point_cloud_turned->push_back(point_cloud_src(u,v));
          }
        }
        
        // publish turned data
        // color
        cv_bridge::CvImage cv_ptr;
        cv_ptr.image = color_image_turned;
        cv_ptr.encoding = "bgr8";
        sensor_msgs::Image::Ptr color_image_turned_msg = cv_ptr.toImageMsg();
        color_image_turned_msg->header.stamp = color_image_msg->header.stamp;
        color_camera_image_pub_.publish(color_image_turned_msg);
        
        
        // point cloud
        point_cloud_turned->header = point_cloud_msg->header;
        point_cloud_turned->height = point_cloud_msg->height;
        point_cloud_turned->width = point_cloud_msg->width;
        //point_cloud_turned->sensor_orientation_ = point_cloud_msg->sensor_orientation_;
        //point_cloud_turned->sensor_origin_ = point_cloud_msg->sensor_origin_;
        point_cloud_turned->is_dense = point_cloud_msg->is_dense;
        point_cloud_pub_.publish(point_cloud_turned);
        
  //      cv::namedWindow("test");
  //      cv::imshow("test", color_image_turned);
  //      cv::waitKey(10);
      }
    }