VisualVector.cpp

Go to the documentation of this file.

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file VisualVector.cpp
*  \brief Implementation of the VisualVector class
*  \author $Author: lljones $
*  \version $Revision: 1.1 $
*  \date    $Date: 2007/03/29 23:03:43 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*!
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "VisualVector.h"
//#include "cv.h"               // Open CV headers
//#include "cxcore.h"   // Open CV headers


///////////////////////////////////////////////////////////////////////
// Construction/Destruction                                          //
///////////////////////////////////////////////////////////////////////

/*! \brief Default Constructor
*/

VisualVector::VisualVector()
{
        WorkingImage = NULL;
        RawImage = NULL;
        Debug = false;

        capture = cvCaptureFromCAM(CV_CAP_ANY);
        if (!capture)
        {
                cout << "no capture device available" << endl;
                exit(1);
        }

        cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 320);
        cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 240);

}

VisualVector::VisualVector(bool debug)
{
        WorkingImage = NULL;
        RawImage = NULL;
        Debug = debug;

        capture = cvCaptureFromCAM(CV_CAP_ANY);
        if (!capture)
        {
                cout << "no capture device available" << endl;
                exit(1);
        }

        cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 320);
        cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 240);

        
}


VisualVector::~VisualVector()
{
        // clean up objects
        cvReleaseCapture(&capture);

}

/////////////////////////////////////////////////////////////////////////
// Get Measurements                                                    //
/////////////////////////////////////////////////////////////////////////

/*! \brief Function to return scalar measurement (not currently used)
*/
Measurement VisualVector::GetMeasurement()
{
// leave me blank
return 0;
}

/*! \brief Function to return Visual Vector measurement in the form of a vector
*/
Vector VisualVector::GetVectorMeasurement()
{
        int check;
        float a1,a2;
        long double a3;
        double a1deg,a2deg,a3deg;       //hold degree values of a1,a2,a3 angles
        int pixelheight,pixelwidth;     //store height and width of WorkingImage (in pixels)

        const int CENTZEROTILT[2] = {140,154};  //zero-tilt location of centroid
        const double RUNIT = 17.2;              //inches to camera
        const double HUNIT = 88.25;             //inches to ceiling
        const double PI = 3.1415926535897932384626433832795028841971693993751;

        // get WorkingImage from capture object
        RawImage = cvQueryFrame(capture);
        if (!RawImage)
        {
                cout << "no WorkingImage captured" << endl;
                exit(1);
        }

        WorkingImage = cvCreateImage(cvSize(RawImage->width,RawImage->height),IPL_DEPTH_8U,1);

        cvConvertImage(RawImage,WorkingImage,CV_CVTIMG_FLIP);

        pixelheight = WorkingImage->height;
        pixelwidth = WorkingImage->width;

        check = FindVertex();

        if (check != 0)
        {
                if (check == 1)
                        cout << "Error within FindVertex:  Equilateral Triangle Inputted.\n";
                else if (check == 2)
                        cout << "Error within FindVertex:  Area mismatch - is whole triangle present?\n";
                else if (check == 3)
                        cout << "Error within FindVertex:  Intensity mismatch - are lights on?\n";
                else if (check == 4)
                        cout << "Error within FindVertex:  Vertex Index out of Image\n";
                else
                        cout << "Error within FindVertex:  Unknown Error\n";
                return 0;
        }


        //cleanup WorkingImage (do NOT clean RawImage b/c derived from capture)
        cvReleaseImage(&WorkingImage);
        WorkingImage = NULL;
        RawImage = NULL;
        //is this neccessary?
        capture = NULL;

        // Running functions
        a3 = RotationAngleFunc(pixelwidth, pixelheight);
        a3deg = (180 * a3) / PI;
        
        a1 = TiltAngle1Func(a3, pixelheight, pixelwidth, CENTZEROTILT,  RUNIT, HUNIT);
        a1deg = (a1 * 180) / PI;

        a2 = TiltAngle2Func(a3, pixelheight, pixelwidth, CENTZEROTILT, RUNIT, HUNIT);
        a2deg = (a2 * 180) / PI;

        if (Debug == true)
        {
                cout << endl << "Corner Locations (furthest vertex listed first):\nx1: "
                        << vertex[0] << "\ty1: " << vertex[1] << "\nx2: " << vertex[2]
                        << "\ty2: "     << vertex[3] << "\nx3: " << vertex[4] << "\ty3: "
                        << vertex[5] << endl << "Centroid:\n  x: " << centroid[0]
                        << "\n  y: " << centroid[1] << endl << endl;
        
                cout << "Angle about the N1 axis (deg) = " << a1deg << endl;
                cout << "Angle about the N2 axis (deg) = " << a2deg << endl;
                cout << "Angle about the N3 axis (deg) = " << a3deg << endl << endl;
        }

        Vector AngleVals(3);
        AngleVals(1) = a1deg;
        AngleVals(2) = a2deg;
        AngleVals(3) = a3deg; 
        return AngleVals;
}

//////////////////////////////////////////////////////////////////////////
// Included Functions                                                   //
////////////////////////////////////////////////////////////////////////// 

/*! \brief Visual Vector Vertex Finder
*/
int VisualVector::FindVertex()
{
        const double MIN_DISTANCE = 45;         //minimum Euclidean distance between pixels
        const double QUALITY = 0.01;            //minimum quality of features (based on eigenvals)
        //const double AREA_MIN = 1400;         //minimum value of triangle area (pixels)
        //const double AREA_MAX = 6000;         //maximum value of triangle area (pixels)
        const int MAX_COUNT = 3;                //maximum number of detected features/points
        int corner_count;                       //will have value <= MAX_COUNT to indicate points found
        int check;                              //if non-zero, error in function present
        double area;                            //area of triangle

        IplImage *temp = NULL, *eigenval = NULL;

        corner_count = MAX_COUNT;
        CvPoint2D32f corners[3];

        eigenval = cvCreateImage(cvSize(WorkingImage->width,WorkingImage->height),32,1);
        temp     = cvCreateImage(cvSize(WorkingImage->width,WorkingImage->height),32,1);

        //Precursor to corner detection - finds eigenvals
        cvCornerMinEigenVal(WorkingImage, eigenval,3,3);
        //detects strong corners/features.  Note that parameters following NULL are optional.
        cvGoodFeaturesToTrack(WorkingImage, eigenval, temp, corners, &corner_count, QUALITY, MIN_DISTANCE, NULL, 3, 0, 0.04);

        //cleanup unneeded images
        cvReleaseImage(&temp); cvReleaseImage(&eigenval);
        temp = NULL; eigenval = NULL;

        vertex[0] = int(corners[0].x); vertex[1] = int(corners[0].y); vertex[2] = int(corners[1].x);
        vertex[3] = int(corners[1].y); vertex[4] = int(corners[2].x); vertex[5] = int(corners[2].y);

        check = VertexSwitch();
        if (check != 0)
                return 1;       //error indicating equilateral triangle detected

        //Calculate area of Triangle
        area = (vertex[0]*vertex[5]-vertex[0]*vertex[3]+vertex[2]*vertex[1]-vertex[2]*vertex[5]
                +vertex[4]*vertex[3]-vertex[4]*vertex[1])/2;
        area = fabs(area);

        //area error check
        //if ((area <= AREA_MIN) || (area >= AREA_MAX))
        //      return 2;  //<Error2>  area mismatch

        //intensity error check

        /*
                ADD THIS LATER
        */

        //determine centroid
        centroid[0] = double(vertex[0]+vertex[2]+vertex[4])/3;
        centroid[1] = double(vertex[1]+vertex[3]+vertex[5])/3;
        
        if (Debug == true)
        {
        //      cout << "area: " << area << endl;
                //create color picture
                IplImage *color = NULL;
                //define colors
                CvScalar red;   red.val[0] = 0; red.val[1] = 0; red.val[2] = 255;
                CvScalar blue;  blue.val[0] = 255; blue.val[1] = 0; blue.val[2] = 0;
                CvScalar black; black.val[0] = 0; black.val[1] = 0; black.val[2] = 0;
                //create color image holder of same dimensions
                color = cvCreateImage(cvSize(WorkingImage->width,WorkingImage->height),8, 3);
                //copy B&W image to color-enabled picture
                cvCvtColor(WorkingImage,color,CV_GRAY2RGB);
                //show verticies in red and centroid in blue
                cvSet2D(color,int(corners[0].y),int(corners[0].x),red);
                cvSet2D(color,int(corners[1].y),int(corners[1].x),red);
                cvSet2D(color,int(corners[2].y),int(corners[2].x),red);
                cvSet2D(color,int(centroid[1]),int(centroid[0]),blue);
                //Save Image with Verticies and Centroid superimposed
                cvSaveImage("ImageFeatures.bmp",color);
                cvReleaseImage(&color);
                color = NULL;
        }

        return 0;
}


/*! \brief Visual Vector Vertex Sorter
*/
int VisualVector::VertexSwitch()
{
        int tempVertex;                 //temporarily stores vertex values for swapping
        double dist1,dist2,dist3;       //holds returns of distance function for swapping

        //distance formula between points
        dist1 = sqrt(pow(double(vertex[0] - vertex[2]),2) + pow(double(vertex[1] - vertex[3]),2));
        dist2 = sqrt(pow(double(vertex[0] - vertex[4]),2) + pow(double(vertex[1] - vertex[5]),2));
        dist3 = sqrt(pow(double(vertex[2] - vertex[4]),2) + pow(double(vertex[3] - vertex[5]),2));

        if ((dist3 < dist1) && (dist3 <= dist2)); //side 3 shortest: 0,1 furthest vertex/no change
        else if ((dist2 < dist1) && (dist2 <= dist3)) //side 2 is shortest: 2,3 is furthest vertex
        {       //switch 2,3 and 0,1
                tempVertex = vertex[0];
                vertex[0] = vertex[2];
                vertex[2] = tempVertex;
                tempVertex = vertex[1];
                vertex[1] = vertex[3];
                vertex[3] = tempVertex;
        }
        else if ((dist1 < dist2) && (dist1 <= dist3))   //side 1 is shortest, 4,5 is furthest vertex
        {       //switch 4,5 and 0,1
                tempVertex = vertex[0];
                vertex[0] = vertex[4];
                vertex[4] = tempVertex;
                tempVertex = vertex[1];
                vertex[1] = vertex[5];
                vertex[5] = tempVertex;
        }
        else    //3 verticies are the same length from each other/Equilateral Triangle
                return 1;

        return 0;       //no-error return
}

/*! \brief Visual Vector Rotation Angle Calculations
*/
long double VisualVector::RotationAngleFunc(int pixelwidth, int pixelheight)
{
        // v1  represents the vertex farthest from the centroid
        // cent represents the centroid
        // pixelwidth and pixelheight are the size of the overall WorkingImage

        const double PI = 3.1415926535897932384626433832795028841971693993751;

        if (centroid[0] < 0 || centroid[1] < 0 || vertex[0] < 0 || vertex[1] < 0){
                cout << "Invalid inputs for the centroid or vertex" << endl;
        }

        // Setting up the camera frame vector
        int cox = pixelwidth/2;                 // Camera origin x coordinate
        int coy = pixelheight/2;                // Camera origin y coordinate
        int c1ex = pixelwidth;                  // Endpoint of c1 axis x coordinate
        int c1ey = pixelheight/2;               // Enpoint of c1 axis y coordinate
        int c1x = c1ex - cox;                   // X component of c1/ b1
        int c1y = c1ey - coy;                   // Y component of c1/ b1


        // Setting up the inertial frame vector
        double n1x = vertex[0] - centroid[0];           // X component of n1
        double n1y = vertex[1] - centroid[1];           // Y component of n1

        // Calculating the rotation angle
        double dot = c1x*n1x+c1y*n1y;   // Dot product of c1 and n1
        double normc1 = sqrt( pow((double)c1x,2) + pow((double)c1y,2) ); // Converting to double to shut up compiler
        double normn1 = sqrt( pow((double)n1x,2) + pow((double)n1y,2) ); // Converting to double to shut up compiler
        double tpart = dot/( normc1 * normn1 );
        long double theta = acos(tpart);

        // Quadrant correction
        if  (vertex[1] > centroid[1]){
                theta = (PI - theta) + PI;
        }

        long double an3 = theta ;
        return an3;
}

/*! \brief Visual Vector Tilt Angle Calculations
*/
float VisualVector::TiltAngle1Func(long double RotAngle,int pixelheight, int pixelwidth, const int centzerotilt[], double runit,double hunit)
{
        // cent represents the centroid
        // RotAngle is the rotation angle calculated with the RotationAngle code
        // pixelheight is the number of pixels high the WorkingImage is
        // pixelwidth is the number of pixel across th WorkingImage is
        // centzerotilt is the location of the centroid when the Whorl is untilted
        // runit is the distance in (meters/feet) from the center of Whorl I's
        //    n3 rotation to the center of the camera lens
        // hunit is the number of (meters/feet) from the center of Whorl I's
        //    rotation to the centroid of the triangle (theoretically a vertical
        //    distance

        // This code deals with the change in y position only

        // Changing coordinate frames
        double Xdiff = centroid[0] - centzerotilt[0];
        double Ydiff = centroid[1] - centzerotilt[1];

        double Yhyp = Ydiff / sin(RotAngle);
        double Yleg = Yhyp * cos(RotAngle);

        double Y_Nframe = (Xdiff - Yleg) * sin(RotAngle);
        double Xhyp = Xdiff - Yleg;
        double Xleg = Xhyp * cos(RotAngle);

        double X_Nframe = Yhyp + Xleg;

        // Correcting for Undefined Values
        if (sin(RotAngle) == 0){
              Y_Nframe = Ydiff;
              X_Nframe = Xdiff;
        }

        // Finding Distance Relative to N-Frame
        double intern1 = pow((pixelheight / 2 - centzerotilt[1]), 2.0);
        double intern2 = pow((pixelwidth / 2 - centzerotilt[0]), 2.0);
        double rpix = sqrt(intern1 + intern2);

        // Finding Tilt Angle
        double hpix = (rpix / runit) * hunit;
        float phiy = atan(Y_Nframe / hpix);
        float an1 = phiy ;
        return an1;
}

float VisualVector::TiltAngle2Func(long double RotAngle,int pixelheight, int pixelwidth, const int centzerotilt[], double runit, double hunit)
{
       // centx  represents the x coordinate of the centroid
        // RotAngle is the rotation angle calculated in RotationAngle code
        // pixelheight is the number of pixels high the WorkingImage is
        // pixelwidth is the number of pixel across th WorkingImage is
        // centzerotilt is the location of the centroid when the Whorl is at zero tilt
        // runit is the distance in (meters/feet) from the center of Whorl I's
        //    n3 rotation to the center of the camera lens
        // hunit is the number of (meters/feet) from the center of Whorl I's
        //    rotation to the centroid of the triangle (theoretically a vertical
        //    distance

        // This code deals with the change in x position only

        // Changing coordinate frames
        double Xdiff = centroid[0] - centzerotilt[0];
        double Ydiff = centroid[1] - centzerotilt[1];

        double Yhyp = Ydiff / sin(RotAngle);
        double Yleg = Yhyp * cos(RotAngle);

        double Y_Nframe = (Xdiff - Yleg) * sin(RotAngle);
        double Xhyp = Xdiff - Yleg;
        double Xleg = Xhyp * cos(RotAngle);

        double X_Nframe = Yhyp + Xleg;

        // Correcting for Undefined Values
        if (sin(RotAngle) == 0){
              Y_Nframe = Ydiff;
              X_Nframe = Xdiff;
        }
        
        // Finding Distances Relative to N-frame
        double intern1 = pow((pixelheight / 2 - centzerotilt[1]), 2.0);
        double intern2 = pow((pixelwidth / 2 - centzerotilt[0]), 2.0);
        double rpix = sqrt(intern1 + intern2);

        // Finding Tilt Angle
        double hpix = (rpix / runit) * hunit;
        float phix = atan(X_Nframe/hpix);
        float an2 = phix ;
        return an2;

}

//////////////////////////////////////////////////////////////////////////
// Header File, for reference                                           //
//////////////////////////////////////////////////////////////////////////
//
//-----------------------------------------------------------------------------------------------------------------------
//#ifndef __VISUAL_VECTOR_H__
//#define __VISUAL_VECTOR_H__
//
//#include <Base/Sensor.h>
//#include <matrix/Matrix.h>
//#include "dsacssinterface.h"
//
//class IplImage;
//
//using namespace O_SESSAME;
//
//class VisualVector : public Sensor
//{
//public:
//        // Contructors/Deconstructors
//        VisualVector();
//        virtual ~VisualVector();
//
//        // Facilitators
//
//        // Inspectors
//        Measurement GetMeasurement();
//        Vector GetVectorMeasurement();
//
//private:
//        int FindVertex(IplImage *WorkingImage, int vertex[],double centroid[]);
//        int VertexSwitch(int vertex[]);
//        
//        long double RotationAngleFunc(int v1[], double cent[], int pixelwidth, int pixelheight);
//        float TiltAngle1Func(double cent[], long double RotAngle,int pixheight, int pixwidth, int centzerotilt[], double runit,double hunit);
//        float TiltAngle2Func(double cent[], long double RotAngle,int pixheight, int pixwidth, int centzerotilt[], double runit, double hunit);
//
//
//};
//
//#endif
//---------------------------------------------------------------------------------------------------------------------------------------------

---