Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
First attempt for detecting bright points
#include <ctime>
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdlib.h>
#include <unistd.h>
#include <raspicam/raspicam.h>
#include <signal.h>
//#include <stdlib.h>
#include <stdio.h>
using namespace std;
int brightpoints = 0;
//int brightpoint = 253;
int brightpointThreshold = 243;
int minBrightPoints = 15;
int threshold = 10;
int xseparation = 200;
int yseparation = 200;
bool isRealPoint(unsigned char *data, int x, int y, int width, int height, int xy);
bool checkHorizontalLine(unsigned char *data, int x, int y, int width, int height, int xy);
bool checkVerticalLine(unsigned char *data, int x, int y, int width, int height, int xy);
void saveImage(unsigned char *data, raspicam::RaspiCam &Camera, bool isYUV);
unsigned char *getImage();
void sigint(int sig){ // can be called asynchronously
printf("Caught signal %d\n", sig);
exit(1);
}
int main ( int argc, char **argv ) {
raspicam::RaspiCam Camera; //Camera object
//Open camera
cout<<"Opening Camera..."<<endl;
Camera.setFormat(raspicam::RASPICAM_FORMAT_YUV420);
if ( !Camera.open()) {cerr<<"Error opening camera"<<endl;return -1;}
//wait a while until camera stabilizes
cout<<"Sleeping for 3 secs"<<endl;
usleep(3000);
signal(SIGINT, sigint);
// read test data:
//unsigned char *newImg = getImage();
cout << "Procession YUV data..." << endl;
int height = Camera.getHeight();
int width = Camera.getWidth();
int framesize = width * height;
int y, u, v, posy, posu, posv;
int pointXMin, pointXMax, pointYMin, pointYMax;
while (1) {
// Clean vars
pointXMin = Camera.getWidth(); pointXMax = 0; pointYMin = Camera.getHeight(); pointYMax = 0; brightpoints = 0;
//allocate memory
unsigned char *newImg = new unsigned char[ Camera.getImageTypeSize ( raspicam::RASPICAM_FORMAT_YUV420 )];
//capture
Camera.grab();
//extract the image in yuv format
Camera.retrieve ( newImg,raspicam::RASPICAM_FORMAT_IGNORE );//get camera image
for (int i = 0, ij=0; i < height; i++) {
for (int j = 0; j < width; j++,ij++) {
//cout << "Bright of point: " << (int)(newImg[ij]) << ", Compare with: " << brightpointThreshold << " = " << ((int)(newImg[ij]) >= brightpointThreshold ? "Yes" : "No" ) << endl;
if (
((int)(newImg[ij]) >= brightpointThreshold)
//// && (int)oldImg[ij] < 250
// (int)(newImg[ij]) == 88
) {
//brightpoints++;
//cout << "Catched bright of point: " << (int)(newImg[ij]) << endl;
//cout << "Point x: " << j << ", Point y: " << i << ", Y: " << y << ", U: " << u << ", V: " << v << endl;
//cout << "Array positions , Y: " << posy << ", U: " << posu << ", V: " << posv << endl;
//if (j < 585 || j > 595 || i < 255 || i > 265) continue; // Why remove borders?/
if ( isRealPoint(newImg, j, i, width, height, ij) ) {
posy = ij;
posu = framesize + (ij / 2);
posv = framesize + (framesize/4) + (ij/2);
y = (int)newImg[posy];
u = (int)newImg[posu];
v = (int)newImg[posv];
//cout << "IJ Pointer: " << ij << ", IJ/2: " << (ij/2) << endl;
//cout << "Point x: " << j << ", Point y: " << i << ", Y: " << y << ", U: " << u << ", V: " << v << endl;
//cout << "Array positions , Y: " << posy << ", U: " << posu << ", V: " << posv << endl;
brightpoints++;
if (j < pointXMin) pointXMin = j;
if (j > pointXMax) pointXMax = j;
if (i < pointYMin) pointYMin = j;
if (i < pointYMax) pointYMax = j;
}
}
}
}
if (pointXMax != 0 && brightpoints > minBrightPoints) {
int pointX = (pointXMin + pointXMax) / 2;
int pointY = (pointYMin + pointYMax) / 2;
cout << "Set X, Y point at: " << pointX << ", " << pointY / 2 << endl;
}
if (pointXMax != 0 && brightpoints > minBrightPoints)
cout << "Total bright points: " << brightpoints << endl;
// SAVE IMAGE:
//saveImage(newImg, Camera, true);
//free resrources
delete newImg;
}
return 0;
}
bool isRealPoint(unsigned char *data, int x, int y, int width, int height, int xy) {
bool ispoint = false;
// WITH THIS WE WALK A SQUARE AROUND THE POINT TO CHECK IF IS AN ISOLATED DOT
ispoint = checkHorizontalLine(data, x, y+yseparation, width, height, xy)
&& checkHorizontalLine(data, x, y-yseparation, width, height, xy)
&& checkVerticalLine(data, x+xseparation, y, width, height, xy)
&& checkVerticalLine(data, x-xseparation, y, width, height, xy);
return ispoint;
}
unsigned char *getImage() {
cout<<"Preparing File..."<<endl;
// open the file:
streampos fileSize;
ifstream file("yuv_test", std::ios::binary);
// get its size:
file.seekg(0, std::ios::end);
fileSize = file.tellg();
file.seekg(0, std::ios::beg);
// read the data:
unsigned char *image = new unsigned char[fileSize];
file.read((char *)image, fileSize);
cout << "File size: " << fileSize << endl;
return image;
}
bool checkHorizontalLine(unsigned char *data, int x, int y, int width, int height, int xy) {
int myxy = (y*width) + x;
for (int i = 0; i < (xseparation*2); i++) {
if (abs( (int)(data[xy]) - (int)(data[myxy-yseparation+i]) ) < threshold ) {
cout << "Horizontal point to check x: " << x << ", Point y: " << y << ", Y: " << (int)(data[xy]) << endl;
cout << "Threshold fail @ x: " << x-xseparation+i << ", Point y: " << y << ", Y_dst: " << (int)(data[myxy-xseparation+i]) << endl;
return false;
}
}
return true;
}
bool checkVerticalLine(unsigned char *data, int x, int y, int width, int height, int xy) {
int myxy = (y*width) + x;
for (int i = 0; i < (yseparation*2); i++) {
if (abs( (int)(data[xy]) - (int)(data[myxy-((i-yseparation)*width)]) ) < threshold ) {
cout << "Vertical point check x: " << x << ", Point y: " << y << ", Y: " << (int)(data[xy]) << endl;
cout << "Threshold fail @ x: " << x-xseparation+i << ", Point y: " << y << ", Y_dst: " << (int)(data[myxy-((i-yseparation)*width)]) << endl;
return false;
}
}
return true;
}
void saveImage ( unsigned char *data, raspicam::RaspiCam &Camera, bool isYUV ) {
//save
std::stringstream filepath;
filepath << "image-yuv-_" << time(NULL);
std::ofstream outFile ( filepath.str().c_str(),std::ios::binary );
if ( !isYUV ) {
outFile<<"P6\n";
outFile<<Camera.getWidth() <<" "<<Camera.getHeight() <<" 255\n";
}
outFile.write ( ( char* ) data,Camera.getImageBufferSize() );
cout<<"Image saved at raspicam_image"<<endl;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment