CPose2D getPataMesa(){
FILE* file=fopen("patamesa.dat","r");
CPose2D pataMesa;
int nDatos(0);
if(file){
float x,y;
// Archivo existe
fscanf(file,"x:%f,y:%f",&x,&y);
pataMesa.x(x);
pataMesa.y(y);
fclose(file);
}
else{
//Archivo no existe
file=fopen("patamesa.dat","w");
for(int i=1;i<21;i++){
Detector detector;
char nombre[100];
sprintf(nombre,"/home/jplata/Eclipse/MedidasPiernas/23Mayo/laser%i.dat",i);
cout << "Fichero: " << nombre << endl;
detector.abrirFichero(nombre,false);
// Clusteres detectados
vector<Cluster> piernas=detector.clusterizar(0.1,3);
detector.printClusters(piernas);
// El ultimo cluster corresponde a la pata de la mesa en las primeras muestras
nDatos++;
pataMesa+=piernas.back().getCentro();
}
pataMesa.x(pataMesa.x()/nDatos);
pataMesa.y(pataMesa.y()/nDatos);
fprintf(file,"x:%f,y:%f",pataMesa.x(),pataMesa.y());
fclose(file);
}
return pataMesa;
}
int main( int argc, const char* argv[] )
{
vector<double> mx,my,px,py;
CDisplayWindowPlots medidasPlot("Medidas");
CDisplayWindowPlots clusterPlot("Cluster");
CDisplayWindowPlots piernasPlot("Piernas");
CDisplayWindowPlots personasPlot("Personas");
vector<CPose2D>* puntos;
CTicTac timer;
Detector detector;
Tracker seguidor;
bool filtroInicializado=false;
// Cargar modelo SVM
struct svm_model *model=svm_load_model("svm_model");
struct svm_node *instancia;
double target;
// datos de cada instancia, 4 en total: 3 carcateristicas más -1 indicando fin
instancia=Malloc(struct svm_node,4);
medidasPlot.hold_on();
double limits[] = {0,2,-2,2};
vector<double> limites (limits, limits + 4);
for(int i=0;i<24;i++){
timer.Tic();
char nombre[100];
sprintf(nombre,"/home/jplata/Eclipse/MedidasPiernas/12Julio/200ms/raw_laser%i.dat",i);
cout << "Fichero: " << nombre << endl;
// Comprobar existencia del archivo
FILE* file=fopen(nombre,"r");
if(!file){
cout << "¡¡¡¡Archivo no encontrado!!! Continuar con el siguiente" << endl;
continue;
}
detector.abrirFichero(nombre,false);
// Medidas
puntos=detector.getPuntos();
Eigen::MatrixXf rectas=detector.eliminarRectas(30,181);
vector<Cluster> piernas=detector.clusterizar(0.1,3);;
mx.clear();
my.clear();
for(unsigned int i=0;i<puntos->size();i++){
mx.push_back(puntos->at(i).x());
my.push_back(puntos->at(i).y());
}
medidasPlot.clear();
string fileName(nombre);
medidasPlot.setWindowTitle("Medidas - " + fileName.substr(fileName.find_last_of("/")+1));
medidasPlot.plot(mx,my,".b2");
for(int j=0;j < rectas.rows();j++){
Grafico::dibujarLinea(&medidasPlot,rectas(j,0),rectas(j,1),limites);
}
clusterPlot.clear();
clusterPlot.setWindowTitle("Cluster - " + fileName.substr(fileName.find_last_of("/")+1));
clusterPlot.hold_on();
piernasPlot.clear();
piernasPlot.setWindowTitle("Piernas - " + fileName.substr(fileName.find_last_of("/")+1));
piernasPlot.hold_on();
personasPlot.clear();
personasPlot.setWindowTitle("Personas - " + fileName.substr(fileName.find_last_of("/")+1));
personasPlot.hold_on();
// Obtengo puntos clusters
string formato[2];
formato[0]=".r2";
formato[1]=".b2";
int f=0;
for(int j=0;j < piernas.size();j++){
puntos=piernas[j].getPuntos();
px.clear();
py.clear();
for(unsigned int k=0;k<puntos->size();k++){
px.push_back(puntos->at(k).x());
py.push_back(puntos->at(k).y());
}
clusterPlot.plot(px,py,formato[f%2]);
// Determinar si es pierna o no
instancia[0].index=1;
instancia[1].index=2;
instancia[2].index=3;
instancia[3].index=-1;
instancia[0].value=piernas[j].getContorno();
instancia[1].value=piernas[j].getAncho();
instancia[2].value=piernas[j].getProfundidad();
target=svm_predict(model,instancia);
if(target==1){
// El clasificador SVM lo reconoce como pierna
piernasPlot.plot(px,py,formato[j%2]);
}
else{
// No es una pierna, lo elimino del vector
piernas.erase(piernas.begin()+j);
j--;
}
f++;
}
vector<CPose2D> personas=detector.buscarPersonas(piernas);
cout << "Personas detectadas: " << personas.size() << endl;
detector.printClusters(piernas);
px.clear();
py.clear();
for(int k=0;k < personas.size(); k++){
px.push_back(personas[k].x());
py.push_back(personas[k].y());
}
piernasPlot.plot(px,py,".c4");
personasPlot.plot(px,py,".r4");
// Filtro de Partículas
if(!filtroInicializado){
// Filtro no inicializado aún
// Compruebo si se ha detectado persona
if(!personas.empty()){
seguidor.inicializar(50,personas[0]);
filtroInicializado=true;
seguidor.drawParticles(&personasPlot);
}
}
else{
CPose2D objetivo;
// Filtro ya inicializado
if(personas.empty()){
// No se ha detectado persona
objetivo=seguidor.obtenerPosicionEstimada(CPose2D(),false);
}
else{
objetivo=seguidor.obtenerPosicionEstimada(personas[0],true);
}
seguidor.drawParticles(&personasPlot);
px.clear();
py.clear();
px.push_back(objetivo.x());
py.push_back(objetivo.y());
personasPlot.plot(px,py,".k8");
}
medidasPlot.axis(-0.5,4,-4,4);
clusterPlot.axis(-0.5,3,-3,3);
piernasPlot.axis(-0.5,3,-3,3);
personasPlot.axis(-0.5,3,-3,3);
cout << "Tiempo total (ms): " << timer.Tac()*1000 << endl;
cout << "Presione cualquier tecla para pasar a la siguiente muestra" << endl;
mrpt::system::os::getch();
}
//print_vector("%f\t",ancho);
//plot1.axis(-0.1,0.5,-0.1,0.5);
//plot2.axis(-0.1,0.5,-0.1,0.5);
cout << "Presione cualquier tecla para terminar:" << endl;
mrpt::system::os::getch();
}