calibrationhelper.cpp

机器人程序

// CalibrationHelper.cpp - by Robin Hewitt, 2004-2005
// http://www.robinhewitt.com/mavis
// This is free software. See license at the bottom
// of this file for terms of use.
//

// ****************************************************************
// todo: Need the option of saving a partial result. For example,
// once f and theta are computed, it makes sense to save these
// even if the next step fails. That's why steps 2-4 have been
// removed from the sequence. The circle wasn't reliably detected
// when it was that far away, and the entire calibration would fail.
// ****************************************************************


//////////////////////////////////////////////////////////////
// Implementation of CalibrationHelper class.
//

#include "CalibrationHelper.h"
#include "Mavis.h"
#include "../objRec/Homebase.h"
#include "../Params.h"
#include "Camera.h"
#include <math.h>
#include <string.h>


const int CalibrationHelper::INITIAL_EST_STEP     = 0;
const int CalibrationHelper::PIVOT_CENTER_STEP    = 1;
const int CalibrationHelper::TOP_CTR_STEP         = 21;  // temporarily removed these from
const int CalibrationHelper::BOTTOM_CTR_STEP      = 31;  // the calibration sequence....
const int CalibrationHelper::APPROACH_STEP        = 41;
const int CalibrationHelper::CALIBRATION_FINISHED = 2;
const int CalibrationHelper::ERROR_EXIT           = 3;

const string CalibrationHelper::INITIAL_EST_MSG =
	"Put circle on the floor, centered in the image.";

const string CalibrationHelper::TOP_CTR_MSG =
	"Move circle back. Align it with the crosshairs.";


//////////////////
//  FUNCTION:   threadproc()
//
DWORD WINAPI CalibrationHelper::threadproc( LPVOID lpThreadData )
{
	CalibrationHelper * pCh = (CalibrationHelper *)lpThreadData;

	pCh->runVideo();
	pCh->bThreadFinished = true;
	return 0;
}


//////////////////////////////////////////////////////////////
// CalibrationHelper
//

//////////////////
//  Constructor
//
CalibrationHelper::CalibrationHelper(Mavis * pMavis)
{
	bStop = bThreadFinished = true;
	pMv = pMavis;
	calibStep = INITIAL_EST_STEP;
	pHomeBase = new Homebase(pMavis);

	f = tiltAngle  = 0.0;
	fTop = fBottom = 0.0;

	imgH = pMv->getImgHeight();
	imgW = pMv->getImgWidth();
	crossHairsY = ((imgH+1) >>1) - 1;
	crossHairsX = ((imgW+1) >>1) - 1;

	// load camera params
	bool isValid;
	Params * pParams = pMavis->getParams();

	// todo: cameraZ -> height
	cameraZ = pParams->getFloatPtValue("Camera", "height", &isValid);
	if( !isValid || cameraZ <= 0 )
	{
		msg = "Missing or invalid ini-file value for camera.height.";
		goto error;
	}

	// todo: cameraY -> fwdPosition
	cameraY = pParams->getFloatPtValue("Camera", "fwdPosition", &isValid);
	if( !isValid )
	{
		// fwdPosition param is missing, so set it to the default value
		cameraY = 0.0;
		pParams->setParam("Camera", "fwdPosition", cameraY);
	}


	whiteDiam = pParams->getFloatPtValue("Homebase", "whiteDiam", &isValid);
	if( !isValid || whiteDiam <= 0 )
	{
		msg = "Missing or invalid ini-file value for Homebase.whiteDiam.";
		goto error;
	}

	prepareNextStep();
	return;

error:
	calibStep = ERROR_EXIT;
}


//////////////////
//  Destructor
//
CalibrationHelper::~CalibrationHelper()
{
	stop();
	if(hVideoThread) CloseHandle(hVideoThread);

	delete pHomeBase;

	// Save camera params if they've been reset successfully
	//if( calibStep > INITIAL_EST_STEP && calibStep < ERROR_EXIT )
	//{
		//Params * pParams = pMv->getParams();
		//pParams->setParam("Camera", "tiltAngle", tiltAngle);
		//pParams->setParam("Camera", "f", f);
	//}
}


//////////////////
//  FUNCTION:   prepareNextStep()
//
void CalibrationHelper::prepareNextStep()
{
	char buf[100];

	switch(calibStep)
	{
	case INITIAL_EST_STEP:
		msg = INITIAL_EST_MSG;
		// run video pass-thru thread
		hVideoThread = CreateThread(
			NULL,                           //default security attributes
			0,                              //default stack size
			CalibrationHelper::threadproc,  //thread function
			this,                           //the thread data
			0,                              //default creation flags
			NULL                            //thread id variable
		);
		break;

	case PIVOT_CENTER_STEP:
		buf[100];
		sprintf(buf, "f = %.2f, tiltAngle = %.2f", f, tiltAngle);
		msg = buf;
		break;

	case TOP_CTR_STEP:
		msg = TOP_CTR_MSG;
		crossHairsY = ( (3*(imgH+1)) >>2) - 1;
		hVideoThread = CreateThread(
			NULL,                           //default security attributes
			0,                              //default stack size
			CalibrationHelper::threadproc,  //thread function
			this,                           //the thread data
			0,                              //default creation flags
			NULL                            //thread id variable
		);
		break;

	case BOTTOM_CTR_STEP:
		buf[100];
		sprintf(buf, "BOTTOM_CTR_STEP");
		msg = buf;
		break;

	case ERROR_EXIT:
		break;

	default:
		break;
	}
}


//////////////////
//  FUNCTION:   nextStep()
//
bool CalibrationHelper::nextStep()
{
	switch(calibStep)
	{
	case INITIAL_EST_STEP:
	{
		stop();

		ObjSighting_t objSighting;
		pHomeBase->locatePrecisely(&objSighting);
		pMv->showFrame();
		if( !objSighting.prob )
		{
			msg = "Can\'t locate circle. Please check position and try again. ";
			msg += "You might also try changing the lighting conditions.";
			calibStep = ERROR_EXIT;
			return false;
		}

		// compute estimates for f and theta
		estimateFocalLenAndAngle(&objSighting);
		Camera * pCamera = pMv->getCamera();
		pCamera->setFocalLen(f);
		pCamera->setTiltAngle(tiltAngle);
		pCamera->setIsCalibrated();

		break;
	}

	case PIVOT_CENTER_STEP:
		break;

	case TOP_CTR_STEP:
		break;

	case BOTTOM_CTR_STEP:
		break;

	default:
		break;
	}

	++calibStep;
	prepareNextStep();

	return true;
}


//////////////////
//  FUNCTION:   estimateFocalLenAndAngle()
//
// There's no closed-form solution, so just step thru the possible elevation
// angles, compute the corresponding focal length, and chose the pair with
// the least error. Do this twice, second time in smaller increments of theta
// that are close to the original estimate.
//
// If recomputeF is false, hold f constant and recompute tiltAngle only.
//
void CalibrationHelper::estimateFocalLenAndAngle(ObjSighting_t * pObjSighting, bool recomputeF)
{
	double deg2rad = PI / 180.0;
	double radius = whiteDiam / 2.0;
	double testTheta;

	double leasterr = 10.0 * pObjSighting->height;
	if(!recomputeF)
		f = pMv->getCamera()->getFocalLen();

	for(testTheta=20; testTheta<91; testTheta++)
	{
		double angle = testTheta * deg2rad;
		double sintheta = sin(angle);
		double costheta = cos(angle);
		double cameradist = cameraZ / sintheta;
		double testF = recomputeF? cameradist * pObjSighting->width / whiteDiam : f;
		double floordist = cameradist * costheta;

		double zLo = floordist - radius;
		double zHi = floordist + radius;
		double vLo = testF * (zLo*sintheta - cameraZ*costheta) / (zLo*costheta + cameraZ*sintheta);
		double vHi = testF * (zHi*sintheta - cameraZ*costheta) / (zHi*costheta + cameraZ*sintheta);

		double ly = vHi - vLo;

		double err = pObjSighting->height - ly;
		if(err < 0) err = -err;
		if(err < leasterr)
		{
			leasterr = err;
			tiltAngle = -testTheta;
			f = testF;
		}
	}

	for(testTheta=-tiltAngle-1; testTheta<-tiltAngle+1; testTheta+=0.1)
	{
		double angle = testTheta * deg2rad;
		double sintheta = sin(angle);
		double costheta = cos(angle);
		double cameradist = cameraZ / sintheta;
		double testF = recomputeF? cameradist * pObjSighting->width / whiteDiam : f;
		double floordist = cameradist * costheta;

		double zLo = floordist - radius;
		double zHi = floordist + radius;
		double vLo = testF * (zLo*sintheta - cameraZ*costheta) / (zLo*costheta + cameraZ*sintheta);
		double vHi = testF * (zHi*sintheta - cameraZ*costheta) / (zHi*costheta + cameraZ*sintheta);

		double ly = vHi - vLo;

		double err = pObjSighting->height - ly;
		if(err < 0) err = -err;
		if(err < leasterr)
		{
			leasterr = err;
			tiltAngle = -testTheta;
			f = testF;
		}
	}
}


//////////////////
//  FUNCTION:   reestimateFocalLen()
//
// When the homebase circle is closer or further than the centerpoint of
// the image frame, the values of focal length and tiltAngle may
// no longer be accurate enough. This method reestimates focal length
// assuming a given (predetermined) camera angle. The circle object will
// presumably be displaced from center - either closer or further away.
//
double CalibrationHelper::reestimateFocalLen(ObjSighting_t * pObjSighting, double theta)
{
	double newF = 0.0;
	double deg2rad = PI / 180.0;
	double hc = cameraZ;                         // height of camera.
	double v = (imgH/2) - (0.5+pObjSighting->y); // v-coord in image. pObjSighting->y is based
	                                             // on pixel indices, so add 0.5 to shift it up
	                                             // by 1/2 to align w/pixel center,

	double du = pObjSighting->width;             // width of homebase circle in image.
	double costheta = cos(theta*deg2rad);
	double sintheta = sin(theta*deg2rad);
	double Y = whiteDiam * v/du;                 // Y axis is parallel to image plane
	double x = (hc*costheta - Y)/sintheta;       // x axis is parallel to the floor
	double Z = x*costheta + hc*sintheta;         // Z axis is perpendicular to the image plane

	newF = du*Z/whiteDiam;

	return newF;
}

//////////////////
//  FUNCTION:   runVideo()
//
void CalibrationHelper::runVideo()
{
	bStop = bThreadFinished = false;
	while( !bStop ) {
		//pBmpBuffer = pMv->nextFrame();
		VideoFrame * pFrame = pMv->getNextFrame();
		pBmpBuffer = pFrame->getData();
		if(bStop) break;
		addCrosshairs();
		if(bStop) break;
		pMv->showFrame();
	}
}


//////////////////
//  FUNCTION:   stop()
//
void CalibrationHelper::stop()
{
	bStop = true;
	while( !bThreadFinished ) Sleep(100L);

	pMv->showFrame();
}


//////////////////
//  FUNCTION:   addCrosshairs()
//
void CalibrationHelper::addCrosshairs()
{
	int x1, y1, x2, y2;
	int h = pMv->getImgHeight();
	int w = pMv->getImgWidth();
	int rgb = 0x00ff0000;

	// horizontal line
	x1 = 0; x2 = w-1;
	y1 = y2 = crossHairsY;
	DrawLine(x1, y1, x2, y2, rgb);

	// vertical line
	y1 = 0; y2 = h-1;
	x1 = x2 = crossHairsX;
	DrawLine(x1, y1, x2, y2, rgb);
}

//////////////////
//  FUNCTION:   DrawLine()
//
void CalibrationHelper::DrawLine(int x1, int y1, int x2, int y2, int rgb)
{
	int delX = x2 - x1;
	int delY = y2 - y1;
	bool bXofY = abs(delY) > abs(delX);

	int iPx, iBuf;
	int w = pMv->getImgWidth();

	double m, b;
	if(bXofY)
	{
		if(y1 > y2)
		{
			int tmp = y1;
			y1 = y2;
			y2 = tmp;
			delX = -delX;
			delY = -delY;
		}
		m = (double)delX/(double)delY;
		b = (double)x1 - m*(double)y1;
		for(int y=y1; y<=y2; y++)
		{
			int x = int(0.5 + m*(double)y + b);
			//ImgUtil::yx2i(w, x, y, &iPx);
			iPx = MVImgUtils::xy2iPx(w,x,y);
			//ImgUtil::i2iBuf(iPx, &iBuf);
			iBuf = MVImgUtils::iPx2iBuf(iPx);
			pBmpBuffer[iBuf+1] = (BYTE)(0xff & rgb);            // blue
			pBmpBuffer[iBuf]   = (BYTE)((0xff00 & rgb) >>8);    // green
			pBmpBuffer[iBuf+2] = (BYTE)((0xff0000 & rgb) >>16); // red
		}
	}
	else
	{
		if(x1 > x2)
		{
			int tmp = x1;
			x1 = x2;
			x2 = tmp;
			delX = -delX;
			delY = -delY;
		}
		m = (double)delY/(double)delX;
		b = (double)y1 - m*(double)x1;

		for(int x=x1; x<=x2; x++)
		{
			int y = int(0.5 + m*(double)x + b);
			//ImgUtil::yx2i(w, x, y, &iPx);
			iPx = MVImgUtils::xy2iPx(w,x,y);
			//ImgUtil::i2iBuf(iPx, &iBuf);
			iBuf = MVImgUtils::iPx2iBuf(iPx);
			pBmpBuffer[iBuf+1] = (BYTE)( 0x000000ff & rgb);        // blue
			pBmpBuffer[iBuf]   = (BYTE)((0x0000ff00 & rgb) >>8);   // green
			pBmpBuffer[iBuf+2] = (BYTE)((0x00ff0000 & rgb) >>16);  // red
		}
	}
}

///////////////////////////////////////////////////////////////////////////////////////
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this
// license. If you do not agree to this license, do not download, install, copy or
// use the software.
//
//
//                        Mavis License Agreement
//
// Copyright (c) 2004-2005, Robin Hewitt (http://www.robin-hewitt.com).
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistributions in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
// This software is provided "as is" and any express or implied warranties, including,
// but not limited to, the implied warranties of merchantability and fitness for a
// particular purpose are disclaimed. In no event shall the authors or contributors be
// liable for any direct, indirect, incidental, special, exemplary, or consequential
// damages (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused and on any
// theory of liability, whether in contract, strict liability, or tort (including
// negligence or otherwise) arising in any way out of the use of this software, even
// if advised of the possibility of such damage.
///////////////////////////////////////////////////////////////////////////////////////