Quadrotor

/************************************************************************

  			Blob.h

FUNCIONALITAT: Definici? de la classe CBlob

AUTOR: Inspecta S.L.

MODIFICACIONS (Modificaci?, Autor, Data):

FUNCTIONALITY: Definition of the CBlob class and some helper classes to perform

			   some calculations on it

AUTHOR: Inspecta S.L.

MODIFICATIONS (Modification, Author, Date):

**************************************************************************/

//! Disable warnings referred to 255 character truncation for the std:map

#pragma warning( disable : 4786 ) 

#ifndef CBLOB_INSPECTA_INCLUDED

#define CBLOB_INSPECTA_INCLUDED

#include <opencv/cxcore.h>

#include "BlobLibraryConfiguration.h"

#include "BlobContour.h"

#ifdef BLOB_OBJECT_FACTORY

	//! Object factory pattern implementation

	#include "..\inspecta\DesignPatterns\ObjectFactory.h"

#endif

//! Type of labelled images

typedef unsigned int t_labelType;

//! Blob class

class CBlob

{

	typedef std::list<CBlobContour> t_contourList;

public:

	CBlob();

	CBlob( t_labelType id, CvPoint startPoint, CvSize originalImageSize );

	~CBlob();

	//! Copy constructor

	CBlob( const CBlob &src );

	CBlob( const CBlob *src );

	//! Operador d'assignaci?

	//! Assigment operator

	CBlob& operator=(const CBlob &src );

	//! Adds a new internal contour to the blob

	void AddInternalContour( const CBlobContour &newContour );

	//! Retrieves contour in Freeman's chain code

	CBlobContour *GetExternalContour()

	{

		return &m_externalContour;

	}

	//! Retrieves blob storage

	CvMemStorage *GetStorage()

	{

		return m_storage;

	}

	//! Get label ID

	t_labelType GetID()

	{

		return m_id;

	}

	//! > 0 for extern blobs, 0 if not

	int	  Exterior( IplImage *mask, bool xBorder = true, bool yBorder = true );

	//! Compute blob's area

	double Area();

	//! Compute blob's perimeter

	double Perimeter();

	//! Compute blob's moment (p,q up to MAX_CALCULATED_MOMENTS)

	double Moment(int p, int q);

	//! Compute extern perimeter 

	double ExternPerimeter( IplImage *mask, bool xBorder  = true, bool yBorder = true );

	//! Get mean grey color

	double Mean( IplImage *image );

	//! Get standard deviation grey color

	double StdDev( IplImage *image );

	//! Indica si el blob est? buit ( no t? cap info associada )

	//! Shows if the blob has associated information

	bool IsEmpty();

	//! Retorna el poligon convex del blob

	//! Calculates the convex hull of the blob

	t_PointList GetConvexHull();

	//! Pinta l'interior d'un blob d'un color determinat

	//! Paints the blob in an image

	void FillBlob( IplImage *imatge, CvScalar color, int offsetX = 0, int offsetY = 0 );

	//! Join a blob to current one (add's contour

	void JoinBlob( CBlob *blob );

	//! Get bounding box

	CvRect GetBoundingBox();

	//! Get bounding ellipse

	CvBox2D GetEllipse();

	//! Minimun X	

	double MinX()

	{

		return GetBoundingBox().x;

	}

	//! Minimun Y

	double MinY()

	{

		return GetBoundingBox().y;

	}

	//! Maximun X

	double MaxX()

	{

		return GetBoundingBox().x + GetBoundingBox().width;

	}

	//! Maximun Y

	double MaxY()

	{

		return GetBoundingBox().y + GetBoundingBox().height;

	}

private:

	//! Deallocates all contours

	void ClearContours();

	//////////////////////////////////////////////////////////////////////////

	// Blob contours

	//////////////////////////////////////////////////////////////////////////

	//! Contour storage memory

	CvMemStorage *m_storage;

	//! External contour of the blob (crack codes)

	CBlobContour m_externalContour;

	//! Internal contours (crack codes)

	t_contourList m_internalContours;

	//////////////////////////////////////////////////////////////////////////

	// Blob features

	//////////////////////////////////////////////////////////////////////////

	//! Label number

	t_labelType m_id;

	//! Area

	double m_area;

	//! Perimeter

	double m_perimeter;

	//! Extern perimeter from blob

	double m_externPerimeter;

	//! Mean gray color

	double m_meanGray;

	//! Standard deviation from gray color blob distribution

	double m_stdDevGray;

	//! Bounding box

	CvRect m_boundingBox;

	//! Bounding ellipse

	CvBox2D m_ellipse;

	//! Sizes from image where blob is extracted

	CvSize m_originalImageSize;

};

#endif //CBLOB_INSPECTA_INCLUDED

#include <ros/ros.h>

#include <stdio.h>

#include <vision/TargetDescriptor.h>

#include <image_transport/image_transport.h>

#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

#include <algorithm>

ros::Publisher pub;

ros::Publisher img_pub;

int hk = 216; 

int sk = 255;

int vk = 255;

int threshold = 25;

bool isRight(sensor_msgs::Image * img, int p){

	return (img->data[p + 1] == img->data[p]);

}

bool isLeft(sensor_msgs::Image * img, int p){

	return (img->data[p - 1] == img->data[p]);

}

bool isUp(sensor_msgs::Image * img, int p){

	return (img->data[p - img->width] == img->data[p]);

}

bool isDown(sensor_msgs::Image * img, int p){

	return (img->data[p + img->width] == img->data[p]);

}

int min(int a, int b){

        if (a <= b) return a;

        else return b;

}

int max(int a, int b){

        if (a >= b) return a;

        else return b;

}

bool hasAnAdjacent(sensor_msgs::Image * image, int j){

	int nl = image->height; // number of lines

	int nc = image->width; 

        int nChannels = image->step / image->width;

	int pos = j / nChannels; 

	if ((pos % nc) != 0){

		if (isLeft(image, pos)) return true;

	}

	if ((pos / nc) != 0){

		if (isUp(image, pos)) return true;

	}

	if ((pos % nc) != nc - 1){

		if (isRight(image, pos)) return true;

	}

	if ((pos / nc) != nc - 1){

		if (isDown(image, pos)) return true;

	}

	else return false;

}

/* Detects the blobs in an image within above defined constraints. Transforms image into 255

 * for pixels in the blob, 0 for not in the blob.

 */

void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){

	int nl = image->height; // number of lines

	int nc = image->step; // number of columns

	int nChannels = image->step / image->width; // effective width

	// process image

	for (int i = 0; i < nl; i++){

		for (int j = 0; j < nc; j+= nChannels){

			if (image->data[i * image->width + j] <= hk + threshold && image->data[j] >= hk - threshold){

				image->data[i * image->width + j] = 255;

				image->data[i * image->width + j+1] = 255;

				image->data[i * image->width + j+2] = 255;

			}

			else {

				image->data[i * image->width + j] = 0;

				image->data[i * image->width + j+1] = 0;

				image->data[i * image->width + j+2] = 0;

			}

		}

	}

	int comXSum = 0;

	int comYSum = 0;

	int blobPixelCount = 0;

	for (int i = 0; i < image->width * image->height; i+=nChannels){

		if ( !(image->data[i] == 255 && hasAnAdjacent(image, i)) ){

			image->data[i] = 0;

			image->data[i + 1] = 0;

			image->data[i + 2] = 0;

       			continue;

		}

		blobPixelCount++;

		comXSum += i / nChannels % image->width;

		comYSum += i / nChannels / image->width;

	}

  if (blobPixelCount != 0){

	// get average

  	comXSum /= blobPixelCount;

  	comYSum /= blobPixelCount;

  }

	*area = blobPixelCount;

	*comX = comXSum;

	*comY = comYSum;

  img_pub.publish(*image);

}

void convertToHSV(sensor_msgs::Image * orig){

  int nChannels = orig->step / orig->width; 

  printf("Converting to HSV \n");

  // get the max, set it as v

  for (int i = 0; i < orig->height * orig->width; i += nChannels){

      int h;

      int s;

      int v; 

      int min;

      // get min, max

      if (orig->data[i] >= orig->data[i+1] && orig->data[i] >= orig->data[i+2]){

        v = orig->data[i];

        if (orig->data[i+1] >= orig->data[i+2]){

            min = orig->data[i+2];

        }

        else min = orig->data[i+1];

      }

      else if (orig->data[i+1] >= orig->data[i] && orig->data[i+1] >= orig->data[i+2]){

        v = orig->data[i+1];

        if (orig->data[i] >= orig->data[i+2]){

          min = orig->data[i+2];

        }

        else min = orig->data[i];

      }

      else {

        v = orig->data[i+2];

        if (orig->data[i] >= orig->data[i+1]){

            min = orig->data[i+1];

        }

        else min = orig->data[i];

      }

      // set s

      if (v != 0){

      s = (v - min) / v; 

      }

      else s = 0;

      // set h

      if (s == 0) h = 0;

      else if (v == orig->data[i]){

        h = 60 * (orig->data[i+1] - orig->data[i+2]) / s;

      }

      else if (v == orig->data[i+1]){

        h = 120 + 60 * (orig->data[i+2] - orig->data[i]) / s;

      }

      else {

        h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s;

      }

      orig->data[i] = h;

      orig->data[i+1] = s;

      orig->data[i+2] = v;

  }

  printf("Conversion complete\n");

}

void target_cb(const sensor_msgs::Image orig) {

  printf("beginning callback \n");

  tf::TransformListener listener;

  geometry_msgs::Point point;

  tf::StampedTransform transform;

  int width = orig.width; 

  int height = orig.height;

  /*try {

    listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);

  }

  catch (tf::TransformException ex) {

    ROS_ERROR("%s", ex.what());

  }*/

  // convert to OpenCV

  sensor_msgs::Image copy = orig;

  sensor_msgs::Image * source = © // pointer to the image

  sensor_msgs::Image hsvImage = orig; // May be time 

  sensor_msgs::Image * hsvPtr = &hsvImage;

  // convert to HSV

  convertToHSV(source);

  // detect blob

  int comX; 

  int comY;

  int area;

  detectBlobs(hsvPtr, &comX, &comY, &area);

  int distance;

  // give distance as inversely proportional to area, for (x,y,z) 

  if (area > 5){

    distance = 100.0 / (float) area;

  }

  else {

    distance = -1;

  } 

  // fill point based on target and tf

  geometry_msgs::Point origPoint;

  // TODO: Change coordinate axes

  origPoint.x = distance;

  origPoint.y = width / 2 - comX;

  origPoint.z = comY - height / 2;

  // TODO transform into point

  // for now just publish that

  pub.publish(origPoint);

  printf("Ending callback");

}

void target_cb_test(const sensor_msgs::Image image){

	geometry_msgs::Point point;

	point.x = 1;

	point.y = 2;

	point.z = 3;

	pub.publish(point);

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "v2v3_converter");

  ros::NodeHandle n;

  pub = n.advertise<geometry_msgs::Point>("vision/target_3d", 1000);

  img_pub = n.advertise<sensor_msgs::Image>("vision/blob_image", 1000);

  ros::Subscriber sub = n.subscribe("/camera/rgb/image_color", 1, target_cb);

  ros::spin();

  return 0;

}

#include "/home/nstanley/OpenCV/OpenCV-2.3.1/include/opencv/cv.h"

#include "/home/nstanley/OpenCV/OpenCV-2.3.1/include/opencv/highgui.h"

#include <opencv/cxcore.h>

#include <ros/ros.h>

#include <stdio.h>

#include <cv_bridge/cv_bridge.h>

#include <sensor_msgs/image_encodings.h>

#include <vision/TargetDescriptor.h>

#include <image_transport/image_transport.h>

#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

#include <algorithm>

#include </home/nstanley/cvblobs8.3_linux/blob.h>

#include </home/nstanley/cvblobs8.3_linux/BlobResult.h>

ros::Publisher pub;

ros::Publisher img_pub;

int hk = 216; 

int sk = 255;

int vk = 255;

//int threshold = 25;

int filter(int r, int g, int b, int threshold) {

    int orange[3] = {216, 255, 255}; // Orange in HSV

    int diff =  (orange[0]-r)*(orange[0]-r) +

                (orange[1]-g)*(orange[1]-g);

    if(diff < threshold) return abs(diff-threshold);

    return 0;

}

void target_cb(const sensor_msgs::Image orig) {

  printf("beginning callback \n");

  tf::TransformListener listener;

  geometry_msgs::Point point;

  tf::StampedTransform transform;

  int width = orig.width; 

  int height = orig.height;

  /*try {

    listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);

  }

  catch (tf::TransformException ex) {

    ROS_ERROR("%s", ex.what());

  }*/

  // convert to OpenCV

  sensor_msgs::Image hsvcopy = orig;

  cv_bridge::CvImagePtr cv_ptr;

  try{

    cv_ptr = cv_bridge::toCvCopy(orig, sensor_msgs::image_encodings::BGR8);

  }

  catch (cv_bridge::Exception& e)  {

    ROS_ERROR("cv_bridge exception: %s", e.what());

    return;

  }

  // detect blob

  IplImage* input;

  *input = cv_ptr->image;

  IplImage* img;

  IplImage *hsv_img;

  IplImage *bw_img;

  IplImage* i1;

  CBlobResult blobs;

  CBlob blobArea;

  // Initialize images, allocate memory

  img = cvCloneImage(input);

  hsv_img = cvCloneImage(img);

  bw_img = cvCloneImage(img);

  i1 = cvCreateImage(cvSize(img->width, img->height), 8, 1);

  // Smooth input image using a Gaussian filter, assign HSV, BW image

  cvSmooth(input, img, CV_GAUSSIAN, 7, 9, 0 ,0);

  cvCvtColor(img, bw_img, CV_RGB2GRAY);

  cvCvtColor(img, hsv_img, CV_RGB2HSV);

  // Simple filter that creates a mask whose color is near orange in i1

  for(int i = 0; i < hsv_img->height; i++) {

      for(int j = 0; j < hsv_img->width; j++) {

          int r = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 2];

          int g = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 1];

          int b = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 0];

          int f = filter(r, g, b, 5000);

          if(f) {

              ((uchar *)(i1->imageData + i*i1->widthStep))[j] = 250;

          }

      }

  }

  // Detect Blobs using the mask and a threshold for area. Sort blobs according to area

  blobs = CBlobResult(bw_img, i1, 100/*, true*/);

  blobs.Filter(blobs, B_INCLUDE, CBlobGetArea(), B_GREATER, 500);

  blobs.GetNthBlob(CBlobGetArea(), blobs.GetNumBlobs() - 1, blobArea); // example

  for (int i = 1; i < blobs.GetNumBlobs(); i++ )

  {

      blobArea = blobs.GetBlob(i);

      if(blobArea.Area() > 150)

      blobArea.FillBlob(img, cvScalar(255, 0, 0));

  }

  // Publish blob image

  cv_bridge::CvImage blob_image;

  //blob_image.header = img->header;

  blob_image.encoding = sensor_msgs::image_encodings::BGR8;

  blob_image.image = *(img->imageData);

  /*sensor_msgs::Image blob_image; 

  blob_image.height = img->height; 

  blob_image.width = img->width;

  blob_image.data = *(img->imageData); */

  img_pub.publish(blob_image);  

  // Clean up 

  cvReleaseImage(&i1);

  cvReleaseImage(&bw_img);

  cvReleaseImage(&hsv_img);

  cvReleaseImage(&img);

  cvReleaseImage(&input);

  //int comX; 

  //int comY;

  //int area;

  //int distance;

  // fill point based on target and tf

//  geometry_msgs::Point origPoint;

  // TODO: Change coordinate axes

//  origPoint.x = distance;

//  origPoint.y = width / 2 - comX;

//  origPoint.z = comY - height / 2;

  // TODO transform into point

  // for now just publish that

//  pub.publish(origPoint);

  printf("Ending callback\n");

}

void target_cb_test(const sensor_msgs::Image image){

	geometry_msgs::Point point;

	point.x = 1;

	point.y = 2;

	point.z = 3;

	pub.publish(point);

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "v2v3_converter");

  ros::NodeHandle n;

  pub = n.advertise<geometry_msgs::Point>("vision/target_3d", 1000);

  img_pub = n.advertise<sensor_msgs::Image>("vision/blob_image", 1000);

  ros::Subscriber sub = n.subscribe("/camera/rgb/image_color", 1, target_cb);

  ros::spin();

  return 0;

}