motion.cpp

机器人程序

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

//////////////////////////////////////////////////////////////
// Implementation of Motion and MotionSighting classes
//

#include "Motion.h"
#include "Mavis.h"
#include "../Params.h"
#include "../camera/Camera.h"


//////////////////////////////////////////////////////////////
// MotionSighting class
//

MotionSighting::MotionSighting()
{

}

MotionSighting::~MotionSighting()
{

}


//////////////////////////////////////////////////////////////
// Motion class
//

bool   Motion::configParamsLoaded = false;
double Motion::pctIllumChange = 11.0;
int    Motion::nPixelsChanged = 100;
int    Motion::nSkippedFrames = 3;

//////////////////
//  constructor
//
Motion::Motion(Mavis * pM) : MVObjBase(pM)
{
	if(configParamsLoaded) return;


	// Load params
	Params * pParams = pM->getParams();
	bool isValid;

	pctIllumChange = pParams->getFloatPtValue("motionDetector", "pctIllumChange", &isValid);
	if(!isValid)
	{
		// set to default and save
		pctIllumChange = 10.0;
		pParams->setParam("motionDetector", "pctIllumChange", pctIllumChange);
	}

	nPixelsChanged = pParams->getIntValue("motionDetector", "nPixelsChanged", &isValid);
	if(!isValid)
	{
		// set to default and save
		nPixelsChanged = 100;
		pParams->setParam("motionDetector", "nPixelsChanged", nPixelsChanged);
	}

	nSkippedFrames = pParams->getIntValue("motionDetector", "nSkippedFrames", &isValid);
	if(!isValid)
	{
		// set to default and save
		nSkippedFrames = 100;
		pParams->setParam("motionDetector", "nSkippedFrames", nSkippedFrames);
	}

	Motion::configParamsLoaded = true;
}


//////////////////
//  destructor
//
Motion::~Motion()
{
	// free any Sighting objects
	MVUtils::deleteElements(&SightingsVector);
}


//////////////////
//  lookOnce()
//
int Motion::lookOnce(ObjLoc_t * pObjLoc)
{
	memset(pObjLoc, 0, sizeof(ObjLoc_t));

	int threshold = (int)(0.5 + pctIllumChange*2.55);
	int w = pMavis->getImgWidth();
	int h = pMavis->getImgHeight();

	VideoFrame * pFrame = 0;

	// get first frame
	pFrame = pMavis->getNextFrame();
	MVImg<int> * pImg1 = 0;
	MVImgUtils::frame2grayscaleImg(*pFrame, &pImg1);
	int * data1 = pImg1->getData();
	int nPixels = w*h;

	// get second frame
	for(int i=0; i<nSkippedFrames; i++)
		pFrame = pMavis->getNextFrame();
	MVImg<int> * pImg2 = 0;
	MVImgUtils::frame2grayscaleImg(*pFrame, &pImg2);
	int * data2 = pImg2->getData();

	// count number of changed pixels
	int iPx;
	int nChanged = 0;
	for(iPx=0; iPx<nPixels; iPx++)
	{
		int val = data2[iPx] - data1[iPx];
		if(val < 0) val = -val;
		if(val >= threshold)
		{
			val = 255;
			++nChanged;
		}
		else
			val = 0;


		data2[iPx] = val;
	}


	/////////////////////////////////////////////////////
	// Debug Section -- look at thresholded difference image
	//
	//pBytes = pMavis->nextFrame();
	/*
	int iBuf;
	int red, blue, green;

	for(iPx=0; iPx<nPixels; iPx++)
	{
		// rgbData uses 3 bytes per pixel
		iBuf = (iPx<<1) + iPx;

		red = blue = green = data2[iPx];


		pBytes[iBuf+RED_OFFSET]   = red;
		pBytes[iBuf+BLUE_OFFSET]  = blue;
		pBytes[iBuf+GREEN_OFFSET] = green;
	}
	*/
	//
	// End of Debug Section
	/////////////////////////////////////////////////////

	// if enough area has changed, it counts as motion,
	// so fill in the location data
	if(nChanged >= nPixelsChanged)
	{
		pObjLoc->prob = 100;
		int ** yxData2 = pImg2->getYXData();
		double  avgX = 0;
		double  avgY = 0;
		for(int y=0; y<h; y++)
		{
			for(int x=0; x<w; x++)
			{
				if( yxData2[y][x] )
				{
					avgX += x;
					avgY += y;
				}
			}
		}
		avgX = avgX / (double)nChanged;
		avgY = avgY / (double)nChanged;

		// Mark the image
		Pixel_t px;
		px.x = (int)(0.5 + avgX);
		px.y = (int)(0.5 + avgY);
		pMavis->markLoc(px, 0xff0000, 20, 20);

		// compute angle
		Camera * pCamera = pMavis->getCamera();
		WorldCoord wc = pCamera->screen2world( px.x, px.y );
		pObjLoc->angle = wc.angle;
		pObjLoc->dist = Motion::pctIllumChange;
		pObjLoc->nFound = 1;
	}

	if(pImg1) delete pImg1;
	if(pImg2) delete pImg2;

	return 0;
}

///////////////////////////////////////////////////////////////////////////////////////
// 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.
///////////////////////////////////////////////////////////////////////////////////////