int main(int argc, const char * argv[]) {
leastSquaresRegression();
return 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;
}
