Sp auv colortest.cpp

/*

Singapore Polytechnic

Singapore Autonomous Underwater Vehicle Challenge 2014

Computer Vision System

Version : PRE ALPHA

* /

//Include Libraries


 * 1) include 


 * 1) include


 * 1) include


 * 1) include 


 * 1) include


 * 1) include 


 * 1) include 


 * 1) include 


 * 1) include "ros/ros.h"


 * 1) include "std_msgs/String.h"


 * 1) include "std_msgs/Int16.h"


 * 1) include "geometry_msgs/Point32.h"


 * 1) include 


 * 1) include

//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("cam_black", 1000);

ros::Publisher camera_pub_rd = n.advertise("cam_red", 1000);

ros::Publisher camera_pub_yl = n.advertise("cam_yellow", 1000);

ros::Publisher camera_pub_state = n.advertise("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;

}