int main(int argc, const char * argv[]) {

    leastSquaresRegression();
    return 0;
}
예제 #2
0
/**************************************
 * Definition: Takes the perceived squares and performs a linear regression 
 *             on their locations of each side, and returns an error that is
 *             the difference in slopes of each side.
 *
 * Parameters: The color of the squares we're supposed to be looking at
 *
 * Returns:    An error in the interval [-1, 1], where 0 is no error,
 *             OR -999, which indicates a conclusion could not be reached
 *             A negative value is an indication to move right
 *             A positive value is an indication to move left
 **************************************/
float Camera::corridorSlopeError(int color, bool *turn, float *certainty) {
    bool hasSlopeRight = false;
    bool hasSlopeLeft = false;
    // find the center of the camera's image
    int width = thresholdedOf(color)->width;
    int center = width / 2;
    bool softLeftTurn = false;
    bool softRightTurn = false;
    *turn = false;

    float error = -999.0;

    // find a line of regression for each side of the image
    regressionLine leftSide = leastSquaresRegression(color, IMAGE_LEFT);
    regressionLine rightSide = leastSquaresRegression(color, IMAGE_RIGHT);
    regressionLine wholeImage = leastSquaresRegression(color, IMAGE_ALL);

    float xIntersect = 0;
    float yIntersect = 0;


    if (leftSide.numSquares >= 2 && rightSide.numSquares >= 2) {
       xIntersect = (rightSide.intercept - leftSide.intercept)/(leftSide.slope - rightSide.slope);
       yIntersect = leftSide.slope*xIntersect + leftSide.intercept;
    }
    else {
       xIntersect = -999;
       yIntersect = -999;
    }
    
    // draw the lines of regression so we can see them
    IplImage *bgr = getBGRImage();
    if (bgr != NULL) {
        CvPoint leftStart;
        CvPoint leftEnd;
        CvPoint rightStart;
        CvPoint rightEnd;
        leftStart.x = 0;
        leftStart.y = ((float)leftSide.slope) * 0 + leftSide.intercept;
        leftEnd.x = ((float)bgr->width / 2.0);
        leftEnd.y = ((float)leftSide.slope) * ((float)bgr->width / 2.0) + leftSide.intercept;
        rightStart.x = (float)bgr->width;
        rightStart.y = ((float)rightSide.slope) * ((float)bgr->width) + rightSide.intercept;
        rightEnd.x = ((float)bgr->width / 2.0);
        rightEnd.y = ((float)rightSide.slope) * ((float)bgr->width / 2.0) + rightSide.intercept;
        cvLine(bgr, leftStart, leftEnd, RED, 3, CV_AA, 0);
        cvLine(bgr, rightStart, rightEnd, GREEN, 3, CV_AA, 0);
        cvShowImage("Slopes", bgr);
        cvReleaseImage(&bgr);
    }

    if (wholeImage.numSquares == 2) {
        if(leftSide.numSquares == 1 && rightSide.numSquares == 1) {
            if(fabs(wholeImage.slope) > MAX_PLANE_SLOPE) {
                *certainty = 0.4 + (0.5 * fmin(1.0, 5*(fabs(wholeImage.slope)-MAX_PLANE_SLOPE))); //arbitrary certainty increase for larger slopes
            }
            if(wholeImage.slope > 0) {
                //turn right
                return -.4 + fmin(.3,.75*fabs(wholeImage.slope - MAX_PLANE_SLOPE));
            } 
            else {
                //turn left
                return .4 + fmin(.3,.75*fabs(wholeImage.slope - MAX_PLANE_SLOPE));
            }
        }
    }

    // did we find a line across the entire screen?
    if (wholeImage.numSquares >= 3) {
        if ((leftSide.numSquares < 2 || rightSide.numSquares < 2) && (leftSide.numSquares != 0 && rightSide.numSquares != 0)) { //No line on either side
            if (wholeImage.rSquared > .9) {
                //we can be fairly sure that we should make a turn move here
                *turn = true;
                *certainty = 1.0;
                //if positive slope, turn right and vice versa
                if (wholeImage.slope > 0) {
                    //turn right
                    return -1; 
                } 
                else {
                    //turn left
                    return 1;
                }
            }
        } 
    }

    //look for extra squares on either side 
    if (leftSide.numSquares >= 2) {
        if (leftSide.rSquared < .9 && !(rightSide.numSquares >= 2 && (fabs(rightSide.slope-leftSide.slope) < .15))) { //bad values up to .9?
            //turn left
            *turn = true;
            *certainty = 0.70;
            if (leftSide.slope < .14) {
                *certainty += 0.10;
            }

            if (rightSide.numSquares < 2) {
                *certainty += 0.10;
            }
 
            return .75; //less dramatic than above
        } 

        //If r-squared is good, then the extra squares are not of the right type!!
        //Unless we see nothing on the other side at all
        if (leftSide.slope < 0.14 && rightSide.numSquares == 0) {
            //turn left
            *turn = true;
            *certainty = 0.50;
 
            if (leftSide.slope < 0) {
                *certainty += 0.15;
            }

            return .75;
        }
    }

    if (rightSide.numSquares >= 2) {
        if (rightSide.rSquared < .9 && !(leftSide.numSquares >= 2 && (fabs(leftSide.slope-rightSide.slope) < .15))) {
            //turn right
            *turn = true;
            *certainty = 0.70;
 
            if (rightSide.slope > -.1) {
                *certainty += 0.10;
            }
 
            if (leftSide.numSquares < 2) {
                *certainty += 0.10;
            }

            return -.75;
        }
       
        
        if (rightSide.slope > -.1 && leftSide.numSquares == 0) {
            //turn right
            *turn = true;
            *certainty = 0.50;
            
            if (rightSide.slope > 0) {
                *certainty += 0.15;
            }
           
            return -.75;
        }
    } 
   
    //base certainty of turning vs. strafing on intersection data (when available)
    
    // did we have enough squares on each side to find a line?
    if (leftSide.numSquares >= 2 && rightSide.numSquares >= 2) { 
        float difference = leftSide.slope + rightSide.slope;
         
        if (difference > MAX_SLOPE_DIFFERENCE) {
            // the difference is large enough that we can say
            // the error is at its max, so we should move right
            *certainty = 0.70;
            return -1; //1 indicates full magnitude and SOME uncertainty
        } 
        else if (difference < -MAX_SLOPE_DIFFERENCE) {
            // we should move left
            *certainty = 0.70;
            return 1;
        } 
        else {
            if (fabs(difference) > MAX_SLOPE_DIFFERENCE*.75) {
                if (softLeftTurn && difference < 0) {
                    //turn left
                    *turn = true;
                    *certainty = 0.55;
                    return .4;
                }
                if (softRightTurn && difference > 0) {
                    //turn right
                    *turn = true;
                    *certainty = 0.55;
                    return -.4;
                }
            }
            // return the error in the range [-1, 1]
            // This should be pretty certain
            *turn = false;
            *certainty = 0.90;
            return -difference / MAX_SLOPE;
        } 
    }

    // we didn't have enough squares to be useful (no certainty)
    *certainty = 0.0;
    return -999.0;
}