/*************************************************************************\
Librería de Sensor Visual (LibSV)
http://code.google.com/p/pfcfnm/
© 2007, Fernando Nieto Moraleda
This project is released under the GPL license.
See http://www.gnu.org/licenses/gpl.html for the full text.
$HeadURL: https://pfcfnm.googlecode.com/svn/trunk/src/LibSV/lucaskanade.cpp $
$Revision: 113 $
$Author: fernandonm $
$Date: 2008-01-07 20:20:14 +0100 (lun, 07 ene 2008) $
Particularización del sensor visual basada en el algoritmo de
Horn-Schunck.
\*************************************************************************/
#include "sensorHS.h"
#include "matematicas.h"
#include "utiles.h"
#include "contadorFps.h"
using namespace LibSV;
SensorHS::SensorHS(
int ai_mallaCols,
int ai_mallaFilas,
int ai_vetanaAncho,
int ai_ventanaAlto) :
m_ctrlFuzzy(NULL),
m_tamMalla(cvSize(ai_mallaCols, ai_mallaFilas)),
m_tamVentana(cvSize(ai_vetanaAncho, ai_ventanaAlto)) // Tamaño de la ventana alrededor de los puntos para el algoritmo de LK
{
}
SensorHS::~SensorHS(void)
{
if (m_ctrlFuzzy != NULL)
{
delete m_ctrlFuzzy;
m_ctrlFuzzy = NULL;
}
}
int SensorHS::inicializar()
{
try
{
m_ctrlFuzzy = new ControladorFuzzyLK();
}
catch (excepcionCargandoFCL ex)
{
std::cerr << ex.what() << std::endl;
return -1;
}
return SensorBase::inicializar();
}
void SensorHS::activarSync(CallbackFn cb)
{
m_running = true;
// Inicialización de las imágenes
IplImage* frame = cvQueryFrame(m_capture); // Prototipo para inicialización
IplImage* greyOrigen = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
IplImage* greyDestino = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
IplImage* pyramidOrigen = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
IplImage* pyramidDestino = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
IplImage* flipped = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
IplImage* eig = cvCreateImage(cvGetSize(frame), IPL_DEPTH_32F, 1);
IplImage* gfTemp = cvCreateImage(cvGetSize(frame), IPL_DEPTH_32F, 1 );
IplImage* temp = NULL;
//IplImage* velx = cvCreateImage(cvGetSize(frame), IPL_DEPTH_32F, 1);
//IplImage* vely = cvCreateImage(cvGetSize(frame), IPL_DEPTH_32F, 1);
CvMat* velx = cvCreateMatHeader(frame->height, frame->width, CV_32FC1 );
cvCreateData(velx);
CvMat* vely = cvCreateMatHeader(frame->height, frame->width, CV_32FC1 );
cvCreateData(vely);
CvPoint2D32f* arrayOrigen = new CvPoint2D32f[ConjuntoPuntos::MAX_COUNT];
CvPoint2D32f* arrayDestino = new CvPoint2D32f[ConjuntoPuntos::MAX_COUNT];
char* status = new char[ConjuntoPuntos::MAX_COUNT];
int flags = 0;
ContadorFps fps;
while (m_running)
{
if (frame = cvQueryFrame(m_capture))
{
cvCvtColor(frame, greyDestino, CV_BGR2GRAY);
// Inicialización del array de puntos característicos de control
ConjuntoPuntos cjtoPtosOrigen(greyDestino);
cjtoPtosOrigen.ToArray(arrayOrigen);
int count = cjtoPtosOrigen.size();
if (count == 0)
continue;
// Aplico el algoritmo de Horn & Schunck
int usePrevious = 0;
double lambda = 0.005; //0.002;
cvCalcOpticalFlowHS(
greyOrigen, greyDestino,
usePrevious,
velx, vely,
lambda,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
// Draw a bunch of vectors in our output window.
int GRIDSIZE = 10;
for(int x=0; x < velx->width; x += GRIDSIZE)
{
for(int y=0; y < velx->height; y += GRIDSIZE)
{
int dx = (int)CV_MAT_ELEM(*velx, float, y, x)*2;
int dy = (int)CV_MAT_ELEM(*vely, float, y, x)*2;
if (abs(dx) >= GRIDSIZE || abs(dy) >= GRIDSIZE)
{
CvPoint p1 = cvPoint(x, y);
CvPoint p2 = cvPoint(x + dx, y + dy);
cvLine(frame, p1, p2, CV_RGB(255L,255L,255L),1);
cvCircle(frame, p2, 1, CV_RGB(255L,255L,255L));
}
}
}
//int nWidth = greyOrigen->width;
//int nHeight = greyOrigen->height;
//float *pOpticalFlow_u = new float[nWidth*nHeight];
//float *pOpticalFlow_v = new float[nWidth*nHeight];
//for(int y=0; y<nHeight; y++)
//{
// for(int x=0; x<nWidth; x++)
// {
// pOpticalFlow_u[y*nWidth+x] = cvmGet(velx, y, x);
// pOpticalFlow_v[y*nWidth+x] = cvmGet(vely, y, x);
// }
//}
Movimiento mov(0,0,0,0);
// Calculo el desplazamiento, lo sobreimprimo a la imagen y lo envío...
//ConjuntoPuntos cjtoPtosDestino(arrayDestino, count);
//Movimiento mov = calcularDesplazamiento(cvGetSize(frame), cjtoPtosOrigen, cjtoPtosDestino, NULL);
//corregir(mov);
//pintarDesplazamiento(frame, arrayOrigen, arrayDestino, count, status);
fps.pintarFps(frame);
cvFlip(frame, flipped); // En .net se almacenan al revés las imágenes...
{
boost::mutex::scoped_lock lock(m_mutex);
if (!m_running)
break;
(*cb)(flipped, &mov);
}
// Guardo la imagen destino como origen de la siguiente iteración
CV_SWAP(greyOrigen, greyDestino, temp);
CV_SWAP(pyramidOrigen, pyramidDestino, temp);
}
sleep(10); // Evita que el thread acapare toda la cpu.
}
delete[] arrayOrigen;
delete[] arrayDestino;
delete[] status;
cvReleaseCapture(&m_capture);
cvReleaseImage(&greyOrigen);
cvReleaseImage(&greyDestino);
cvReleaseImage(&pyramidOrigen);
cvReleaseImage(&pyramidDestino);
cvReleaseImage(&flipped);
//cvReleaseImage(&velx);
//cvReleaseImage(&vely);
m_detenido.notify_one();
}
