source: flair-src/trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp @ 214

//  created:    2012/04/12
//  filename:   OpticalFlowSpeed.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    calcul de la vitesse à partir du flux optique
//
//
/*********************************************************************/

#include "OpticalFlowSpeed.h"
#include "OpticalFlowData.h"
#include <Matrix.h>
#include <Object.h>

using std::string;
using namespace flair::core;

namespace flair
{
namespace filter
{

OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent,string name) : IODevice(parent,name)
{
  cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
  desc->SetElementName(0,0,"vx");
  desc->SetElementName(1,0,"vy");
  output=new Matrix(this,desc,floatType,name);
  delete desc;

        AddDataToLog(output);
  
  SetIsReady(true);
}

OpticalFlowSpeed::~OpticalFlowSpeed(void)
{
}

void OpticalFlowSpeed::UpdateFrom(const io_data *data)
{
    OpticalFlowData *input=(OpticalFlowData*)data;
    float deplx,deply;
    int nb_depl=0;

    deplx=0;
    deply=0;

    input->GetMutex();
    for(int i=0;i<input->NbFeatures();i++) {
        if(input->FoundFeature()[i]!=0) {
            deplx+=input->PointsB()[i].x-input->PointsA()[i].x;
            deply+=input->PointsB()[i].y-input->PointsA()[i].y;
            nb_depl++;
        }
    }
    input->ReleaseMutex();
    Time deltaT=data->DataTime()-output->DataTime();
    output->SetDataTime(data->DataTime());

    if(nb_depl!=0) {
        output->SetValue(0,0,deplx/(nb_depl*deltaT)*1000*1000*1000);
        output->SetValue(1,0,deply/(nb_depl*deltaT)*1000*1000*1000);
    }

    ProcessUpdate(output);
}

float OpticalFlowSpeed::Vx(void) const
{
    return output->Value(0,0);
}

float OpticalFlowSpeed::Vy(void) const
{
    return output->Value(1,0);
}

core::Matrix *OpticalFlowSpeed::Output() const
{
    return output;
}
} // end namespace filter
} // end namespace flair