First attempt for detecting bright points

simpletest_raspicam.cpp

#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;
}