camera.cpp

机器人程序

// Camera.h - by Robin Hewitt, 2005-2005
// The initial version of this class was kindly
// contributed by Ed LeBouthillier, 2004
// http://home.earthlink.net/~apendragn
// This is free software. See license at the bottom
// of this file for terms of use.
//

//////////////////////////////////////////////////////////////
// Implementation of the Camera class
//

#include <stdio.h>
#include <math.h>
#include "Camera.h"
#include "../mavistypes.h"
#include "../Params.h"
#include "../Logger.h"


Camera::Camera(Mavis * pM)
{
	pMavis = pM;
	camera_X = 0.0;               // Left/right offset of camera from center of robot


	Logger * pLogger = pMavis->getLogger();


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

	isCalibrated = pParams->getIntValue("Camera", "calibrated", &isValid);
	if(!isValid)
	{
		isCalibrated = 0;
		pLogger->writelnToLog("\tCamera#calibrated paramter is missing from ini file.");
	}

	f = pParams->getFloatPtValue("Camera", "f", &isValid);
	if(!isValid)
	{
		f = 400.0;
		pLogger->writelnToLog("\tCamera#f paramter is missing from ini file.");
	}

	height = pParams->getFloatPtValue("Camera", "height", &isValid);
	if(!isValid) pLogger->writelnToLog("\tCamera#height paramter is missing from ini file.");

	tiltAngle = pParams->getFloatPtValue("Camera", "tiltAngle", &isValid);
	if(!isValid)
	{
		tiltAngle = -30;
		pLogger->writelnToLog("\tCamera#tiltAngle paramter is missing from ini file.");
	}
	sinTiltAngle = sin(tiltAngle * PI/180.0);
	cosTiltAngle = cos(tiltAngle * PI/180.0);

	fwdPosition = pParams->getFloatPtValue("Camera", "fwdPosition", &isValid);
	if(!isValid)
	{
		fwdPosition = 0;
		pLogger->writelnToLog("\tCamera#fwdPosition paramter is missing from ini file.");
	}

	// image width and height
	imgWidth  = (double)pMavis->getImgWidth();
	imgHeight = (double)pMavis->getImgHeight();
	halfImgHeight = imgHeight/2.0;
	halfImgWidth  = imgWidth/2.0;

	pLogger->writelnToLog(
		"\tCamera.f           = %.2f pixels\n"
		"\tCamera.height      = %.2f mm\n"
		"\tCamera.tiltAngle   = %.2f degrees\n"
		"\tCamera.fwdPosition = %.2f mm\n"
		"\tCamera.calibrated  = %d",
		f, height, tiltAngle, fwdPosition, isCalibrated
	);
}

Camera::~Camera()
{

}


void Camera::getCameraData(CameraData_t * pCameraData)
{
	pCameraData->f         = f;
	pCameraData->cameraHt  = height;
	pCameraData->tiltAngle = tiltAngle;
	pCameraData->cameraFwd = fwdPosition;
}


void Camera::setFocalLen(double newFocalLen)
{
	f = newFocalLen;
	pMavis->getParams()->setParam("Camera", "f", f);
	Logger * pLogger = pMavis->getLogger();
	pLogger->writelnToLog("Camera#f set to %.2f", f);
}

void Camera::setTiltAngle(double theta)
{
	tiltAngle = theta;
	pMavis->getParams()->setParam("Camera", "tiltAngle", tiltAngle);
	Logger * pLogger = pMavis->getLogger();
	pLogger->writelnToLog("Camera#tiltAngle set to %.2f", tiltAngle);
}

void Camera::setIsCalibrated(int newValue)
{
	isCalibrated = newValue;
	if(newValue) isCalibrated = 1;  // keep ini-file convention
	pMavis->getParams()->setParam("Camera", "calibrated", isCalibrated);
	Logger * pLogger = pMavis->getLogger();
	pLogger->writelnToLog("Camera#calibrated set to %d", isCalibrated);
}


WorldCoord Camera::screen2world(int xPix, int yPix)
{
	WorldCoord wc;

	double u = 0.5+ (double)(xPix) - halfImgWidth;
	double v = 0.5+ (double)(yPix) - halfImgHeight;

	double z  = height*(v*sinTiltAngle - f*cosTiltAngle);
	       z /= v*cosTiltAngle + f*sinTiltAngle;
	double x  = u*(z*cosTiltAngle - height*sinTiltAngle)/f;

	double d = sqrt(x*x + z*z);
	double phi = atan2(x,z) * 180.0 / PI;

	wc.x = x;
	wc.y = z;
	wc.z = 0;

	wc.distance = d + fwdPosition;
	wc.angle    = phi;

	return wc;
}


bool Camera::getScale(
	Pixel_t * pPix,
	double constraint,
	LocConstraint_t ct,
	double * pScale)
{
	double u = 0.5 + (double)(pPix->x) - halfImgWidth;
	double v = 0.5 + (double)(pPix->y) - halfImgHeight;
	double zc;

	*pScale = 0;
	if(!isCalibrated) return false;

	if(HEIGHT_CONSTRAINT == ct)
	{
		double yw = constraint - height;
		double k1 = yw * sinTiltAngle;
		double k2 = yw * cosTiltAngle;
		double denom = v*cosTiltAngle + f*sinTiltAngle;
		if( fabs(denom) < 1e-6 ) return false;
		double zw = (f*k2 - v*k1) / denom;
		zc = zw*cosTiltAngle + k1;
	}
	else
	{
		double xw = constraint - fwdPosition;
		//todo: add scale calculations for fwd distance constraint
		return false;
	}

	*pScale = f/zc;
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////
// 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, Ed LeBouthillier (http://home.earthlink.net/~apendragn) and
// 2004-2005, Robin Hewitt.
//
// 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.
///////////////////////////////////////////////////////////////////////////////////////