[Robotflow-cvs] RobotFlow/Vision/src ColorHistExtraction.cc,NONE,1.1 MeanShiftTracker.cc,NONE,1.1 VisualFeatureDesc.cc,NONE,1.1 VisualHistogramDesc.cc,NONE,1.1 VisualROI.cc,NONE,1.1 VisualTarget.cc,NONE,1.1 VisualTargetManager.cc,NONE,1.1 Makefile.am,1.29,1.30

[Pierre M. <sid...@us...>]

Update of /cvsroot/robotflow/RobotFlow/Vision/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6253

Modified Files:
	Makefile.am 
Added Files:
	ColorHistExtraction.cc MeanShiftTracker.cc 
	VisualFeatureDesc.cc VisualHistogramDesc.cc VisualROI.cc 
	VisualTarget.cc VisualTargetManager.cc 
Log Message:
Added OpenCV depend files

--- NEW FILE: VisualFeatureDesc.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "VisualFeatureDesc.h"
#include "operators.h"
#include "conversion.h"
#include "vmethod.h"
#include "Vector.h"

//
// Declaring known types
//
DECLARE_TYPE(VisualFeatureDesc<double>)

typedef VisualFeatureDesc<double> VisualDoubleFeatureDesc;
typedef Vector<VisualFeatureDesc<double> *> VisualDoubleFeatureDescVec;

//
// Adding operators
//


// Concat operator using two VisualFeatureDesc inputs
template <class X, class Y, class Z>
ObjectRef concatVFDFunction(ObjectRef op1, ObjectRef op2) {
	RCPtr<X> op1Value = op1;
	RCPtr<Y> op2Value = op2;
	
	//creating output vector
	RCPtr<Z> resultValue = RCPtr<Z>(Z::alloc(2));
	
	//concat 2 values into a vector
	(*resultValue)[0] =  &(*op1Value);
	(*resultValue)[1] =  &(*op2Value);
	
	return resultValue;
}

REGISTER_DOUBLE_VTABLE_TEMPLATE(
	concatVtable,
	concatVFDFunction,
	VisualFeatureDesc<double>,
	VisualFeatureDesc<double>,
	Vector<VisualFeatureDesc<double> *>,
	0)

// Concat operator using two Vector<VisualFeatureDesc *> inputs
template <class X, class Y, class Z>
ObjectRef concatVFDVecVecFunction(ObjectRef op1, ObjectRef op2) {
	RCPtr<X> op1Value = op1;
	RCPtr<Y> op2Value = op2;
	
	RCPtr<Z> resultValue(Z::alloc(op1Value->size() + op2Value->size()));
	
	//copy first part
	for (int i =0; i < op1Value->size(); i++) {
		(*resultValue)[i] = (*op1Value)[i];
	}
	
	//copy last part
	for (int i =0; i < op2Value->size(); i++) {
		(*resultValue)[i + op1Value->size()] = (*op2Value)[i];
	}
	
	return resultValue;
}
REGISTER_DOUBLE_VTABLE_TEMPLATE(
	concatVtable,
	concatVFDVecVecFunction,
	Vector<VisualFeatureDesc<double> *>,
	Vector<VisualFeatureDesc<double> *>,
	Vector<VisualFeatureDesc<double> *>,
	0)

// Concat operator using a Vector<VisualFeatureDesc *> input
// followed by a VisualFeatureDesc input
template<class X, class Y, class Z>
ObjectRef concatVectorVFDFunction(ObjectRef op1, ObjectRef op2) {
	RCPtr<X> op1Value = op1;
	RCPtr<Y> op2Value = op2;
	
	//creating new vector
	RCPtr<Z> resultValue(Z::alloc(op1Value->size() + 1));
	
	//copying values from vector
	for (int i = 0; i < resultValue->size(); i++) {    
		(*resultValue)[i] = (*op1Value)[i];
	}
	
	//adding last element
	(*resultValue)[resultValue->size() - 1] = &(*op2Value);
	
	return resultValue;
}
REGISTER_DOUBLE_VTABLE_TEMPLATE(
	concatVtable,
	concatVectorVFDFunction,
	Vector<VisualFeatureDesc<double> *>,
	VisualFeatureDesc<double>,
	Vector<VisualFeatureDesc<double> *>,
	0)

// Concat operator using a VisualFeatureDesc input
// followed by a Vector<VisualFeatureDesc *> input
template<class X, class Y, class Z>
ObjectRef concatVFDVectorFunction(ObjectRef op1, ObjectRef op2) {
	RCPtr<X> op1Value = op1;
	RCPtr<Y> op2Value = op2;
	
	//creating new vector
	RCPtr<Z> resultValue(Z::alloc(op2Value->size() + 1));
	
	//copying values from vector
	for (int i = 1; i < resultValue->size(); i++) {    
		(*resultValue)[i] = (*op2Value)[i - 1];
	}
	
	//adding first element
	(*resultValue)[0] = &(*op1Value);
	
	return resultValue;
}
REGISTER_DOUBLE_VTABLE_TEMPLATE(
	concatVtable,
	concatVFDVectorFunction,
	VisualFeatureDesc<double>,
	Vector<VisualFeatureDesc<double> *>,
	Vector<VisualFeatureDesc<double> *>,
	0)
	
// ToVect conversion 
template <class T, class U>
ObjectRef VisualFeatDesc2VectorConversion (ObjectRef in) {
	RCPtr<T> FromVFD = in;
	RCPtr<U> ToVector(U::alloc(1));
	(*ToVector)[0] = &(*FromVFD);
	return ToVector;
}

REGISTER_CONVERSION_TEMPLATE(
	VisualFeatureDesc<double>, 
	Vector<VisualFeatureDesc<double> *>, 
	VisualFeatDesc2VectorConversion);


static int dummy_template_vtable_init_for_VFDdouble_to_vector_VFDdouble = \
  vmethod()->registerFunct0(VisualFeatDesc2VectorConversion<VisualFeatureDesc<double>,Vector<VisualFeatureDesc<double> *> >,&typeid(VisualDoubleFeatureDesc),"toVect");

//REGISTER_VTABLE0(toVect, VisualFeatureDesc<double>, VisualFeatDesc2VectorConversion<VisualFeatureDesc<double>,Vector<VisualFeatureDesc<double> *> >, 6);


--- NEW FILE: VisualTarget.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "VisualTarget.h"

DECLARE_TYPE(VisualTarget<double>)



--- NEW FILE: ColorHistExtraction.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#include "ColorHistExtraction.h"

DECLARE_NODE(ColorHistExtraction)
DECLARE_TYPE(ColorHistExtraction)

  /*Node
   *
   * @name ColorHistExtraction
   * @category RobotFlow:Vision:FeaturesExtraction
   * @description Color histogram features extraction.
   *
   * @parameter_name FRAME_WIDTH
   * @parameter_type int
   * @parameter_value 320
   * @parameter_description Video frame width.
   *
   * @parameter_name FRAME_HEIGHT
   * @parameter_type int
   * @parameter_value 240
   * @parameter_description Video frame height.
   *
   * @parameter_name NUM_CHANNELS
   * @parameter_type int
   * @parameter_value 3
   * @parameter_description Number of channels in video frame.
   *
   * @input_name IN_IMAGE
   * @input_type Image
   * @input_description Current frame to process.
   *
   * @input_name NUM_BINS
   * @input_type Vector<int>
   * @input_description Number of bins for each dimension of the histogram.
   *
   * @input_name CURRENT_ROI
   * @input_type VisualROI
   * @input_description The current region of interest.
   *
   * @input_name CURRENT_TARGET
   * @input_type VisualTarget
   * @input_description The current target to estimate the mean shif location.
   *
   * @input_name TARGET_DESC_IDX
   * @input_type int
   * @input_description The corresponding descriptor index in the current target descriptors vector.
   *
   * @input_name PREPROCESS_FRAME
   * @input_type bool
   * @input_description Flag indicating to preprocess a new image.
   *
   * @output_name OUT_FEATURES
   * @output_type Vector<VisualFeatureDesc<double> *>
   * @output_description Output features descriptor.
   *
   * @output_name OUT_MS_LOCATION
   * @output_type Vector<double>
   * @output_description Mean shift vector of location.
   *
   * @output_name PREPROCESS_COMPLETED
   * @output_type int
   * @output_description Output to force preprocessing.
   *
   END*/

   
//
// Default constructor for Object 
//
ColorHistExtraction::ColorHistExtraction()
: m_width(-1),
m_height(-1),
m_numChannels(-1),
m_init(false),
m_roi(NULL),
m_colorHistogram(NULL)
{

}

//
// Constructor with complete intialisation
//
ColorHistExtraction::ColorHistExtraction(int i_width, int i_height, 
	int i_numChannels, const Vector<int> *i_numBins)
: m_width(i_width),
m_height(i_height),
m_numChannels(i_numChannels),
m_init(false),
m_roi(NULL),
m_colorHistogram(NULL)
{
	Initialize(i_numBins);
}

//
// Copy constructor
//
ColorHistExtraction::ColorHistExtraction(const ColorHistExtraction& i_ref)
{
	throw new GeneralException("Exception in ColorHistExtraction::ColorHistExtraction: copy constructor not yet implemented.",__FILE__,__LINE__);
}

//
// BufferedNode constructor
//
ColorHistExtraction::ColorHistExtraction(string nodeName, ParameterSet params)
: VisualFeaturesExtraction<double>(nodeName, params),
m_init(false),
m_roi(NULL),
m_colorHistogram(NULL)
{
	m_imageInID = addInput("IN_IMAGE");
	m_numBinsInID = addInput("NUM_BINS");
	m_roiInID = addInput("CURRENT_ROI");
	m_targetInID = addInput("CURRENT_TARGET");
	m_targetDescIdxInID = addInput("TARGET_DESC_IDX");
	m_useNextImgInID = addInput("PREPROCESS_FRAME");
	
	m_featuresOutID = addOutput("OUT_FEATURES");
	m_msLocOutID = addOutput("OUT_MS_LOCATION");
	m_ppCompletedOutID = addOutput("PREPROCESS_COMPLETED");

	m_width = dereference_cast<int>(parameters.get("FRAME_WIDTH"));
	m_height = dereference_cast<int>(parameters.get("FRAME_HEIGHT"));
	m_numChannels = dereference_cast<int>(parameters.get("NUM_CHANNELS"));
}
	
ColorHistExtraction::~ColorHistExtraction()
{
	delete m_roi;
	delete m_colorHistogram;
	
	cvReleaseImage(&m_curImage);
}

// Modified BufferedNode request method to support cyclic node connection
void ColorHistExtraction::request(int output_id, const ParameterSet &req) 
{
	if (req.exist("LOOKAHEAD")) {
		outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD")));
	}
	
	if (req.exist("LOOKBACK")) {
		outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK")));
	}
	
	if (req.exist("INORDER")) {
		inOrder = true;
	}
	
	int outputLookAhead=0, outputLookBack=0;

	outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead);
	outputLookBack =max(outputLookBack, outputs[output_id].lookBack);
	
	// Every output usually requires these inputs
	ParameterSet nextImgReq, numBinsReq;
	nextImgReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_useNextImgInID].lookAhead+outputLookAhead)));
	nextImgReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_useNextImgInID].lookBack+outputLookBack)));
	inputs[m_useNextImgInID].node->request(inputs[m_useNextImgInID].outputID,nextImgReq);
	
	numBinsReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_numBinsInID].lookAhead+outputLookAhead)));
	numBinsReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_numBinsInID].lookBack+outputLookBack)));
	inputs[m_numBinsInID].node->request(inputs[m_numBinsInID].outputID,numBinsReq);
	
	if (output_id == m_featuresOutID) {
		ParameterSet myReq;
		myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookAhead+outputLookAhead)));
		myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookBack+outputLookBack)));
		inputs[m_roiInID].node->request(inputs[m_roiInID].outputID,myReq);
	}
	else if (output_id == m_msLocOutID) {
		ParameterSet myReq, myReq2;
		myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetDescIdxInID].lookAhead+outputLookAhead)));
		myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetDescIdxInID].lookBack+outputLookBack)));
		inputs[m_targetDescIdxInID].node->request(inputs[m_targetDescIdxInID].outputID,myReq);
		
		myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookAhead+outputLookAhead)));
		myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookBack+outputLookBack)));
		inputs[m_targetInID].node->request(inputs[m_targetInID].outputID,myReq2);
	}
	else if (output_id == m_ppCompletedOutID) {
		ParameterSet myReq;
		myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookAhead+outputLookAhead)));
		myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookBack+outputLookBack)));
		inputs[m_imageInID].node->request(inputs[m_imageInID].outputID, myReq);
	}
	else {
		throw new GeneralException ("ColorHistExtraction::request : unknown output ID.",__FILE__,__LINE__);
	}

}

void ColorHistExtraction::calculate(int output_id, int count, Buffer &out)
{
	try {
		if (!m_init) {
			RCPtr<Vector<int> > numBinsRef = getInput(m_numBinsInID, count);
			Initialize(&(*numBinsRef));
		}
		
		bool useNext = dereference_cast<bool>(getInput(m_useNextImgInID, count));
		
		if (useNext) {
			RCPtr<Image> imageRef = getInput(m_imageInID, count);
		
			// Verify input image sanity
			if (imageRef->get_width() != m_width ||
				imageRef->get_height() != m_height ||
				imageRef->get_pixelsize() != m_numChannels) {
				throw new GeneralException ("ColorHistExtraction::calculate : image parameters do not correspond to given input.",__FILE__,__LINE__);
			}
			
			// Copy input image
			memcpy(m_curImage->imageData, imageRef->get_data(), m_numBytesInFrame);
		}
		
		if (output_id == m_featuresOutID) {
			ObjectRef roiRef = getInput(m_roiInID, count);
			
			if (!roiRef->isNil()) {
				ExtractFeatures(&(object_cast<VisualROI>(roiRef)));
				
				/*
				VisualHistogramDesc<double, unsigned char> *outFeatures = 
					new VisualHistogramDesc<double, unsigned char>(*m_colorHistogram);
				*/
				Vector<VisualFeatureDesc<double> *> *outVec = Vector<VisualFeatureDesc<double> *>::alloc(1);
				(*outVec)[0] = new VisualHistogramDesc<double, unsigned char>(*m_colorHistogram);
				
				(*outputs[m_featuresOutID].buffer)[count] = ObjectRef(outVec);
			}
			else {
				(*outputs[m_featuresOutID].buffer)[count] = ObjectRef(nilObject);
			}
		}
		else if (output_id == m_msLocOutID) {
			// X,Y location
			Vector<double> *outMSLoc = new Vector<double>(2);
			
			ObjectRef targetRef = getInput(m_targetInID, count);
			
			if (!targetRef->isNil()) {
				// Get descriptor index to use
				int descIdx = dereference_cast<int>(getInput(m_targetDescIdxInID, count));
				
				EstimateMSLocation(&(object_cast<VisualTarget<double> >(targetRef)), descIdx, outMSLoc);
				
				(*outputs[m_msLocOutID].buffer)[count] = ObjectRef(outMSLoc);
			}
			else {
				(*outputs[m_msLocOutID].buffer)[count] = ObjectRef(nilObject);
			}
		}
		else if (output_id == m_ppCompletedOutID) {
			// Preprocess image than output true
			RCPtr<Image> imageRef = getInput(m_imageInID, count);
		
			// Verify input image sanity
			if (imageRef->get_width() != m_width ||
				imageRef->get_height() != m_height ||
				imageRef->get_pixelsize() != m_numChannels) {
				throw new GeneralException ("ColorHistExtraction::calculate : image parameters do not correspond to given input.",__FILE__,__LINE__);
			}
			
			// Copy input image
			memcpy(m_curImage->imageData, imageRef->get_data(), m_numBytesInFrame);
			(*outputs[m_ppCompletedOutID].buffer)[count] = ObjectRef(Int::alloc(1));
		}
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in ColorHistExtraction::calculate:",__FILE__,__LINE__));
	}
}

void ColorHistExtraction::ExtractFeatures(VisualROI *i_roi)
{
	try {
		m_colorHistogram->ComputeKernelWeightedHist((const unsigned char *)m_curImage->imageData, 
			255, 1.0, m_width, m_height, i_roi);
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in ColorHistExtraction::ExtractFeatures:",__FILE__,__LINE__));
	}
}

void ColorHistExtraction::ExtractFeatures(IplImage *i_input, VisualROI *i_roi)
{
	try {
		m_colorHistogram->ComputeKernelWeightedHist((const unsigned char *)i_input->imageData, 
			255, 1.0, m_width, m_height, i_roi);
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in ColorHistExtraction::ExtractFeatures:",__FILE__,__LINE__));
	}
}

void ColorHistExtraction::EstimateMSLocation(const VisualTarget<double> *i_targetRef, 
	int i_descIdx, Vector<double> *o_msLocVec)
{
	try {
		const double *refModelBins = i_targetRef->GetCstDescriptor(i_descIdx)->GetCstFeatures();
		const VisualROI *curROI = i_targetRef->GetCstROI();
		double cueWeight = i_targetRef->GetCueWeight(i_descIdx);
		
		// Extract candidate features histogram
		m_colorHistogram->ComputeKernelWeightedHist((const unsigned char *)m_curImage->imageData, 
			255, 1.0, m_width, m_height, curROI);
		
		// Evaluate mean shift location
		m_colorHistogram->ComputeMSLocation((const unsigned char *)m_curImage->imageData, 
			255, 1.0, m_width, m_height, curROI, refModelBins, cueWeight, o_msLocVec);
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in ColorHistExtraction::EstimateMSLocation:",__FILE__,__LINE__));
	}
}

//
// Private mehtods
//

void ColorHistExtraction::Initialize(const Vector<int> *i_numBins)
{
	m_numPixels = m_width*m_height;
	m_numBytesInFrame = m_numPixels*3;
	
	CvSize imgSize;
	imgSize.width = m_width;
	imgSize.height = m_height;
	m_curImage = cvCreateImage(imgSize, IPL_DEPTH_8U, m_numChannels);
	
	m_colorHistogram = new VisualHistogramDesc<double, unsigned char>(e_VISUALHIST_BhattacharyyaCoeff, 
		true, m_numChannels, i_numBins);
	m_roi = new VisualROI();
	
	m_init = true;
}

--- NEW FILE: VisualTargetManager.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...)

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#include "VisualTargetManager.h"

DECLARE_NODE(VisualTargetManager)
DECLARE_TYPE(VisualTargetManager)

  /*Node
   *
   * @name VisualTargetManager
   * @category RobotFlow:Vision:Tracking
   * @description Target manager to handle target initialization, adaptation and deletion.
   *
   * @parameter_name FRAME_WIDTH
   * @parameter_type int
   * @parameter_value 320
   * @parameter_description Video frame width.
   *
   * @parameter_name FRAME_HEIGHT
   * @parameter_type int
   * @parameter_value 240
   * @parameter_description Video frame height.
   *
   * @parameter_name NUM_CHANNELS
   * @parameter_type int
   * @parameter_value 3
   * @parameter_description Number of channels in video frame.
   *
   * @parameter_name MAX_NUM_TARGET
   * @parameter_type int
   * @parameter_value 10
   * @parameter_description Maximum number of target to keep track of.
   *
   * @parameter_name TARGET_MATCH_THRES
   * @parameter_type float
   * @parameter_value 0.6
   * @parameter_description Minimum confidence to assume a target match.
   *
   * @parameter_name TARGET_ADAPT_THRES
   * @parameter_type float
   * @parameter_value 0.9
   * @parameter_description Adaptation treshold for each target cue model.
   *
   * @parameter_name TARGET_ADAPT_RATE
   * @parameter_type float
   * @parameter_value 0.1
   * @parameter_description Adaptation rate for each target cue model.
   *
   * @parameter_name CUE_ADAPT_RATE
   * @parameter_type float
   * @parameter_value 0.2
   * @parameter_description Adaptation rate for each cue weight.
   *
   * @input_name IMAGE_IN
   * @input_type Image
   * @input_description Current video frame.
   *
   * @input_name PREPROCESS_COMPLETED
   * @input_type Vector<int>
   * @input_description Flags indicating the completion of the features extraction preprocessing stage.
   *
   * @input_name DETECTED_ROI
   * @input_type VisualROI
   * @input_description A region of interest detected as a potential target region.
   *
   * @input_name FEATURES_VECTOR
   * @input_type Vector<VisualFeatureDesc<double> *>
   * @input_description Features descriptors vector (probably requested to adapt or init a target)
   *
   * @input_name TRACKED_TARGET
   * @input_type VisualTarget<double>
   * @input_description An updated target by a tracking routine.
   *
   * @output_name IMAGE_OUT
   * @output_type Image
   * @output_description Current image with identified target.
   *
   * @output_name CURRENT_ROI
   * @output_type VisualROI
   * @output_description Current region of interest to extract features.
   *
   * @output_name CURRENT_TARGET
   * @output_type VisualTarget<double>
   * @output_description Current target to track.
   *
   * @output_name TARGET_PROB
   * @output_type double
   * @output_description Current target likelihood at current tracked position.
   *
   * @output_name TARGET_DELTA_X
   * @output_type float
   * @output_description Current target position relative to the image x center.
   *
   * @output_name TARGET_DELTA_Y
   * @output_type float
   * @output_description Current target position relative to the image y center.
   *
   * @output_name FRAME_NAME
   * @output_type string
   * @output_description Current frame name for saving purposes.
   *
   END*/

//
// Default constructor for Object 
//
VisualTargetManager::VisualTargetManager()
: m_target(NULL)
{

}

//
// BufferedNode constructor
//
VisualTargetManager::VisualTargetManager(std::string nodeName, ParameterSet params)
: BufferedNode(nodeName, params), 
m_target(NULL)
{
	m_imageInID = addInput("IMAGE_IN");
	m_ppCompletedInID = addInput("PREPROCESS_COMPLETED");
	m_roiInID = addInput("DETECTED_ROI");
	m_featVecInID = addInput("FEATURES_VECTOR");
	m_targetInID = addInput("TRACKED_TARGET");

	m_imageOutID = addOutput("IMAGE_OUT");
	m_roiOutID = addOutput("CURRENT_ROI");
	m_targetOutID = addOutput("CURRENT_TARGET");
	m_targetProbOutID = addOutput("TARGET_PROB");
	m_targetDXOutID = addOutput("TARGET_DELTA_X");
	m_targetDYOutID = addOutput("TARGET_DELTA_Y");
	m_nameOutID = addOutput("FRAME_NAME");

	m_width = dereference_cast<int>(parameters.get("FRAME_WIDTH"));
	m_height = dereference_cast<int>(parameters.get("FRAME_HEIGHT"));
	m_numChannels = dereference_cast<int>(parameters.get("NUM_CHANNELS"));
	m_maxNumTargets = dereference_cast<int>(parameters.get("MAX_NUM_TARGET"));
	m_targetMatchThres = dereference_cast<float>(parameters.get("TARGET_MATCH_THRES"));
	m_targetAdaptThres = dereference_cast<float>(parameters.get("TARGET_ADAPT_THRES"));
	m_targetAdaptRate = dereference_cast<float>(parameters.get("TARGET_ADAPT_RATE"));
	m_cueAdaptRate = dereference_cast<float>(parameters.get("CUE_ADAPT_RATE"));
	
	m_numPixels = m_width*m_height;
	m_numBytesInFrame = m_numPixels*m_numChannels;
	m_imgXCen = (float)(m_width)*0.5f;
	m_imgYCen = (float)(m_height)*0.25f;
	
	CvSize imgSize;
	imgSize.width = m_width;
	imgSize.height = m_height;
	
	m_curImage = cvCreateImage(imgSize, IPL_DEPTH_8U, m_numChannels);
	
	m_target = new VisualTarget<double>();
	m_refTarget = RCPtr<VisualTarget<double> >(m_target);
	
	m_roiColor[0] = 255;
	m_roiColor[1] = 0;
	m_roiColor[2] = 0;
}
	
VisualTargetManager::~VisualTargetManager()
{
	delete m_target;
	cvReleaseImage(&m_curImage);
}

// Modified BufferedNode request method to support cyclic node connection
void VisualTargetManager::request(int output_id, const ParameterSet &req) 
{
	if (req.exist("LOOKAHEAD")) {
		outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD")));
	}
	
	if (req.exist("LOOKBACK")) {
		outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK")));
	}
	
	if (req.exist("INORDER")) {
		inOrder = true;
	}
	
	int outputLookAhead=0, outputLookBack=0;

	outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead);
	outputLookBack =max(outputLookBack, outputs[output_id].lookBack);
	
	if (output_id == m_targetProbOutID) {
		ParameterSet myReq, myReq2, myReq3, myReq4;
		
		myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookAhead+outputLookAhead)));
		myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookBack+outputLookBack)));
		inputs[m_roiInID].node->request(inputs[m_roiInID].outputID,myReq);
		
		myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookAhead+outputLookAhead)));
		myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookBack+outputLookBack)));
		inputs[m_ppCompletedInID].node->request(inputs[m_ppCompletedInID].outputID,myReq2);
		
		myReq3.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookAhead+outputLookAhead)));
		myReq3.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookBack+outputLookBack)));
		inputs[m_targetInID].node->request(inputs[m_targetInID].outputID,myReq3);
		
		myReq4.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_featVecInID].lookAhead+outputLookAhead)));
		myReq4.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_featVecInID].lookBack+outputLookBack)));
		inputs[m_featVecInID].node->request(inputs[m_featVecInID].outputID,myReq4);
	}
	else if (output_id == m_targetOutID) {
		// CURRENT_TARGET output does not required any inputs
		return;
	}
	else if (output_id == m_roiOutID) {
		// CURRENT_ROI output does not required any inputs
		return;
	}
	else if (output_id == m_imageOutID) {
		ParameterSet myReq;
		
		myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookAhead+outputLookAhead)));
		myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookBack+outputLookBack)));
		inputs[m_imageInID].node->request(inputs[m_imageInID].outputID, myReq);
	}
	else if (output_id == m_targetDXOutID) {
		// TARGET_DELTA_X output does not required any inputs
		return;
	}
	else if (output_id == m_targetDYOutID) {
		// TARGET_DELTA_Y output does not required any inputs
		return;
	}
	else if (output_id == m_nameOutID) {
		// FRAME_NAME output does not required any inputs
		return;
	}
	else {
		throw new GeneralException ("VisualTargetManager::request : unknown output ID.",__FILE__,__LINE__);
	}

}

void VisualTargetManager::calculate(int output_id, int count, Buffer &out)
{
	try {
		if (output_id == m_imageOutID) {
			fflush(stdout);
			// Get current image
			RCPtr<Image> imageRef = getInput(m_imageInID, count);
			
			// Verify input image sanity
			if (imageRef->get_width() != m_width ||
				imageRef->get_height() != m_height ||
				imageRef->get_pixelsize() != m_numChannels) {
				throw new GeneralException ("VisualTargetManager::calculate : image parameters do not correspond to given input.",__FILE__,__LINE__);
			}
			
			memcpy(m_curImage->imageData, imageRef->get_data(), m_numBytesInFrame);
			
			if (m_target->IsValid()) {
				// Draw current target's ROI in current image
				m_target->GetCstROI()->DrawROI(m_curImage, (const unsigned char *)m_roiColor);
				
				// Copy current image for output
				Image *outImage = Image::alloc(m_width, m_height, m_numChannels);

				unsigned char *p_src = (unsigned char *)m_curImage->imageData;
				unsigned char *p_dst = (unsigned char *)outImage->get_data();
				for (int p=0; p<m_numPixels; p++) {
					p_dst[0] = p_src[2];
					p_dst[1] = p_src[1];
					p_dst[2] = p_src[0];
					p_dst += 3;
					p_src += 3;
				}
				//memcpy(outImage->get_data(), m_curImage->imageData, m_numBytesInFrame);
				
				(*outputs[m_imageOutID].buffer)[count] = ObjectRef(outImage);
			}
			else {
				// If no current target, output unmodified image
				(*outputs[m_imageOutID].buffer)[count] = ObjectRef(imageRef);
			}
		}
		else if (output_id == m_roiOutID) {
			if (m_target->IsValid()) {
				// Output directly the reference since it should
				// only be used as a constant reference
				//VisualROI *outROI = new VisualROI(*(m_target->GetCstROI()));
				//(*outputs[m_roiOutID].buffer)[count] = ObjectRef(outROI);
				//(*outputs[m_roiOutID].buffer)[count] = ObjectRef(RCPtr<VisualROI>(m_target->GetROI()));
				static RCPtr<VisualROI> refROI = RCPtr<VisualROI>(m_target->GetROI());
				(*outputs[m_roiOutID].buffer)[count] = refROI;
			}
			else {
				// No ROI initialized, so output nilObject
				(*outputs[m_roiOutID].buffer)[count] = ObjectRef(nilObject);
			}
		}
		else if (output_id == m_targetOutID) {
			if (m_target->IsValid()) {
				// Output directly the reference since it should
				// only be used as a constant reference
				//VisualTarget<double> *outTarget = new VisualTarget<double>(*m_target);
				//(*outputs[m_targetOutID].buffer)[count] = ObjectRef(outTarget);
				static RCPtr<VisualTarget<double> > refTarget = RCPtr<VisualTarget<double> >(m_target);
				(*outputs[m_targetOutID].buffer)[count] = m_refTarget;
			}
			else {
				// No ROI initialized, so output nilObject
				(*outputs[m_targetOutID].buffer)[count] = ObjectRef(nilObject);
			}
		}
		else if (output_id == m_targetProbOutID) {
			if (m_target->GetCurrentAge() < -10) {
				// Target is too old to track
				// Invalidate it
				m_target->SetValidity(false);
			}
				
			if (!m_target->IsValid()) {
				// Try to initialize a new target
				// Get detected ROI
				ObjectRef roiRef = getInput(m_roiInID, count);
				m_ppCompleted = false;
				
				if (!roiRef->isNil()) {
					// Initialize target at current ROI
					m_target->SetROI(&(object_cast<VisualROI>(roiRef)));
					m_target->SetValidity(true);
					
					if (!m_ppCompleted) {
						// Force features extraction preprocessing
						RCPtr<Vector<int> > preprocessRef = getInput(m_ppCompletedInID, count);
						m_ppCompleted = true;
					}
					
					// Get features for current ROI and use them
					// as the target feature model
					ObjectRef featVecRef = getInput(m_featVecInID, count);
					if (featVecRef->isNil()) {
						throw new GeneralException ("VisualTargetManager::calculate : cannot initialize target with null input FEATURES_VECTOR.",__FILE__,__LINE__);
					}
					
					m_target->SetDescriptorsVec(&(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef)));
					m_target->InitCueWeights();
					m_target->InitAges();
				}
			}
			
			if (m_target->IsValid()) {
				double sim = 0.0;
				bool targetMatch = false;
				
				// Force features extraction preprocessing only once per frame
				if (!m_ppCompleted) {
					RCPtr<Vector<int> > preprocessRef = getInput(m_ppCompletedInID, count);
					m_ppCompleted = true;
				}
				
				// Start timer
				ftime(&m_t1);
				
				// Track current target
				ObjectRef targetRef = getInput(m_targetInID, count);
				
				if (!targetRef->isNil()) {
					*m_target = object_cast<VisualTarget<double> >(targetRef);
				
					// Get features to evalute the likelihood of the current
					// target at the given tracked position
					ObjectRef featVecRef = getInput(m_featVecInID, count);
					if (featVecRef->isNil()) {
						throw new GeneralException ("VisualTargetManager::calculate : cannot evaluate the target's likelihood with null input FEATURES_VECTOR.",__FILE__,__LINE__);
					}
					
					sim = m_target->SimilarityWCueAdapt(
						&(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef)), 
						m_cueAdaptRate);
						
					targetMatch = (sim >= m_targetMatchThres);
					
					m_target->AgeTarget(targetMatch);
					if (sim > m_targetAdaptThres) {
						m_target->Adapt(&(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef)),
							m_targetAdaptRate);
					}
				}
				
				// End timer
				ftime(&m_t2);
				
				// Display time used
				double timeDiff=(m_t2.time-m_t1.time)+((m_t2.millitm-m_t1.millitm)/1000.0);
				cout << "Total run time (sec): " << timeDiff << endl;
				
				(*outputs[m_targetProbOutID].buffer)[count] = ObjectRef(Double::alloc(sim));
			}
			else {
				// If no current target, output 0.0
				(*outputs[m_targetProbOutID].buffer)[count] = ObjectRef(Double::alloc(0.0));
			}
		}
		else if (output_id == m_targetDXOutID) {
			if (m_target->IsValid()) {
				float deltaX = (float)(m_target->GetCstROI()->GetXCen()) - m_imgXCen;
				(*outputs[m_targetDXOutID].buffer)[count] = ObjectRef(Float::alloc(deltaX));
			}
			else {
				(*outputs[m_targetDXOutID].buffer)[count] = nilObject;
			}
		}
		else if (output_id == m_targetDYOutID) {
			if (m_target->IsValid()) {
				float deltaY = m_imgYCen - (float)(m_target->GetCstROI()->GetYCen());
				(*outputs[m_targetDYOutID].buffer)[count] = ObjectRef(Float::alloc(deltaY));
			}
			else {
				(*outputs[m_targetDYOutID].buffer)[count] = nilObject;
			}
		}
		else if (output_id == m_nameOutID) {
			String *outName = new String("/home/moip1501/images/FaceTracking");
			char idx[12];
			sprintf(idx, "%05d", count);
			(*outName) += idx;
			(*outName) += ".jpg";

			(*outputs[m_nameOutID].buffer)[count] = ObjectRef(outName);
		}
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in VisualTargetManager::calculate:",__FILE__,__LINE__));
	}
}

--- NEW FILE: VisualROI.cc ---
#include "VisualROI.h"

DECLARE_TYPE(VisualROI)

VisualROI::VisualROI()
: m_type(), m_xCen(0), m_yCen(0), 
m_hsX(0), m_hsY(0), m_angle(0), 
m_perim(NULL), m_normVects(NULL), m_mask(NULL)
{

}

VisualROI::VisualROI(e_VISUALROI_type i_type, int i_xCen, int i_yCen,
	int i_hsX, int i_hsY, int i_angle)
:m_type(i_type), m_xCen(i_xCen), m_yCen(i_yCen),
m_hsX(i_hsX), m_hsY(i_hsY), m_angle(i_angle),
m_perim(NULL), m_normVects(NULL), m_mask(NULL)
{
	switch(m_type) {
	case e_VISUALROI_rectangular:
		MakeRectangularRegion();
		break;
	case e_VISUALROI_elliptical:
		MakeEllipticalRegion();
		break;
	case e_VISUALROI_unknown:
	default:
		throw new GeneralException ("VisualROI::VisualROI : unknown region geometric type",__FILE__,__LINE__);
	}
}

VisualROI::VisualROI(const VisualROI& i_ref)
:m_perim(NULL), m_normVects(NULL), m_mask(NULL)
{
	try {
		m_type = i_ref.m_type;
		m_xCen = i_ref.m_xCen;
		m_yCen = i_ref.m_yCen;
		m_hsX = i_ref.m_hsX;
		m_hsY = i_ref.m_hsY;
		m_angle = i_ref.m_angle;
		
		Reset(m_hsX, m_hsY, m_angle);
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception caught in VisualROI::VisualROI:",__FILE__,__LINE__));
	}
}

VisualROI::~VisualROI()
{
	delete [] m_perim;
	delete [] m_normVects;
	delete [] m_mask;
}

VisualROI & VisualROI::operator =(const VisualROI &i_ref)
{
	// Avoid self assignment
	if (&i_ref == this) {
		return *this;
	}
	
	try {
		m_type = i_ref.m_type;
		m_xCen = i_ref.m_xCen;
		m_yCen = i_ref.m_yCen;
		
		if (m_hsX == i_ref.m_hsX &&
			m_hsY == i_ref.m_hsY &&
			m_angle == i_ref.m_angle) {
			// Nothing else to do
			return *this;	
		}
		
		m_hsX = i_ref.m_hsX;
		m_hsY = i_ref.m_hsY;
		m_angle = i_ref.m_angle;
		
		Reset(m_hsX, m_hsY, m_angle);
		
		return *this;
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception caught in VisualROI::VisualROI:",__FILE__,__LINE__));
	}
}

// Default routine to print a VisualROI object to an output stream
void VisualROI::printOn(ostream &out) const
{
	throw new GeneralException ("VisualROI::printOn : routine not implemented yet",__FILE__,__LINE__);
}

// Default routine to read a VisualROI object from an input stream
void VisualROI::readFrom(istream &in)
{
	throw new GeneralException ("VisualROI::readFrom : routine not implemented yet",__FILE__,__LINE__);
}

void VisualROI::DrawROI(IplImage *io_frame, const unsigned char *i_color) const
{
	if (!io_frame) {
		throw new GeneralException ("VisualROI::DrawROI : invalid image reference.",__FILE__,__LINE__);
	}
	
	if (!i_color) {
		throw new GeneralException ("VisualROI::DrawROI : invalid color reference.",__FILE__,__LINE__);
	}
	
	if (!m_perim) {
		throw new GeneralException ("VisualROI::DrawROI : cannot draw ROI with uninitialized region data.",__FILE__,__LINE__);
	}
	
	unsigned char *p_pixels = (unsigned char *)io_frame->imageData;
	const short *p_perim = this->GetCstPerim();
	int imgWidth = io_frame->width;
	int imgHeight = io_frame->height;
	int numChannels = io_frame->nChannels;
	int i, c, x, y;
	short deltaX, deltaY;
	bool broken = true;

	// Start at the top center of the region
	x = m_xCen;
	y = m_yCen;
	p_pixels += numChannels*(y*imgWidth + x);

	// Overlay region of interest
	for (i=m_perimLength; i>0; i--)  {
		deltaX = *p_perim++;
		deltaY = *p_perim++;
		x += deltaX;
		y += deltaY;

		// Draw only if region is visible
		if (y>0 && y<imgHeight && x>0 && x<imgWidth) {
			if (!broken) {
				// Relative position
				p_pixels += numChannels*(deltaY*imgWidth + deltaX);
			}
			else {
				// Absolute position
				p_pixels = (unsigned char *)(io_frame->imageData + numChannels*(y*imgWidth + x));
				broken = false;
			}

			for (c=0; c<numChannels; c++) {
				*p_pixels++ = i_color[c];
			}

			p_pixels -= numChannels;
		}
		else {
			broken = true;
		}
	}
}

void VisualROI::Reset(int i_hsX, int i_hsY, int i_angle)
{
	delete [] m_perim;
	delete [] m_normVects;
	delete [] m_mask;

	m_hsX = i_hsX;
	m_hsY = i_hsY;
	m_angle = i_angle;

	switch(m_type) {
	case e_VISUALROI_rectangular:
		MakeRectangularRegion();
		break;
	case e_VISUALROI_elliptical:
		MakeEllipticalRegion();
		break;
	case e_VISUALROI_unknown:
	default:
		throw new GeneralException ("VisualROI::VisualROI : unknown region geometric type",__FILE__,__LINE__);
	}
}

//
// Public accessor methods
//
e_VISUALROI_type VisualROI::GetType() const
{
	return m_type;
}

int VisualROI::GetPerimLength() const
{
	return m_perimLength;
}

short *VisualROI::GetPerim()
{
	return m_perim;
}

const short *VisualROI::GetCstPerim() const
{
	return (const short *)m_perim;
}

float *VisualROI::GetNormVects()
{
	return m_normVects;
}

const float *VisualROI::GetCstNormVects() const
{
	return (const float *)m_normVects;
}

int VisualROI::GetXCen() const
{
	return m_xCen;
}

int VisualROI::GetYCen() const
{
	return m_yCen;
}

int VisualROI::GetHSX() const
{
	return m_hsX;
}

int VisualROI::GetHSY() const
{
	return m_hsY;
}

int VisualROI::GetAngle() const
{
	return m_angle;
}

int VisualROI::GetArea() const
{
	return m_area;
}

unsigned char *VisualROI::GetMask()
{
	return m_mask;
}

const unsigned char *VisualROI::GetCstMask() const
{
	return (const unsigned char *)m_mask;
}

void VisualROI::SetXCen(int i_xCen)
{
	m_xCen = i_xCen;
}
	
void VisualROI::SetYCen(int i_yCen)
{
	m_yCen = i_yCen;
}
	
void VisualROI::SetHSX(int i_hsX)
{
	if (i_hsX == m_hsX) {
		// Nothing to do
		return;
	}
	
	try {
		m_hsX = i_hsX;
		Reset(m_hsX, m_hsY, m_angle);
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception caught in VisualROI::SetHSX:",__FILE__,__LINE__));
	}
}

void VisualROI::SetHSY(int i_hsY)
{
	if (i_hsY == m_hsY) {
		// Nothing to do
		return;
	}
	
	try {
		m_hsY = i_hsY;
		Reset(m_hsX, m_hsY, m_angle);
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception caught in VisualROI::SetHSY:",__FILE__,__LINE__));
	}
}

void VisualROI::SetAngle(int i_angle)
{
	if (i_angle == m_angle) {
		// Nothing to do
		return;
	}
	
	try {
		m_angle = i_angle;
		Reset(m_hsX, m_hsY, m_angle);
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception caught in VisualROI::SetAngle:",__FILE__,__LINE__));
	}
}

//
// Private routines
//
void VisualROI::MakeEllipticalRegion()
{
	short *ptr;
	float *ptrn;
	int xrad2 = m_hsX * m_hsX;
	int yrad2 = m_hsY * m_hsY;
	double ratio = ((double) xrad2) / ((double) yrad2);
	int hsXbkpt = (int) (xrad2 / sqrt((double) xrad2 + yrad2));
	int hsYbkpt = (int) (yrad2 / sqrt((double) xrad2 + yrad2));
	int x, y, xOff, yOff, xAbs, yAbs;
	int curX, curY;

	// Number of iterations in loops below
	m_perimLength = 4 * hsXbkpt + 4 * hsYbkpt + 4;
	m_perim = new short[2*m_perimLength];
	m_normVects = new float[2*m_perimLength];
	ptr = m_perim;
	ptrn = m_normVects;

	// Perimeter starts at top and proceeds clockwise
	*(ptr)++ = curX = 0;
	*(ptr)++ = curY = - m_hsY;
	*ptrn++ = 0.0f;
	*ptrn++ = -1.0f;

	// From 0 to 45 degrees, starting from top clockwise of an unoriented ellipse
	for (x = 1; x <= hsXbkpt ; x++)  {
		y = (int) (-m_hsY * sqrt(1 - ((double) x / m_hsX)*((double) x / m_hsX)));
		*(ptr)++ = x - curX;
		*(ptr)++ = y - curY;
		*ptrn++ = (float) (       x  / sqrt(x*x + ratio*y*y));
		*ptrn++ = (float) ((ratio*y) / sqrt(x*x + ratio*y*y));
		curX = x;
		curY = y;
	}
	
	// From 45 to 135 degrees (including right axis)
	for (y = - hsYbkpt ; y <= hsYbkpt ; y++)  {
		x = (int) (m_hsX * sqrt(1 - ((double) y / m_hsY)*((double) y / m_hsY)));
		*(ptr)++ = x - curX;
		*(ptr)++ = y - curY;
		*ptrn++ = (float) (       x  / sqrt(x*x + ratio*y*y));
		*ptrn++ = (float) ((ratio*y) / sqrt(x*x + ratio*y*y));
		curX = x;
		curY = y;
	}
	// From 135 to 225 degrees (including down axis)
	for (x = hsXbkpt  ; x >= - hsXbkpt ; x--)  {
		y = (int) (m_hsY * sqrt(1 - ((double) x / m_hsX)*((double) x / m_hsX)));
		*(ptr)++ = x - curX;
		*(ptr)++ = y - curY;
		*ptrn++ = (float) (       x  / sqrt(x*x + ratio*y*y));
		*ptrn++ = (float) ((ratio*y) / sqrt(x*x + ratio*y*y));
		curX = x;
		curY = y;
	}
	// From 225 to 315 degrees (including left axis)
	for (y = hsYbkpt ; y >= - hsYbkpt ; y--)  {
		x = (int) (-m_hsX * sqrt(1 - ((double) y / m_hsY)*((double) y / m_hsY)));
		*(ptr)++ = x - curX;
		*(ptr)++ = y - curY;
		*ptrn++ = (float) (       x  / sqrt(x*x + ratio*y*y));
		*ptrn++ = (float) ((ratio*y) / sqrt(x*x + ratio*y*y));
		curX = x;
		curY = y;
	}
	// From 315 to 360 degrees
	for (x = - hsXbkpt ; x < 0 ; x++)  {
		y = (int) (-m_hsY * sqrt(1 - ((double) x / m_hsX)*((double) x / m_hsX)));
		*(ptr)++ = x - curX;
		*(ptr)++ = y - curY;
		*ptrn++ = (float) (       x  / sqrt(x*x + ratio*y*y));
		*ptrn++ = (float) ((ratio*y) / sqrt(x*x + ratio*y*y));
		curX = x;
		curY = y;
	}

	m_area = MakeRegionMask();
}

void VisualROI::MakeRectangularRegion()
{
	throw new GeneralException ("VisualROI::MakeRectangularRegion : method not yet implemented.",__FILE__,__LINE__);
}

int VisualROI::MakeRegionMask()
{
	unsigned char *p_mask;
	short *p_perim;
	int *mins, *maxs;
	int *p_min, *p_max;
	int x, y;
	int i, j;
	int sizeX = 2*(m_hsX)+1;
	int sizeY = 2*(m_hsY)+1;
	int numPixels = 0;

	mins = new int[sizeY];
	maxs = new int[sizeY];
	m_mask = new unsigned char[sizeX*sizeY];
	
	// Init mins and maxs
	p_min = mins;
	p_max = maxs;
	for (i = 0 ; i < sizeY ; i++)  {
		*p_min++ =  999;
		*p_max++ = -999;
	}

	// Init mask
	memset(m_mask, 0, sizeX*sizeY);

	// Fill in max and min vectors
	p_perim = m_perim;
	x = m_hsX + *p_perim++;
	y = m_hsY + *p_perim++;
	p_max = maxs + y;
	p_min = mins + y;
	*p_max = x;
	*p_min = x;
	for (i=1; i<m_perimLength; ++i)  {
		x += *p_perim++;
		p_max += *p_perim;
		p_min += *p_perim++;
		if (x > *p_max) {
			*p_max = x;
		}
		if (x < *p_min) {
			*p_min = x;
		}
	}
	
	// Use max and min vectors to fill in mask.
	// The interior of the object is filled in
	// and the perimeter is excluded.
	p_min = mins;
	p_max = maxs;
	for (i = 0 ; i < sizeY ; i++)  {
		p_mask = (m_mask) + i * sizeX;
		if (*p_min < *p_max)  {
			p_mask += *p_min + 1;
			for (j = *p_min + 1 ; j < *p_max ; j++) {
				*p_mask++ = 255;
				numPixels++;
			}
		}
		p_min++;
		p_max++;
	}

	// Clean memory
	delete [] mins;
	delete [] maxs;

	return numPixels;
}

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/Makefile.am,v
retrieving revision 1.29
retrieving revision 1.30
diff -C2 -d -r1.29 -r1.30
*** Makefile.am	12 Apr 2005 19:23:04 -0000	1.29
--- Makefile.am	12 Apr 2005 19:34:03 -0000	1.30
***************
*** 7,11 ****
  
  if WITH_OPENCV
! OPENCV_SOURCES = 
  else
  OPENCV_SOURCES =
--- 7,17 ----
  
  if WITH_OPENCV
! OPENCV_SOURCES = ColorHistExtraction.cc \
! 	MeanShiftTracker.cc \
! 	VisualFeatureDesc.cc \
! 	VisualHistogramDesc.cc \
! 	VisualROI.cc \
! 	VisualTarget.cc \
! 	VisualTargetManager.cc
  else
  OPENCV_SOURCES =

--- NEW FILE: MeanShiftTracker.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#include "MeanShiftTracker.h"

DECLARE_NODE(MeanShiftTracker)
DECLARE_TYPE(MeanShiftTracker)

  /*Node
   *
   * @name MeanShiftTracker
   * @category RobotFlow:Vision:Tracking
   * @description Mean shift region/object tracker.
   *
   * @parameter_name FRAME_WIDTH
   * @parameter_type int
   * @parameter_value 320
   * @parameter_description Video frame width.
   *
   * @parameter_name FRAME_HEIGHT
   * @parameter_type int
   * @parameter_value 240
   * @parameter_description Video frame height.
   *
   * @parameter_name NUM_CHANNELS
   * @parameter_type int
   * @parameter_value 3
   * @parameter_description Number of channels in video frame.
   *
   * @parameter_name MAX_NUM_MS_ITER
   * @parameter_type int
   * @parameter_value 20
   * @parameter_description Maximum number of mean shift iterations.
   *
   * @parameter_name MS_DIST_EPSILON
   * @parameter_type float
   * @parameter_value 2.0
   * @parameter_description Minimal distance to consider that the mean shift has found a maximum.
   *
   * @input_name IMAGE_IN
   * @input_type Image
   * @input_description Current video frame.
   *
   * @input_name PREPROCESS_COMPLETED
   * @input_type Vector<int>
   * @input_description Flags indicating the completion of the features extraction preprocessing stage.
   *
   * @input_name CURRENT_TARGET
   * @input_type VisualTarget<double>
   * @input_description Current target to track.
   *
   * @input_name MS_LOCATION
   * @input_type Vector<double>
   * @input_description Mean shift vector of location.
   *
   * @output_name TRACKING_FINISHED
   * @output_type bool
   * @output_description Flag indicating that the tracking has completed.
   *
   * @output_name IMAGE_OUT
   * @output_type Image
   * @output_description Current image to process.
   *
   * @output_name CURRENT_TARGET
   * @output_type VisualTarget<double>
   * @output_description Current target being tracked (to compute mean shift location).
   *
   * @output_name TRACKED_TARGET
   * @output_type Image
   * @output_description Target updated according to the tracking algorithm.
   *
   END*/

//
// Mean shift object/region tracker for RobotFlow 
//
// Implementation is based on the following publication:
//
// D. Comaniciu, V. Ramesh, P. Meer. "Kernel-based object tracking", 
// IEEE Transactions on Pattern Analysis Machine Intelligence, vol 25, 
// pp. 564-577, 2003.
//
 
//
// Default constructor for Object 
//
MeanShiftTracker::MeanShiftTracker()
: VisualTracker(),
m_maxNumMSIter(0),
m_minMSDistEpsilon(0.0),
m_curTarget(NULL)
{

}

MeanShiftTracker::MeanShiftTracker(int i_maxNumMSIter, double i_minMSDistEpsilon)
: VisualTracker(),
m_maxNumMSIter(i_maxNumMSIter),
m_minMSDistEpsilon(i_minMSDistEpsilon),
m_curTarget(NULL)
{
	m_curTarget = new VisualTarget<double>();
}

//
// BufferedNode constructor
//
MeanShiftTracker::MeanShiftTracker(string nodeName, ParameterSet params)
: VisualTracker(nodeName, params),
m_curTarget(NULL)
{
	m_imageInID = addInput("IMAGE_IN");
	m_ppCompletedInID = addInput("PREPROCESS_COMPLETED");
	m_targetInID = addInput("CURRENT_TARGET");
	m_msLocVecInID = addInput("MS_LOCATION");

	m_finishedOutID = addOutput("TRACKING_FINISHED");
	m_imageOutID = addOutput("IMAGE_OUT");
	m_curTargetOutID = addOutput("CURRENT_TARGET");
	m_targetOutID = addOutput("TRACKED_TARGET");

	m_width = dereference_cast<int>(parameters.get("FRAME_WIDTH"));
	m_height = dereference_cast<int>(parameters.get("FRAME_HEIGHT"));
	m_numChannels = dereference_cast<int>(parameters.get("NUM_CHANNELS"));
	m_maxNumMSIter = dereference_cast<int>(parameters.get("MAX_NUM_MS_ITER"));
	m_minMSDistEpsilon = dereference_cast<float>(parameters.get("MS_DIST_EPSILON"));
	
	m_numPixels = m_width*m_height;
	m_numBytesInFrame = m_numPixels*m_numChannels;
	
	CvSize imgSize;
	imgSize.width = m_width;
	imgSize.height = m_height;
	
	m_curImage = cvCreateImage(imgSize, IPL_DEPTH_8U, m_numChannels);
	
	m_curTarget = new VisualTarget<double>();
	m_finished = false;
	m_initMS = false;
}
	
MeanShiftTracker::~MeanShiftTracker()
{
	delete m_curTarget;
	cvReleaseImage(&m_curImage);
}

// Modified BufferedNode request method to support cyclic node connection
void MeanShiftTracker::request(int output_id, const ParameterSet &req) 
{
	if (req.exist("LOOKAHEAD")) {
		outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD")));
	}
	
	if (req.exist("LOOKBACK")) {
		outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK")));
	}
	
	if (req.exist("INORDER")) {
		inOrder = true;
	}
	
	int outputLookAhead=0, outputLookBack=0;

	outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead);
	outputLookBack =max(outputLookBack, outputs[output_id].lookBack);
	
	if (output_id == m_finishedOutID) {
		// TRACKING_FINISHED output does not required any inputs
		return;
		
	}
	else if (output_id == m_imageOutID) {
		// IMAGE_OUT output does not required any inputs
		return;
	}
	else if (output_id == m_curTargetOutID) {
		// CURRENT_TARGET output does not required any inputs
		return;
	}
	else if (output_id == m_targetOutID) {
		ParameterSet myReq, myReq2, myReq3, myReq4;
		
		myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookAhead+outputLookAhead)));
		myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookBack+outputLookBack)));
		inputs[m_imageInID].node->request(inputs[m_imageInID].outputID,myReq);
		
		myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookAhead+outputLookAhead)));
		myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookBack+outputLookBack)));
		inputs[m_ppCompletedInID].node->request(inputs[m_ppCompletedInID].outputID,myReq2);
		
		myReq3.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookAhead+outputLookAhead)));
		myReq3.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookBack+outputLookBack)));
		inputs[m_targetInID].node->request(inputs[m_targetInID].outputID,myReq3);
		
		myReq4.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_msLocVecInID].lookAhead+outputLookAhead)));
		myReq4.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_msLocVecInID].lookBack+outputLookBack)));
		inputs[m_msLocVecInID].node->request(inputs[m_msLocVecInID].outputID,myReq4);
	}
	else {
		throw new GeneralException ("MeanShiftTracker::request : unknown output ID.",__FILE__,__LINE__);
	}
}

void MeanShiftTracker::calculate(int output_id, int count, Buffer &out)
{
	try {
		if (output_id == m_finishedOutID) {
			(*outputs[m_finishedOutID].buffer)[count] = ObjectRef(Bool::alloc(!m_finished));
			if (m_finished) {
				m_initMS = false;
				m_finished = false;
			}
		}
		else if (output_id == m_imageOutID) {
			// Copy current image for output
			Image *outImage = Image::alloc(m_width, m_height, m_numChannels);
			memcpy(outImage->get_data(), m_curImage->imageData, m_numBytesInFrame);
			
			(*outputs[m_imageOutID].buffer)[count] = ObjectRef(outImage);
		}
		else if (output_id == m_curTargetOutID) {
			// Output current target
			if (m_curTarget->GetCstROI()) {
				//VisualTarget<double> *outTarget = new VisualTarget<double>(*m_curTarget);
				static RCPtr<VisualTarget<double> > refTarget = RCPtr<VisualTarget<double> >(m_curTarget);
				(*outputs[m_curTargetOutID].buffer)[count] = ObjectRef(refTarget);
			}
			else {
				(*outputs[m_curTargetOutID].buffer)[count] = ObjectRef(nilObject);
			}
		}
		else if (output_id == m_targetOutID){
			if (!m_initMS) {
				// First get current target to track
				ObjectRef targetRef = getInput(m_targetInID, count);
				
				if (targetRef->isNil()) {
					// Invalid target, output nilObject
					(*outputs[m_targetOutID].buffer)[count] = ObjectRef(nilObject);
					return;
				}
				
				// Set current target reference to tracker's target model
				*m_curTarget = object_cast<VisualTarget<double> >(targetRef);
				
				// Get current image
				RCPtr<Image> imageRef = getInput(m_imageInID, count);
				
				// Verify input image sanity
				if (imageRef->get_width() != m_width ||
					imageRef->get_height() != m_height ||
					imageRef->get_pixelsize() != m_numChannels) {
					throw new GeneralException ("MeanShiftTracker::calculate : image parameters do not correspond to given input.",__FILE__,__LINE__);
				}
				
				memcpy(m_curImage->imageData, imageRef->get_data(), m_numBytesInFrame);
				
				// Force features extraction preprocessing
				RCPtr<Vector<int> > preprocessRef = getInput(m_ppCompletedInID, count);
				
				m_numIter = 0;
				
				m_initMS = true;
			}
			
			
			// Apply mean shift tracking
			int numDesc = m_curTarget->GetNumDescriptors();
			double distanceMoved = 0.0;

			int xLoc = m_curTarget->GetCstROI()->GetXCen();
			int yLoc = m_curTarget->GetCstROI()->GetYCen();
			double newXLoc = 0.0;
			double newYLoc = 0.0;
			
			// Get mean shift location
			RCPtr<Vector<double> > msLocVecRef = getInput(m_msLocVecInID, count);
			
			// For each visual cue
			for (int i=0; i<numDesc; i++) {
				// Location of target is a weighted sum of all the cues locations
				newXLoc += (*msLocVecRef)[2*i];
				newYLoc += (*msLocVecRef)[2*i+1];
			}
			
			// Compute distance moved during this iteration
			double deltaX = newXLoc - (double)xLoc;
			double deltaY = newYLoc - (double)yLoc;
			distanceMoved = sqrt(deltaX*deltaX + deltaY*deltaY);
			
			// Set new location
			xLoc = (int)(round(newXLoc));
			yLoc = (int)(round(newYLoc));
			m_curTarget->GetROI()->SetXCen(xLoc);
			m_curTarget->GetROI()->SetYCen(yLoc);

			m_numIter++;

			if ( (distanceMoved <= m_minMSDistEpsilon) || (m_numIter >= m_maxNumMSIter) ) {
				m_finished = true;
			}
				
			if (!m_finished) {
				(*outputs[m_targetOutID].buffer)[count] = ObjectRef(nilObject);
			}
			else {
				// Output current target with ROI position updated
				// A copy must be done since it will be probably be modified
				//VisualTarget<double> *outTarget = new VisualTarget<double>(*m_curTarget);
				static RCPtr<VisualTarget<double> > outTarget = RCPtr<VisualTarget<double> >(m_curTarget);
				(*outputs[m_targetOutID].buffer)[count] = ObjectRef(outTarget);
			}
		}
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in MeanShiftTracker::calculate:",__FILE__,__LINE__));
	}
}

	// Make a mean shift iteration
	
	// Make a weighted sum of the features and adapt the weights
	// rate*w_new = quality_mesurement - w_old
	// where quality_mesurement = normDist(prob(estimate), avg(prob(all pos for current cue)))
	// or
	// w_new = rateInv*quality_mesurement - (1-rateInv)*w_old
	// quality_mesurement = likelihood(estimate from last frame)/sum(likelihood(all pos for current cue from last frame))
	// or quality_mesurement => {exp(-error)|current_cue}/sum(exp(-error)|for all cues)
	//
	// For mean shift:
	//	For each cue:
	//			For each pixel in ROI
	//				probRef = m_curTarget.GetCstDescriptorsVec.getProb(pixel(x,y))
	//				probTmp = i_descVec.getProb(pixel(x,y))
	//				if (probTmp > 0)
	//					wi=cueWeight*sqrt(probRef/probTmp);
	//					float kern = derivateKernel(window.getCenter(),position,radius);
	//					numerator.x += wi*kern*position.x;
	//					numerator.y += wi*kern*position.y;
	//					denominator += wi*kern;
	//				end if
	//			end for
	//		end for
	//		estimate new location
	//	end for
	// end mean shift
	//
	// double sumLikelihhods = 0.0;
	// For each cue:
	//	likelihood = EstimateTarget
	//	sumLikelihhods += likelihood
	// end for
	// For each cue:
	//	cueWeight = rateInv*likelihood/sumLikelihhods + (1-rateInv)*cueWeight;
	// end for



--- NEW FILE: VisualHistogramDesc.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "VisualHistogramDesc.h"

//
// Declaring known types
//
static int VISUALHISTDESC_VAR_0 = Object::addObjectType<VisualHistogramDesc<double, unsigned char> >
	("<VisualHistogramDesc<double,unsigned char>>", new ObjectFactory<VisualHistogramDesc<double, unsigned char> >("<VisualHistogramDesc<double,unsigned char>>"));
static int VISUALHISTDESC_VAR_1 = Object::addObjectType<VisualHistogramDesc<double, unsigned int> >
	("<VisualHistogramDesc<double,unsigned int>>", new ObjectFactory<VisualHistogramDesc<double, unsigned int> >("<VisualHistogramDesc<double,unsigned int>>"));