Sp auv colortest.cpp

/\*

Singapore Polytechnic

Singapore Autonomous Underwater Vehicle Challenge 2014

Computer Vision System

Version : PRE ALPHA

\*/

//Include Libraries

#include <stdlib.h>

#include <iostream>

#include <vector>

#include <stdio.h>

#include <string>

#include <opencv2/imgproc/imgproc.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/opencv.hpp>

#include "ros/ros.h"

#include "std\_msgs/String.h"

#include "std\_msgs/Int16.h"

#include "geometry\_msgs/Point32.h"

#include <boost/lexical\_cast.hpp>

#include <sstream>

//Declarations

using namespace cv;

int state = 0;

int hitflare = 0;

int count = 0;

bool ifgui = true;

Mat hsvbottom;

int H;

int S;

int V;

int bot\_L\_r;

int bot\_L\_g;

int bot\_L\_b;

int bot\_U\_r;

int bot\_U\_g;

int bot\_U\_b;

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

{

//SETUP

ros::init(argc, argv, "auv\_camera");

ros::NodeHandle n;

ros::Publisher camera\_pub\_bl = n.advertise<geometry\_msgs::Point32>("cam\_black", 1000);

ros::Publisher camera\_pub\_rd = n.advertise<geometry\_msgs::Point32>("cam\_red", 1000);

ros::Publisher camera\_pub\_yl = n.advertise<geometry\_msgs::Point32>("cam\_yellow", 1000);

ros::Publisher camera\_pub\_state = n.advertise<std\_msgs::String>("cam\_state", 1000);

ros::Rate loop\_rate(10);

std\_msgs::String cam\_state;

geometry\_msgs::Point32 cam\_black;

geometry\_msgs::Point32 cam\_red;

geometry\_msgs::Point32 cam\_blue;

//Initialise Bottom Camera

FILE \* pfile;

pfile = fopen("results.txt","a");

fprintf(pfile," \n");

fprintf(pfile,"Start of test:\n");

VideoCapture cambottom(1);

if(!cambottom.isOpened()) //Checking sequence

{

std::cout<<"\n Not found \n";

return -1;

}

cambottom.set(CV\_CAP\_PROP\_FRAME\_WIDTH, 320);

cambottom.set(CV\_CAP\_PROP\_FRAME\_HEIGHT, 240);

namedWindow("Control", CV\_WINDOW\_AUTOSIZE);

int iLowH = 0;

int iHighH = 179;

int iLowS = 0;

int iHighS = 255;

int iLowV = 0;

int iHighV = 255;

cvCreateTrackbar("LowH", "Control", &iLowH, 179);

cvCreateTrackbar("HighH", "Control", &iHighH, 179);

cvCreateTrackbar("LowS", "Control", &iLowS, 255);

cvCreateTrackbar("HighS", "Control", &iHighS, 255);

cvCreateTrackbar("LowV", "Control", &iLowV, 255);

cvCreateTrackbar("HighV", "Control", &iHighV, 255);

if(ifgui) namedWindow("Original Frame (Bottom)",1);

if(ifgui) namedWindow("Thresholded Frame (Bottom)",1);

cam\_state.data="black";

camera\_pub\_state.publish(cam\_state);

while(1)

{

count += 1;

//pfile = fopen("results.txt","w");

Mat framebottom;

cambottom >> framebottom; // Get a new frame from camera

GaussianBlur(framebottom, framebottom, Size(7,7), 1.5, 1.5);

cvtColor(framebottom, hsvbottom, CV\_BGR2HSV);

Mat imgThresholded;

inRange(framebottom, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);

erode(imgThresholded, imgThresholded,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));                  //Eliminate the small particles

dilate(imgThresholded, imgThresholded,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

erode(imgThresholded, imgThresholded,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

dilate(imgThresholded, imgThresholded,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

Moments oMoments = moments(imgThresholded);

double dM01 = oMoments.m01;

double dM10 = oMoments.m10;

double dArea = oMoments.m00;

if(dArea >2000)

{

int posX = dM10/dArea;

int posY = dM01/dArea;

if(ifgui) circle(framebottom,cvPoint(posX,posY),5,cvScalar(0,255,255),4);

}

//unsigned char\*input = (unsigned char\*)(hsvbottom.data);

unsigned char\*input = (unsigned char\*)(framebottom.data);

double totalH = 0;

double totalS = 0;

double totalV =0;

double count = 0;

//for(int i = 110; i < 130; i++){       //rows

//for(int j= 150; j< 170;j++){      //columns

//input[framebottom.step \* i + j\*3 ] = 255;

//input[framebottom.step \* i + j\*3 + 1 ] = 255;

//input[framebottom.step \* i + j\*3 + 2 ] = 255;

//}

//          }

for(int i = 110; i < 130; i++){     //rows

for(int j= 150; j< 170;j++){        //columns

//H = input[hsvbottom.step \* i + j\*3 ];             //Hue

H= input[framebottom.step \* i + j\*3 ] ;                             //blue

totalH = totalH + H;

//S = input[hsvbottom.step \* i + j\*3 + 1 ];         //saturation

S= input[framebottom.step \* i + j\*3 + 1 ];                             //green

totalS = totalS + S;

//V = input[hsvbottom.step \* i + j\*3 + 2 ];         //value

V = input[framebottom.step \* i + j\*3 + 2 ];                             //red

totalV = totalV + V;

count++;                }

}

totalH = totalH / count;

totalS = totalS / count;

totalV = totalV / count;

printf (" H  %d S %d V %d \n",(unsigned char) totalH,(unsigned char) totalS,(unsigned char) totalV);

CvPoint P1;

P1.x=150;

P1.y=110;

CvPoint P2;

P2.x=170;

P2.y=130;

rectangle(framebottom, P1 , P2, Scalar(0,0,255),1,8);

if(ifgui) imshow("Original Frame (Bottom)", framebottom);

if(ifgui) imshow("[Black] Thresholded Frame (Bottom)", imgThresholded);

if(waitKey(30) >= 0) break;

}

// the camera will be deinitialized automatically in VideoCapture destructor

return 0;

}