/* +---------------------------------------------------------------------------+
| The Mobile Robot Programming Toolkit (MRPT) C++ library |
| |
| http://mrpt.sourceforge.net/ |
| |
| Copyright (C) 2005-2010 University of Malaga |
| |
| This software was written by the Machine Perception and Intelligent |
| Robotics Lab, University of Malaga (Spain). |
| Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
| |
| This file is part of the MRPT project. |
| |
| MRPT is free software: you can redistribute it and/or modify |
| it under the terms of the GNU General Public License as published by |
| the Free Software Foundation, either version 3 of the License, or |
| (at your option) any later version. |
| |
| MRPT 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 General Public License for more details. |
| |
| You should have received a copy of the GNU General Public License |
| along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
| |
+---------------------------------------------------------------------------+ */
#include <mrpt/config.h>
#if MRPT_HAS_OPENCV
#define CV_NO_CVV_IMAGE // Avoid CImage name crash
#include <cv.h>
#include <highgui.h>
#ifdef CImage // For old OpenCV versions (<=1.0.0)
#undef CImage
#endif
#endif
#ifdef _CH_
#pragma package <opencv>
#endif
#define CV_NO_BACKWARD_COMPATIBILITY
#ifndef _EiC
#include <stdio.h>
#include <ctype.h>
#endif
#include <mrpt/utils.h>
#include <mrpt/slam.h>
#include <mrpt/hwdrivers.h>
#include <mrpt/vision.h>
using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::system;
using namespace mrpt::slam;
using namespace mrpt::utils;
using namespace mrpt::gui;
using namespace std;
typedef vector_uint patch_ind;
typedef vector<patch_ind> indices_list;
typedef vector_byte triangle;
typedef vector<triangle> triangle_list;
typedef std::vector<CvPoint2D32f> CvPoint2D32fVector;
#define cvSet1 cvSetReal1D
#define cvGet1 cvGetReal1D
#define cvSet2 cvSetReal2D
#define cvGet2 cvGetReal2D
//Sequence has a pointer to an image, and a list of triangle-patches, a list of patch-indices and a mask that contain all triangles.
struct img_seq_data {
img_seq_data() : img(NULL), mask(NULL)
{
}
triangle_list tri_list;
indices_list ind_list;
IplImage *img;
CvMat *mask;
};
vector<img_seq_data> data_img;
CDisplayWindow win("Reference"), win1("Video");//, win2("Patch-Tform"), win3("Patch_LM"); //win4("AUX");
CDisplayWindow win2("Pruebas");
unsigned int f, t;
//IplImage *current;
CArrayDouble<6> C;
CImage img_aux, imgColor(1,1,3);
std::vector<CArrayDouble<3> > po1, po2, po3;
CArrayDouble<3> v_init1, v_init2, v_init3;
unsigned int v1, v2, v3;
CvPoint2D32f* center = 0;
//vector_double po1(3), po2(3), po3(3);
//-------------------------------------
//| NCC-VECTORPATCH FUNCTION |
//-------------------------------------
//Normalised Cross Correlation between two vector patches
//The Matlab code for that:
//a = a - mean(a);
//b = b - mean(b);
//r = sum(a.*b)/sqrt(sum(a.*a)*sum(b.*b));
//
//double ncc_vector( const vector_byte &patch1, const vector_byte &patch2 )
//{
// ASSERT_( patch1.size()==patch2.size() );
//
// double numerator = 0, sum_a = 0, sum_b = 0, result, a_mean, b_mean;
// a_mean = patch1.mean();
// b_mean = patch2.mean();
//
// for(unsigned int i=0; i<patch1.size(); i++)
// {
// numerator = numerator + (patch1[i]-a_mean)*(patch2[i]-b_mean);
// sum_a = sum_a + square(patch1[i]-a_mean);
// sum_b = sum_b + square(patch2[i]-b_mean);
// }
// result=numerator/sqrt(sum_a*sum_b);
// return result;
//}
//--------------------------------
//| COST FUNCTION |
//---------------------------------
inline size_t floorS(double d) {
return static_cast<size_t>(floor(d));
}
//typedef CLevenbergMarquardtTempl<vector_double,vector<img_seq_data> > CMyLevMarq;
typedef CLevenbergMarquardtTempl<vector_double, img_seq_data > CMyLevMarq;
void CostFunction(const vector_double &X, const img_seq_data &last, vector_double &cost)
{
CMatrixDouble33 tform;
vector_byte patch2;
vector_word xy1(3); xy1[2]=1;
vector_double xy2(3);
vector_float xx(3), yy(3);
unsigned int x, y, i;
vector_double pt1(3), pt2(3), pt3(3);
float d1_2, d2_2, d3_2;
//CImage last_img;
bool inside_img = true;
CTicTac tictac;
try
{
tictac.Tic();
cost.resize(1);
//Most generic case (8 parameters)
tform.set_unsafe(0,0,X[0]); tform.set_unsafe(0,1,X[1]); tform.set_unsafe(0,2,X[2]);
tform.set_unsafe(1,0,X[3]); tform.set_unsafe(1,1,X[4]); tform.set_unsafe(1,2,X[5]);
tform.set_unsafe(2,0,X[6]); tform.set_unsafe(2,1,X[7]); tform.set_unsafe(2,2,1);
tform.multiply_Ab(po1[t],pt1);
pt1[0] = pt1[0]/pt1[2]; pt1[1] = pt1[1]/pt1[2];
tform.multiply_Ab(po2[t],pt2);
pt2[0] = pt2[0]/pt2[2]; pt2[1] = pt2[1]/pt2[2];
tform.multiply_Ab(po3[t],pt3);
pt3[0] = pt3[0]/pt3[2]; pt3[1] = pt3[1]/pt3[2];
xx[0] = pt1[0]; xx[1] = pt2[0]; xx[2] = pt3[0];
yy[0] = pt1[1]; yy[1] = pt2[1]; yy[2] = pt3[1];
//Show Image and points
//win3.showImageAndPoints( last.img, xx, yy);
//mrpt::system::pause();
d1_2 = square(pt1[0]-C[0]) + square(pt1[1]-C[1]);
d2_2 = square(pt2[0]-C[2]) + square(pt2[1]-C[3]);
d3_2 = square(pt3[0]-C[4]) + square(pt3[1]-C[5]);
if( d1_2>400 || d2_2>400 || d3_2>400 ){
cost[0] = 2;
cout<< tictac.Tac() <<" segs (CostFunction)." <<endl;
return;
}
//Load Patch
for ( i=0; i<data_img[0].tri_list[t].size(); i++)
{
x = data_img[0].ind_list[t][i]%data_img[0].img->widthStep + 1;
y = data_img[0].ind_list[t][i]/data_img[0].img->widthStep + 1;
xy1[0] = x; xy1[1] = y;
tform.multiply_Ab(xy1,xy2); //xy2 = xy1 * tform;
xy2[0]/=xy2[2]; xy2[1]/=xy2[2]; //xy2[2]/=xy2[2]; //SET THE LAST ELEMENT TO 1.
if ( xy2[0]>(data_img[0].img->width-2) || xy2[0]<1 || xy2[0]>(data_img[0].img->height-2) || xy2[0]<1 ){
inside_img = false;
break;
}
patch2.resize(data_img[0].tri_list[t].size());
patch2[i] = round((unsigned char)(last.img->imageData[floorS(xy2[0]-1)+(floorS(xy2[1])-1)*data_img[0].img->widthStep])*(floorS(xy2[0])+1-xy2[0])*(floorS(xy2[1])+1-xy2[1]) +
(unsigned char)(last.img->imageData[floorS(xy2[0]-1)+floorS(xy2[1])*data_img[0].img->widthStep])*(floorS(xy2[0])+1-xy2[0])*(xy2[1]-floorS(xy2[1])) +
(unsigned char)(last.img->imageData[floorS(xy2[0])+(floorS(xy2[1])-1)*data_img[0].img->widthStep])*(xy2[0]-floorS(xy2[0]))*(floorS(xy2[1])+1-xy2[1]) +
(unsigned char)(last.img->imageData[floorS(xy2[0])+floorS(xy2[1])*data_img[0].img->widthStep])*(xy2[0]-floorS(xy2[0]))*(xy2[1]-floorS(xy2[1])));
}
if(inside_img)
cost[0] = 1 - ncc_vector(data_img[0].tri_list[t], patch2);
else
cost[0] = 2;
cout<< tictac.Tac() <<" segs (CostFunction)." <<endl;
}
catch(std::exception &e)
{
std::cout << e.what() << std::endl;
std::cout << "Cost Function couldn't be evaluated" << endl;
}
catch(...)
{
std::cout << "Runtime error." << std::endl;
std::cout << "Cost Function couldn't be evaluated" << endl;
}
}
//--------------------------------
//| NEAREST ELEMENT |
//---------------------------------
//int nearest(
//--------------------------------
//| AREA OF TRIANGLE |
//---------------------------------
double TriangleArea(double ax, double ay, double bx, double by, double cx, double cy)
{
double area;
area = 0.5*sqrt( (square(ax-bx)+square(ay-by))*(square(ax-cx)+square(ay-cy)) - square((ax-bx)*(ax-cx)+(ay-by)*(ay-cy)) );
return area;
}
double TriangleArea(CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c)
{
double area;
area = 0.5*sqrt( (square(a.x-b.x)+square(a.y-b.y))*(square(a.x-c.x)+square(a.y-c.y)) - square((a.x-b.x)*(a.x-c.x)+(a.y-b.y)*(a.y-c.y)) );
return area;
}
//--------------------------------
//| POINT IN TRIANGLE |
//---------------------------------
bool PointInTriangle(CvPoint2D32f p, CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c)
{
double cp1, cp2;
bool s1, s2, s3;
// same_side(p,a, b,c)
cp1 = (c.x - b.x) * (p.y - b.y) - (c.y - b.y) * (p.x - b.x);
cp2 = (c.x - b.x) * (a.y - b.y) - (c.y - b.y) * (a.x - b.x);
s1 = (cp1 * cp2) >= 0;
// same_side(p,b, a,c)
cp1 = (c.x - a.x) * (p.y - a.y) - (c.y - a.y) * (p.x - a.x);
cp2 = (c.x - a.x) * (b.y - a.y) - (c.y - a.y) * (b.x - a.x);
s2 = (cp1 * cp2) >= 0;
// same_side(p,c, a,b)
cp1 = (b.x - a.x) * (p.y - a.y) - (b.y - a.y) * (p.x - a.x);
cp2 = (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
s3 = (cp1 * cp2) >= 0;
return( s1 & s2 & s3 );
}
//--------------------------------
//| LOAD PATCHES |
//---------------------------------
//loadPatches(CvMat *Ptos, CvMat *mesh)
//Takes a matrix with the image points, and another matrix with the triangle mesh to build a mask,
//which assign to each position of the image the number of the triangle of the referred mesh in which it is,
//or 0 in the case that it doesn't belong to the mesh.
//At the same time, it loads the patch vectors of each triangle. Taking the original image as a global variable.
void loadPatches(CFeatureList &Ptos, CMatrix &mesh, CvMat** mask, triangle_list &tri, indices_list &ind, IplImage *ipl)
{
*mask = cvCreateMat(ipl->height, ipl->width, CV_16UC1); //Para cuando mask es un puntero
cvZero(*mask);
int idx1, idx2, idx3;
CvPoint2D32f p1, p2, p3;
int num_facets = mesh.getRowCount();
for(int i = 1; i <= num_facets; i++) //CAMBIAR ESTRUCTURA, A UN UNICO FOR PARA EL RECORRIDO DE LA IMAGEN
{
idx1 = mesh(i-1,0)-1;
idx2 = mesh(i-1,1)-1;
idx3 = mesh(i-1,2)-1;
p1 = cvPoint2D32f( Ptos[idx1]->x, Ptos[idx1]->y);
p2 = cvPoint2D32f( Ptos[idx2]->x, Ptos[idx2]->y);
p3 = cvPoint2D32f( Ptos[idx3]->x, Ptos[idx3]->y);
//idx1 = (int)cvGet2D(mesh, (i - 1), 0) - 1;
//idx2 = (int)cvGet2D(mesh, (i - 1), 1) - 1;
//idx3 = (int)cvGet2D(mesh, (i - 1), 2) - 1;
//p1 = cvPoint2D32f( cvGet2D(points, idx1, 0), cvGet2(points, idx1, 1));
//p2 = cvPoint2D32f( cvGet2D(points, idx2, 0), cvGet2(points, idx2, 1));
//p3 = cvPoint2D32f( cvGet2D(points, idx3, 0), cvGet2(points, idx3, 1));
for(int y = 1; y <= ipl->height; y++)
{
for(int x = 1; x <= ipl->width; x++)
{
if(PointInTriangle(cvPoint2D32f(x, y), p1, p2, p3))
{
cvSet2((*mask), (y - 1), (x - 1), (double)i );
tri[i-1].push_back( ipl->imageData[ x - 1 + (y - 1)*ipl->widthStep ] );
ind[i-1].push_back( x - 1 + (y - 1)*ipl->widthStep );
}
}
}
}
}
//--------------------------------
//| FOLLOW-PATCH |
//---------------------------------
void followPatch(CMatrixDouble33 &tform, vector_byte &tri, IplImage *ipl)
{
int x, y;
vector_double xy1(3), xy2(3);
tri.resize(data_img[0].tri_list[t].size());
for(unsigned int i = 0; i < data_img[0].ind_list[t].size(); i++)
{
x = data_img[0].ind_list[t][i] % data_img[0].img->widthStep +1;
y = data_img[0].ind_list[t][i] / data_img[0].img->widthStep +1;
xy1[0] = x; xy1[1] = y; xy1[2] = 1;
tform.multiply_Ab(xy1,xy2);
xy2[0]/=xy2[2]; xy2[1]/=xy2[2]; //xy2[2]/=xy2[2];
//ind[i] = ( xy2[0] + xy2[1]*data_img[0].img->widthStep );
tri[i] = round((unsigned char)(ipl->imageData[floorS(xy2[0]-1)+(floorS(xy2[1])-1)*data_img[0].img->widthStep])*(floorS(xy2[0])+1-xy2[0])*(floorS(xy2[1])+1-xy2[1]) +
(unsigned char)(ipl->imageData[floorS(xy2[0]-1)+floorS(xy2[1])*data_img[0].img->widthStep])*(floorS(xy2[0])+1-xy2[0])*(xy2[1]-floorS(xy2[1])) +
(unsigned char)(ipl->imageData[floorS(xy2[0])+(floorS(xy2[1])-1)*data_img[0].img->widthStep])*(xy2[0]-floorS(xy2[0]))*(floorS(xy2[1])+1-xy2[1]) +
(unsigned char)(ipl->imageData[floorS(xy2[0])+floorS(xy2[1])*data_img[0].img->widthStep])*(xy2[0]-floorS(xy2[0]))*(xy2[1]-floorS(xy2[1])));
}
}
//--------------------------------
//| EXTRACT FEATURES |
//---------------------------------
//CFeatureList ExtractFeaturesFrame1(CImage img)
//{
// //CDisplayWindow wind("1st Frame");
// CFeatureExtraction fExt;
// CFeatureList features, f_out;
// //CTicTac tictac;
//
// cout << "1ST FRAME LOADED" << endl;
//
// //tictac.Tic();
//
// //KLT
// cout << "Extracting KLT features ... [boot.txt]\n";
// fExt.options.featsType = featKLT;
// fExt.options.harrisOptions.tile_image = false;
// fExt.options.patchSize=0;
// fExt.detectFeatures( img, features, 0,1000);
//
//
////PRINT THE KLT FEATURES IN THE IMAGE
// //char* cstr = new char [3];
// //for(unsigned int i = 0; i < features.size(); i++)
// //{
// // sprintf( cstr, "%d", i );
// // img.textOut( features.getByID(i)->x, features.getByID(i)->y, cstr, 255 );
// //}
// ////wind.showImageAndPoints(img, features);
// ////mrpt::system::pause();
// //delete cstr;
//
// unsigned int chosen[5] = { 14, 18, 34, 64, 70 };
// for(unsigned int i = 0; i < 5; i++)
// f_out.push_back(features.getByID(chosen[i]));
//
// //wind.showImageAndPoints(img, f_out);
// //mrpt::system::pause();
//
// //// CODE TO GET RECURSIVELY 2 VECTORS WITH THE FEATURES COORDINATES
// //vector_float x, y;
// //x.resize(features.size());
// //y.resize(features.size());
// //for(unsigned int i = 0; i < x.size(); i++)
// //{
// // x[i] = features.getByID(i)->x;
// // y[i] = features.getByID(i)->y;
// //}
// //wind.showImageAndPoints(img, x, y);
//
//
//
//
//
// ////HARRIS + SURF
// //cout << "Extracting Harris-SURF features ... [boot.txt]\n";
// //fExt.options.featsType = featHarris;
// //fExt.options.harrisOptions.tile_image = false;
// //fExt.options.patchSize=0;
// //fExt.detectFeatures( img, features, 0,1000 );
// //fExt.computeDescriptors( img, features, descSURF );
// //wind.showImageAndPoints(img, features);
// //mrpt::system::pause();
//
// //char* cstr = new char [3];
// //for(unsigned int i = 0; i < features.size(); i++)
// //{
// // sprintf( cstr, "%d", i );
// // img.textOut( features.getByID(i)->x, features.getByID(i)->y, cstr, 255 );
// //}
// //wind.showImageAndPoints(img, features);
// //mrpt::system::pause();
// //delete cstr;
//
// //unsigned int chosen[5] = { 14, 18, 34, 64, 70 };
// //for(unsigned int i = 0; i < 5; i++)
// // f_out.push_back(features.getByID(chosen[i]));
//
// return f_out;
//}
void detectClusteres(CFeatureList &ptos, CvMat* samples, CvMat* clusters, CvMat* centers, int cluster_count )
{
for(unsigned int i=0; i<ptos.size(); i++)
{
cvSet2D( samples, i, 0, cvScalar(ptos[i]->x) );
cvSet2D( samples, i, 1, cvScalar(ptos[i]->y) );
}
cvKMeans2( samples, cluster_count, clusters, cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 10, 1.0 ) , 1, 0, 0, centers );
}
/*---------------------------------------------------------------
Aux. functions needed by ransac_homographies
---------------------------------------------------------------*/
namespace mrpt
{
namespace math
{
template <typename T>
void ransacHomography_fit(
const CMatrixTemplateNumeric<T> &allData,
const vector_size_t &useIndices,
vector< CMatrixTemplateNumeric<T> > &fitModels )
{
ASSERT_(useIndices.size()==3);
try
{
//BUILD AFFINE TRANSFORMATION
CMatrixDouble66 D;
CArrayDouble<6> A;
double dett;
CArrayDouble<3> pt1, pt2, pt3;
pt1[0] = allData(0,useIndices[0]); pt1[1] = allData(1,useIndices[0]); pt1[2] = 1;
pt2[0] = allData(0,useIndices[1]); pt2[1] = allData(1,useIndices[1]); pt2[2] = 1;
pt3[0] = allData(0,useIndices[2]); pt3[1] = allData(1,useIndices[2]); pt3[2] = 1;
D.fill(0); //D is the inverse of B, B is the original matrix of pts
dett = (-pt1[0]*pt3[1]+pt1[0]*pt2[1]+pt3[1]*pt2[0]-pt2[1]*pt3[0]-pt2[0]*pt1[1]+pt3[0]*pt1[1]); //This is the square root of minus det(B) -> sqtr(-det(B))
D.set_unsafe(0,0, (-pt3[1]+pt2[1])/dett);
D.set_unsafe(0,2, -(pt1[1]-pt3[1])/dett);
D.set_unsafe(0,4, (-pt2[1]+pt1[1])/dett);
D.set_unsafe(1,0, -(pt2[0]-pt3[0])/dett);
D.set_unsafe(1,2, (pt1[0]-pt3[0])/dett);
D.set_unsafe(1,4, -(pt1[0]-pt2[0])/dett);
D.set_unsafe(2,0, (-pt2[1]*pt3[0]+pt3[1]*pt2[0])/dett);
D.set_unsafe(2,2, -(pt1[0]*pt3[1]-pt3[0]*pt1[1])/dett);
D.set_unsafe(2,4, (pt1[0]*pt2[1]-pt2[0]*pt1[1])/dett);
D.set_unsafe(3,1, (-pt3[1]+pt2[1])/dett);
D.set_unsafe(3,3, -(pt1[1]-pt3[1])/dett);
D.set_unsafe(3,5, (-pt2[1]+pt1[1])/dett);
D.set_unsafe(4,1, -(pt2[0]-pt3[0])/dett);
D.set_unsafe(4,3, (pt1[0]-pt3[0])/dett);
D.set_unsafe(4,5, -(pt1[0]-pt2[0])/dett);
D.set_unsafe(5,1, (-pt2[1]*pt3[0]+pt3[1]*pt2[0])/dett);
D.set_unsafe(5,3, -(pt1[0]*pt3[1]-pt3[0]*pt1[1])/dett);
D.set_unsafe(5,5, (pt1[0]*pt2[1]-pt2[0]*pt1[1])/dett);
//D.saveToTextFile("d:\\D.txt");
C[0] = allData(2,useIndices[0]);
C[1] = allData(3,useIndices[0]);
C[2] = allData(2,useIndices[1]);
C[3] = allData(3,useIndices[1]);
C[4] = allData(2,useIndices[2]);
C[5] = allData(3,useIndices[2]);
D.multiply_Ab(C,A);
fitModels.resize(1);
CMatrixTemplateNumeric<T> &TForm = fitModels[0]; //CMatrixDouble33 TForm;
TForm.setSize(3,3);
TForm.set_unsafe(0,0, A[0]);
TForm.set_unsafe(0,1, A[1]);
TForm.set_unsafe(0,2, A[2]);
TForm.set_unsafe(1,0, A[3]);
TForm.set_unsafe(1,1, A[4]);
TForm.set_unsafe(1,2, A[5]);
TForm.set_unsafe(2,0, 0);
TForm.set_unsafe(2,1, 0);
TForm.set_unsafe(2,2, 1);
}
catch(exception &)
{
fitModels.clear();
return;
}
}
template <typename T>
void ransacHomography_distance(
const CMatrixTemplateNumeric<T> &allData,
const vector< CMatrixTemplateNumeric<T> > & testModels,
const T distanceThreshold,
unsigned int & out_bestModelIndex,
vector_size_t & out_inlierIndices )
{
ASSERT_( testModels.size()==1 )
out_bestModelIndex = 0;
double d;
const CMatrixDouble &M = testModels[0];
ASSERT_( size(M,1)==3 && size(M,2)==3 )
CArrayDouble<3> pt, pt_r;
const size_t N = size(allData,2);
out_inlierIndices.clear();
out_inlierIndices.reserve(100);
for (size_t i=0;i<N;i++)
{
pt[0] = allData.get_unsafe(0,i);
pt[1] = allData.get_unsafe(1,i);
pt[2] = 1;
testModels[0].multiply_Ab( pt, pt_r );
d = sqrt( square( pt_r[0] - allData(2,i) ) + square(pt_r[1] - allData(3,i) ) );
if ( d < distanceThreshold )
out_inlierIndices.push_back(i);
}
//Increase Homography Inliers
if (out_inlierIndices.size() >= 15 )
{
//A)Search and store natural vertices in clockwise direction (greatest and smallest x & y coordinates)
vector_size_t vertices(4,out_inlierIndices[0]);
for (size_t i=1;i<out_inlierIndices.size();i++)
{
if( allData(3,out_inlierIndices[i]) > allData(3,vertices[0]) )
vertices[0] = out_inlierIndices[i];
if( allData(2,out_inlierIndices[i]) > allData(2,vertices[1]) )
vertices[1] = out_inlierIndices[i];
if( allData(3,out_inlierIndices[i]) < allData(3,vertices[2]) )
vertices[2] = out_inlierIndices[i];
if( allData(2,out_inlierIndices[i]) < allData(2,vertices[3]) )
vertices[3] = out_inlierIndices[i];
}
//std::set<size_t> v; //Este tampoco me vale porque reordena los indices
//v.insert( vertices.begin(), vertices.end() );
////ESTA FORMA DE BORRAR REPETICIONES GENERA ERRORES "ALEATORIOS" -> HAY QUE CAMBIARLA
//for (vector_size_t::iterator it1=vertices.begin(); it1!=(vertices.end()-1); ++it1) //Erase repeated vertex //poly.removeRepeatedVertices();
//{
// for (vector_size_t::iterator it2=it1+1; it2!=vertices.end(); ++it2)
// {
// if( (*it1)==(*it2) )
// {
// it1 = vertices.erase(it1);
// break;
// }
// }
//}
vector_size_t::iterator it1;
for (size_t i=0; i<vertices.size()-1; ++i) //Erase repeated vertex //poly.removeRepeatedVertices();
{
for (size_t j=i+1; j<vertices.size(); ++j)
{
if( vertices[i]==vertices[j] )
{
it1 = vertices.begin()+j;
vertices.erase(it1);
break;
}
}
}
//Look for inliers out the initial polygon and transform the polygon to contain all
TPolygon2D poly;
vector<TPoint2D> vertex;
TPoint2D point;
size_t seg_id, point_id;
double dist, max_dist; //dist & max_dist refer to the nearest line of the contour
bool vert;
vertex.resize( vertices.size() );
for (size_t i=0;i<vertices.size();i++)
{
vertex[i] = TPoint2D( allData(2,vertices[i]), allData(3,vertices[i]) );
}
poly = TPolygon2D( vertex );
////PLOT INITIAL POLYGON
//img_aux.colorImage( imgColor ); // Create a colorimage
//for(size_t j=0;j<out_inlierIndices.size(); j++)
//{
// imgColor.cross(round(allData(2,out_inlierIndices[j])),round(allData(3,out_inlierIndices[j])),TColor::blue,'+');
// //imgColor.textOut(round(allData(2,out_inlierIndices[j])),round(allData(3,out_inlierIndices[j])), format("%d", out_inlierIndices[j]), 255 );
//}
//for(size_t j=0;j<(vertices.size()-1); j++)
//{
// imgColor.line( allData(2,vertices[j]), allData(3,vertices[j]), allData(2,vertices[j+1]), allData(3,vertices[j+1]), 255 );
// imgColor.textOut(round(allData(2,vertices[j])),round(allData(3,vertices[j])), format("%d", vertices[j]), 255 );
//}
//imgColor.textOut(round(allData(2,vertices[vertices.size()-1])),round(allData(3,vertices[vertices.size()-1])), format("%d", vertices[vertices.size()-1]), 255 );
//imgColor.textOut(round(allData(2,out_inlierIndices[0])),round(allData(3,out_inlierIndices[0])), format("%d", out_inlierIndices[0]), 255 );
//imgColor.line( allData(2,vertices[vertices.size()-1] ), allData(3,vertices[vertices.size()-1] ), allData(2,vertices[0]), allData(3,vertices[0]), 255 );
//win2.showImage(imgColor);
//mrpt::system::pause();
do{
max_dist = 0;
//Start adding the farest points to the polygon (to yhe polygon boundary)
std::vector<TSegment2D> sgs;
poly.getAsSegmentList(sgs);
//TPoint2D cntr;
//poly.getCenter(cntr);
for (size_t i=0;i<out_inlierIndices.size();i++) //NO SE EVALUA NADA MAS K EL 1ER PTO
{
vert = false;
for(size_t j=0;j<vertices.size();j++)
{
if( vertices[j] == out_inlierIndices[i] )
vert = true;
}
point = TPoint2D( allData(2,out_inlierIndices[i]), allData(3,out_inlierIndices[i]) );
if( !poly.contains(point) && !vert )
{
size_t j = 1;
for (vector<TSegment2D>::const_iterator it=sgs.begin();it!=sgs.end();++it) //NO SE EVALUA NADA MAS K EL 1ER PTO
{
TLine2D l = TLine2D(*it);
dist = -( l.signedDistance(point) );
if( dist > max_dist+1 ) //modifica el poligono para ptos extremadamente proximos->Evitarlo con if( dist > max_dist+1 )
{
max_dist = dist;
point_id = out_inlierIndices[i];
seg_id = j;
}
j++;
}
}
}
if( max_dist != 0 )
{
vertices.insert( vertices.begin()+seg_id, point_id );
vertex.insert( vertex.begin()+seg_id, TPoint2D( allData(2,vertices[seg_id]), allData(3,vertices[seg_id]) ) );
poly = TPolygon2D( vertex );
}
//img_aux.colorImage( imgColor ); // Create a colorimage
//for(size_t j=0;j<out_inlierIndices.size(); j++)
// imgColor.cross(round(allData(2,out_inlierIndices[j])),round(allData(3,out_inlierIndices[j])),TColor::blue,'+');
//for(size_t j=0;j<vertices.size()-1; j++)
//{
// imgColor.line( allData(2,vertices[j]), allData(3,vertices[j]), allData(2,vertices[j+1]), allData(3,vertices[j+1]), 255 );
// imgColor.textOut(round(allData(2,vertices[j])),round(allData(3,vertices[j])), format("%d", vertices[j]), 255 );
//}
//imgColor.line( allData(2,vertices[vertices.size()-1] ), allData(3,vertices[vertices.size()-1] ), allData(2,vertices[0]), allData(3,vertices[0]), 255 );
//imgColor.textOut(round(allData(2,vertices[vertices.size()-1])),round(allData(3,vertices[vertices.size()-1])), format("%d", vertices[vertices.size()-1]), 255 );
//win2.showImage(imgColor);
//mrpt::system::pause();
}while( max_dist != 0 );
img_aux.colorImage( imgColor ); // Create a colorimage
for(size_t j=0;j<out_inlierIndices.size(); j++)
imgColor.cross(round(allData(2,out_inlierIndices[j])),round(allData(3,out_inlierIndices[j])),TColor::blue,'+');
for(size_t j=0;j<vertices.size()-1; j++)
{
imgColor.line( allData(2,vertices[j]), allData(3,vertices[j]), allData(2,vertices[j+1]), allData(3,vertices[j+1]), 255 );
imgColor.textOut(round(allData(2,vertices[j])),round(allData(3,vertices[j])), format("%d", vertices[j]), 255 );
}
imgColor.line( allData(2,vertices[vertices.size()-1] ), allData(3,vertices[vertices.size()-1] ), allData(2,vertices[0]), allData(3,vertices[0]), 255 );
imgColor.textOut(round(allData(2,vertices[vertices.size()-1])),round(allData(3,vertices[vertices.size()-1])), format("%d", vertices[vertices.size()-1]), 255 );
win2.showImage(imgColor);
mrpt::system::pause();
//if( vertices.size()==2 ) //Look for the farest inlier
//{
// TSegment2D sgs = TSegment2D( TPoint2D( allData(2,vertices[0]), allData(3,vertices[0]) ), TPoint2D( allData(2,vertices[1]), allData(3,vertices[1]) ) );
// double dist, max_dist = 0;
// for (size_t i=0;i<out_inlierIndices.size();i++)
// {
// if( (i != vertices[0]) && (i != vertices[1]) )
// {
// dist = sgs.distance( TPoint2D( allData(2,out_inlierIndices[i]), allData(2,out_inlierIndices[i]) ) );
// if ( dist > max_dist )
// {
// max_dist = dist;
// vertices.push_back(out_inlierIndices[i]);
// }
// }
// }
//}
//Check the rest of points inside the polygon
size_t k = 0;
size_t outliers = 0;
vector_size_t more_inlierIndices;
dist = 0; //dist is now used to check how consistent are the rest of inside points with the homography
for (size_t i=0; i<N; i++)
{
point = TPoint2D( allData(2,i), allData(3,i) );
if( i!=out_inlierIndices[k] )
{
if( poly.contains(point) )
{
pt[0] = allData.get_unsafe(0,i);
pt[1] = allData.get_unsafe(1,i);
pt[2] = 1;
testModels[0].multiply_Ab( pt, pt_r );
d = sqrt( square( pt_r[0] - allData(2,i) ) + square(pt_r[1] - allData(3,i) ) );
if ( d < 3*distanceThreshold )
more_inlierIndices.push_back(i);
else
{
outliers++;
dist += d;
}
}
}
else
{
if( k!=out_inlierIndices.size()-1 )
k++;
}
}
dist /= outliers;
//if( outliers > 15 || dist > 5*3*distanceThreshold )
// out_inlierIndices.clear();
if( (more_inlierIndices.size()>15) && (outliers < 15) && (dist < 5*3*distanceThreshold) )
out_inlierIndices.insert( out_inlierIndices.end(), more_inlierIndices.begin(), more_inlierIndices.end() );
}
}
/** Return "true" if the selected points are a degenerate (invalid) case.
*/
template <typename T>
bool ransacHomography_degenerate(
const CMatrixTemplateNumeric<T> &allData,
const mrpt::vector_size_t &useIndices )
{
//double r_a, r_b, r_c, dist;
//r_a = ( allData.get_unsafe( 3, useIndices[0] ) - allData.get_unsafe( 3, useIndices[1] ) );
//r_b = ( allData.get_unsafe( 2, useIndices[1] ) - allData.get_unsafe( 2, useIndices[0] ) );
//r_c = ( allData.get_unsafe( 3, useIndices[1] )*allData.get_unsafe( 2, useIndices[0] ) - allData.get_unsafe( 3, useIndices[0] )*allData.get_unsafe( 2, useIndices[1] ) );
//dist = r_a*allData.get_unsafe( 2, useIndices[2] ) + r_b*allData.get_unsafe( 3, useIndices[2] ) + r_c;
//if ( dist < 3 )
// return true;
//else
return false;
}
} // end namespace
} // end namespace
/*---------------------------------------------------------------
ransac_homographies
---------------------------------------------------------------*/
//template <typename NUMTYPE>
void ransac_homographies(
CFeatureList ref,
CFeatureList vid,
vector<CMatrixTemplateNumeric<double> > &out_detected_planes,
const double threshold,
const size_t min_inliers_for_valid_plane,
vector< vector<size_t> > &plane_ind)
{
MRPT_START
ASSERT_( ref.size()==vid.size() )
if ( ref.empty() ) return;
out_detected_planes.clear();
CMatrixTemplateNumeric<double> remainingPoints( 4, ref.size() );
for(unsigned int i=0; i<ref.size(); i++)
{
remainingPoints.set_unsafe(0,i,ref[i]->x);
remainingPoints.set_unsafe(1,i,ref[i]->y);
remainingPoints.set_unsafe(2,i,vid[i]->x);
remainingPoints.set_unsafe(3,i,vid[i]->y);
}
// ---------------------------------------------
// For each plane:
// ---------------------------------------------
mrpt::vector_size_t other_inliers, other_inliers2;
for (;;)
{
mrpt::vector_size_t this_best_inliers;
CMatrixTemplateNumeric<double> this_best_model;
math::RANSAC_Template<double>::execute(
remainingPoints,
ransacHomography_fit,
ransacHomography_distance,
ransacHomography_degenerate,
threshold,
3, // Minimum set of points
this_best_inliers,
this_best_model,
true, // Verbose
0.999 // Prob. of good result
);
// Is this plane good enough?
if (this_best_inliers.size()>=min_inliers_for_valid_plane)
{
// Add this plane to the output list:
out_detected_planes.push_back(this_best_model);
//plane_ind.push_back(this_best_inliers);
if ( out_detected_planes.size() == 1 )
{
plane_ind.push_back(this_best_inliers);
other_inliers = this_best_inliers;
}
else
{
mrpt::vector_size_t this_best_inliers_r; //Restored Index of Inliers
this_best_inliers_r.resize( this_best_inliers.size() );
unsigned int j = 0;
for(int i=0; i<this_best_inliers.size(); i++)
{
if( j==other_inliers.size() )
this_best_inliers_r[i] = this_best_inliers[i] + j;
else if( ( this_best_inliers[i] + j < other_inliers[j] ) )
this_best_inliers_r[i] = this_best_inliers[i] + j;
else
{
j++;
i--;
}
//if(i==(this_best_inliers.size()-2) )
// mrpt::system::pause();
}
plane_ind.push_back(this_best_inliers_r);
//Upload other_inliers
unsigned int i = 0;
j = 0;
other_inliers2.resize( other_inliers.size() + this_best_inliers_r.size() );
while( (j+i) < other_inliers2.size() )
{
if ( j==this_best_inliers_r.size() ){
other_inliers2[i+j] = other_inliers[i];
i++;
}else if (i==other_inliers.size()){
other_inliers2[i+j] = this_best_inliers_r[j];
j++;
}else if ( (this_best_inliers_r[j] < other_inliers[i]) ){
other_inliers2[i+j] = this_best_inliers_r[j];
j++;
}else{
other_inliers2[i+j] = other_inliers[i];
i++;
}
//if( (j+i)==(other_inliers2.size()-2) )
// mrpt::system::pause();
}
other_inliers = other_inliers2;
}
//SHOW IMAGE AND SET OF INLIERS
//img_aux.colorImage( imgColor ); // Create a colorimage
//for(size_t j=0;j<this_best_inliers.size(); j++)
// imgColor.cross(round(remainingPoints(2, this_best_inliers[j])),round(remainingPoints(3, this_best_inliers[j])),TColor::blue,'+');
//win2.showImage(imgColor);
//mrpt::system::pause();
// Discard the selected points so they are not used again for finding subsequent planes:
remainingPoints.removeColumns(this_best_inliers);
}
else
{
break; // Do not search for more planes.
}
}
MRPT_END
}
CFeatureList ExtractFeaturesFrame1(IplImage *img, CFeatureList &f_out)//, double &average_x, double &average_y, double &std_x, double &std_y)
{
CFeatureList features;
//CTicTac tictac;
cout << "1ST FRAME LOADED" << endl;
//tictac.Tic();
//KLT
cout << "Extracting KLT features ... \n";
//fExt.options.featsType = featKLT;
//fExt.options.harrisOptions.tile_image = false;
//fExt.options.patchSize=0;
//fExt.detectFeatures( img, features, 0,1000);
IplImage *grey = 0, *prev_grey = 0; //, *pyramid = 0, *prev_pyramid = 0, *image = 0
int win_size = 11;
const int MAX_COUNT = 1000;
//std::vector<CvPoint2D32fVector> points(2);
//unsigned nFeatures = 5;
//points[0].resize(nFeatures);
//points[1].resize(nFeatures);
//std::vector<char> status;
CvPoint2D32f* points[2] = {0,0};//, *swap_points
char* status = 0;
points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
status = (char*)cvAlloc(MAX_COUNT);
grey = cvCreateImage( cvGetSize(img), 8, 1 );
IplImage* eig = cvCreateImage( cvGetSize(grey), 32, 1 );
IplImage* temp = cvCreateImage( cvGetSize(grey), 32, 1 );
double quality = 0.01;
double min_distance = 10;
//cvCopy( frame, image, 0 );
//cvCvtColor( img, grey, CV_BGR2GRAY );
cvCopy( img, grey, 0 );
int count = 0;
count = MAX_COUNT;
cvGoodFeaturesToTrack( grey, eig, temp, points[1], &count,
quality, min_distance, 0, 3, 0, 0.04 );
cvFindCornerSubPix( grey, &points[1][0], count,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
cvReleaseImage( &eig );
cvReleaseImage( &temp );
vector_double xx, yy;
xx.resize(count); yy.resize(count);
// Array conversion OpenCV->MRPT
CFeatureList::iterator itFeat;
for( int i = 0; i < count ; i++ )
{
CFeaturePtr feat = CFeature::Create();
feat->x = points[1][i].x;
feat->y = points[1][i].y;
feat->ID = i;
feat->type = featKLT;
features.push_back(feat);
xx[i] = feat->x; yy[i] = feat->y;
} // end for
////CLUSTERING
//int num_clusteres = 15;
//int sample_count = features.size();
//CvMat* samples = cvCreateMat( sample_count, 2, CV_32F );
// CvMat* clusters = cvCreateMat( sample_count, 1, CV_32S );
//CvMat* centers = cvCreateMat( num_clusteres, 2, CV_32F );
//detectClusteres( features, samples, clusters, centers , num_clusteres );
//CImage image;
//image.loadFromIplImage(img);
//for(int i=0; i<num_clusteres; i++)
// image.cross( cvGetReal2D(centers, i, 0), cvGetReal2D(centers, i, 1), 255, 'o' );
//double dist=1000;
//CFeaturePtr nearest = features.nearest( cvGetReal2D(centers, 0, 0), cvGetReal2D(centers, 0, 1), dist );
//image.cross( nearest->x, nearest->y, 0, 'x' );
cout<< features.size() << " features found\n" <<endl;
win.showImageAndPoints(grey, features);
//mrpt::system::pause();
//FEATURE STATISTICS
// double average_x = 0, average_y = 0, std_x = 0, std_y = 0;// average_x = xx.mean();
// average_y = yy.mean();
// std_x = xx.std();
// std_y = yy.std();
// cout<< average_x <<endl << average_y <<endl << std_x <<endl << std_y <<endl ;
// unsigned int x_min, x_max, y_min, y_max;
// x_min = average_x - std_x/5;
// x_max = average_x + std_x/5;
// y_min = average_y - std_y/5;
// y_max = average_y + std_y/5;
// for(unsigned int i = 0; i < features.size(); i++)
// {
// if( features[i]->x > x_min && features[i]->x < x_max && features[i]->y > y_min && features[i]->y < y_max )
// f_out.push_back(features[i]);
// }
//SEARCH WINDOW
// while( f_out.size()<10 )
// {
// x_min -= std_x/10;
// x_max += std_x/10;
// y_min -= std_y/10;
// y_max += std_y/10;
// for(unsigned int i = 0; i < features.size(); i++){
// if( features[i]->x > x_min && features[i]->x < x_max && features[i]->y > y_min && features[i]->y < y_max )
// f_out.push_back(features[i]);
// }
// }
//
// SELECT 3 POINTS
// double maxArea = 0;
// for(unsigned int i = 0; i < f_out.size(); i++){
// for(unsigned int j = i; j < f_out.size(); j++){
// for(unsigned int k = j; k < f_out.size(); k++){
// if( TriangleArea( f_out[i]->x, f_out[i]->y, f_out[j]->x, f_out[j]->y, f_out[k]->x, f_out[k]->y ) > maxArea ){
// maxArea = TriangleArea( f_out[i]->x, f_out[i]->y, f_out[j]->x, f_out[j]->y, f_out[k]->x, f_out[k]->y );
// v1 = i; v2 = j; v3 = k;
// }
// }
// }
// }
// v_init1[0] = f_out[v1]->x; v_init1[1] = f_out[v1]->y; v_init1[2] = 1;
// v_init2[0] = f_out[v2]->x; v_init2[1] = f_out[v2]->y; v_init2[2] = 1;
// v_init3[0] = f_out[v3]->x; v_init3[1] = f_out[v3]->y; v_init3[2] = 1;
//
////PRINT THE KLT FEATURES IN THE IMAGE
// CDisplayWindow wind("1st Frame");
// //CImage image;
// //image.loadFromIplImage(img);
//
// IplImage *img2 = cvCreateImage(cvSize(img->width, img->height), img->depth, 1);
// cvCopy(img,img2);
// // Draw Image and square.
// CvPoint pt1, pt2, pt3, pt4;
// CvScalar color = CV_RGB( 255, 255, 255 ); //color = CV_RGB( r, g, b );
// pt1 = cvPoint( v_init1[0], v_init1[1] );
// pt2 = cvPoint( v_init2[0], v_init2[1] );
// pt3 = cvPoint( v_init3[0], v_init3[1] );
// cvLine(img2, pt1, pt2, color, 1, 8, 0);
// cvLine(img2, pt1, pt3, color, 1, 8, 0);
// cvLine(img2, pt3, pt2, color, 1, 8, 0);
//
// pt1 = cvPoint( x_min, y_min );
// pt2 = cvPoint( x_max, y_min );
// pt3 = cvPoint( x_max, y_max );
// pt4 = cvPoint( x_min, y_max );
// cvLine(img2, pt1, pt2, color, 1, 8, 0);
// cvLine(img2, pt2, pt3, color, 1, 8, 0);
// cvLine(img2, pt3, pt4, color, 1, 8, 0);
// cvLine(img2, pt4, pt1, color, 1, 8, 0);
//
//
// ////TextOut the feature ID
// //for(unsigned int i = 0; i < features.size(); i++)
// //{
// // image.textOut( features.getByID(i)->x, features.getByID(i)->y, format("%d", i), 255 );
// //}
// //unsigned int chosen[5] = { 455, 309, 69, 85, 412 };
// //for(unsigned int i = 0; i < 5; i++)
// //{
// // sprintf( cstr, "%d", chosen[i] );
// // image.textOut( features.getByID(chosen[i])->x, features.getByID(chosen[i])->y, cstr, 255 );
// //}
//
// cout<< features.size() << " features found\n" <<endl;
// win.showImageAndPoints(img2, features);
// mrpt::system::pause();
//
// //unsigned int chosen[5] = { 14, 18, 34, 64, 70 };
// //unsigned int chosen[5] = { 455, 309, 69, 85, 412 };
// //for(unsigned int i = 0; i < 5; i++)
// // f_out.push_back(features.getByID(chosen[i]));
//
// //return f_out;
return features;
}
//--------------------------------
//| MATCH FEATURES |
//---------------------------------
void matchFeatures(CFeatureList &list1, CFeatureList &list2, CMatchedFeatureList &matched_out)
{
//CTicTac tictac;
TMatchingOptions opt;
opt.useXRestriction=false;
bool epipolar_match = false;
if(epipolar_match)
{
opt.useEpipolarRestriction=true;
opt.parallelOpticalAxis=false;
opt.epipolar_TH=2.0;
opt.maxEDSD_TH=0.18;
opt.EDSD_RATIO=0.6;
opt.F.loadFromTextFile("FM_.txt");
}
else
{
opt.useEpipolarRestriction=false;
opt.maxEDSD_TH=0.18;
opt.EDSD_RATIO=0.6;
}
cout<<"\nCalculating matches...\n";
opt.matching_method=TMatchingOptions::mmDescriptorSURF;
//tictac.Tic();
mrpt::vision::utils::matchFeatures2( list1, list2, matched_out, opt);
//cout << "Detected " << data->Harris_matched.size() << " Harris matches in " << endl;
//cout << format(" %.03fms",tictac.Tac()*1000) << endl;
matched_out.saveToTextFile("HarrisSURF_match.txt");
}
int main()
{
unsigned int i, j;
int count;
CImage im1, im2, patch;
IplImage *patch_tform, *patch_LM;
CFeatureList boot, flow_klt, seed_feat;
double dett, distance2, dist, distance = 0;
CvPoint pt1, pt2, pt3;
CvScalar color = CV_RGB( 255, 255, 255 ); //color = CV_RGB( r, g, b );
CMatrix mesh, correlations;
CMatrixDouble66 B, D;
CArrayDouble<6> A;
CMatrixDouble33 TForm;
vector_double optimal_x;
vector_double initial_x;
vector_double increments_x;
vector_double y;
CMyLevMarq::TResultInfo info;
CvCapture *video = cvCaptureFromFile("d:\\test4_proc.avi");
IplImage *image = 0, *grey = 0, *prev_grey = 0, *pyramid = 0, *prev_pyramid = 0, *swap_temp;
int win_size = 11;
const int MAX_COUNT = 500;
CvPoint2D32f* points[2] = {0,0}, *swap_points;
char* status = 0;
int flags = 0, flagsT = 0;
//int add_remove_pt = 0;
//CvPoint pt;
int k;
try
{
//LOAD BOOTING INFORMATION
//boot.loadFromTextFile("d:\\points.txt");
//flow_klt.loadFromTextFile("d:\\points.txt");
mesh.loadFromTextFile("d:\\mesh4.txt");
img_seq_data aux1;
IplImage *frame = cvQueryFrame(video);
ASSERT_(frame);
ASSERT_(frame->depth==IPL_DEPTH_8U);
aux1.img = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
//COLOR DISCRIMINATION
//IplImage* r = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
//IplImage* g = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
//IplImage* b = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
//IplImage* bk = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
//cvZero(bk);
//cvSplit(frame, b, g, r, 0);
//cvMul(b, bk, b, 1.2);
//cvMul(g, bk, b, 1.2);
//cvMul(r, bk, b, 0.6);
//cvMerge(b, g, r, 0, frame);
cvCvtColor( frame, aux1.img, CV_BGR2GRAY );
im1.loadFromIplImage( aux1.img );
//im1.saveToFile( "D:\\rgb2.bmp" );
//im1.loadFromFile("D:\\corner.jpg");
//patch_tform = (IplImage*) im1.getAsIplImage();
//boot = ExtractFeaturesFrame1(patch_tform);
// ALLOCATE ALL THE BUFFERS //
image = cvCreateImage( cvGetSize(frame), 8, 3 );
image->origin = frame->origin;
grey = cvCreateImage( cvGetSize(frame), 8, 1 );
prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
status = (char*)cvAlloc(MAX_COUNT);
flags = 0;
boot = ExtractFeaturesFrame1(aux1.img, seed_feat);//, average_x, average_y, std_x, std_y);
boot.saveToTextFile("D:\\boot_features.txt");
count = boot.size();
for( i = 0; i < count ; i++ ) //Copy boot to flow_klt
{
CFeaturePtr feat = CFeature::Create();
feat->x = boot[i]->x;
feat->y = boot[i]->y;
feat->ID = i;
feat->type = featKLT;
feat.make_unique();
flow_klt.push_back(feat);
points[0][i].x = feat->x;
points[0][i].y = feat->y;
} // end for
//boot = ExtractFeaturesFrame1(im1);
//boot.saveToTextFile("d:\\boot.txt");
//flow_klt.loadFromTextFile("d:\\boot.txt");
//flow_klt = ExtractFeaturesFrame1(aux1.img);
//flow_klt = ExtractFeaturesFrame1(im1);
//// CREATE AND ALLOCATE ALL THE BUFFERS FOR SINGLE PATCHES//
// //std::vector<IplImage> tri_ref( mesh.getRowCount() );
// //std::vector<IplImage> tri_vid( mesh.getRowCount() );
// //tri_ref = (IplImage*)cvAlloc(numTri*sizeof(tri_ref[0]));
// int numFeatTri = 5;
// IplImage* tri_ref[ 9 ];
// IplImage* tri_vid[ 9 ];
// //IplImage* prev_greyT[ 9 ];
// CvPoint2D32f* pointsT[2] = {0,0};
// char* statusT = 0;
// pointsT[0] = (CvPoint2D32f*)cvAlloc(numFeatTri*sizeof(points[0][0]));
// pointsT[1] = (CvPoint2D32f*)cvAlloc(numFeatTri*sizeof(points[0][0]));
// statusT = (char*)cvAlloc(numFeatTri);
// IplImage *pyramidT = 0, *prev_pyramidT = 0;
// pyramidT = cvCreateImage( cvGetSize(frame), 8, 1 );
// prev_pyramidT = cvCreateImage( cvGetSize(frame), 8, 1 );
////LOAD INITIAL FEATURES
// FILE* file;
// fopen_s(&file,"D:\\frame_proc4.fea","r");
// int countT;
// float x, y;
// fscanf_s(file, "%d", &count);
// for( i = 0; i < count; i++)
// {
// CFeaturePtr feat = CFeature::Create();
// fscanf_s(file, "%f\t%f", &x, &y);
// feat->x = x;
// feat->y = y;
// feat->ID = i;
// feat->type = featKLT;
// boot.push_back(feat);
// CFeaturePtr feat2 = feat;
// feat2.make_unique();
// flow_klt.push_back(feat2);
// points[0][i].x = x;
// points[0][i].y = y;
// }
// fclose(file);
////VERTEX OF REFERENCE TRIANGLE:
//po1.resize( mesh.getRowCount() );
//po2.resize( mesh.getRowCount() );
//po3.resize( mesh.getRowCount() );
//for(t=0; t<mesh.getRowCount(); t++)
//{
// po1[t][0] = boot[mesh(t,0)-1]->x; po1[t][1] = boot[mesh(t,0)-1]->y; po1[t][2] = 1;
// po2[t][0] = boot[mesh(t,1)-1]->x; po2[t][1] = boot[mesh(t,1)-1]->y; po2[t][2] = 1;
// po3[t][0] = boot[mesh(t,2)-1]->x; po3[t][1] = boot[mesh(t,2)-1]->y; po3[t][2] = 1;
//}
//aux1.tri_list.resize( mesh.getRowCount() );
//aux1.ind_list.resize( mesh.getRowCount() );
//loadPatches(boot, mesh, &(aux1.mask), aux1.tri_list, aux1.ind_list, aux1.img); //LOAD PATCHES
//data_img.push_back( aux1 );
//correlations.resize( 400, mesh.getRowCount() ); //->PASARLO A LA ESTRUCTURA DE DATOS !!!!!!!!!!!!!!!!
//correlations.fill(0);
//for( i=0; i<mesh.getRowCount(); i++ )
//{
// pt1 = cvPoint( boot[mesh(i,0)-1]->x, boot[mesh(i,0)-1]->y);
// pt2 = cvPoint( boot[mesh(i,1)-1]->x, boot[mesh(i,1)-1]->y);
// pt3 = cvPoint( boot[mesh(i,2)-1]->x, boot[mesh(i,2)-1]->y);
// cvLine(aux1.img, pt1, pt2, color, 1, 8, 0);
// cvLine(aux1.img, pt1, pt3, color, 1, 8, 0);
// cvLine(aux1.img, pt3, pt2, color, 1, 8, 0);
//}
////win.showImage(aux1.img);
////cvShowImage( "Reference", aux1.img );
//im1.loadFromIplImage(aux1.img);
////NCC STATISTICS (HISTOGRAM)
// char* cstr = new char [20];
// for( j=0; j<mesh.getRowCount(); j++ )
// {
// vector_double hist = aux1.tri_list[j].histogram( 0, 255, 256, false);
// //std::string fichNam = format( "D:\\histogram%u.txt",(j+1) );
// //vectorToTextFile(hist,fichNam);
// double media = 0; double length = hist.sumAll(); //=aux1.tri_list[0].size();
// for( i=0; i<256; i++)
// {
// media = media + i*hist[i]/length;
// }
// double desviacion, varianza = 0;
// for( i=0; i<256; i++)
// {
// varianza = varianza + square(i-media)*(hist[i]/length);
// }
// desviacion = sqrt(varianza);
// cout<< "Histogram T" <<j << "\taverage: " << media <<" \tdesviacion: " << desviacion <<endl <<endl;
// sprintf( cstr, "%d %.2f", (j+1), desviacion );
// im1.textOut( (boot[mesh(j,0)-1]->x + boot[mesh(j,1)-1]->x + boot[mesh(j,2)-1]->x)/3-20, (boot[mesh(j,0)-1]->y + boot[mesh(j,1)-1]->y + boot[mesh(j,2)-1]->y)/3, cstr, 255 );
// }
// delete cstr;
//win.showImage(im1);
//mrpt::system::pause();
////PATCH FEATURES
//CFeatureExtraction fExt;
//std::vector<CFeatureList> feat_patch_ref( mesh.getRowCount() );
//std::vector<CFeatureList> feat_patch_vid( mesh.getRowCount() );
//fExt.options.featsType = featKLT;
//fExt.options.harrisOptions.tile_image = false;
//fExt.options.patchSize=0;
//// DRAW patch_ref
//for( t=0; t<mesh.getRowCount(); t++ )
//{
// //vector_float xx(3); xx[0] = pt1.x; xx[1] = pt2.x; xx[2] = pt3.x;
// //vector_float yy(3); yy[0] = pt1.y; yy[1] = pt2.y; yy[2] = pt3.y;
// patch_tform = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
// for(i=0; i<aux1.tri_list[t].size(); i++)
// {
// patch_tform->imageData[aux1.ind_list[t][i]] = aux1.tri_list[t][i];
// }
// tri_ref[t] = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
// //prev_greyT[t] = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
// cvCopy( patch_tform, tri_ref[t], 0 );
// patch.loadFromIplImage(patch_tform);
// //patch.saveToFile( format("D:\\ref%d.bmp", (t+1) ) );
// //win3.showImageAndPoints( patch, boot );
// //fExt.detectFeatures( tri_ref[t], feat_patch_ref[t], 0, numFeatTri);
// IplImage* eig = cvCreateImage( cvGetSize(tri_ref[t]), 8, 1 );
// IplImage* temp = cvCreateImage( cvGetSize(tri_ref[t]), 8, 1 );
// CvPoint2D32f* puntos = {0};
// puntos = (CvPoint2D32f*)cvAlloc(numFeatTri*sizeof(puntos[0]));
// double quality = 0.01;
// double min_distance = 10;
// cvGoodFeaturesToTrack( tri_ref[t], eig, temp, puntos, &numFeatTri,
// quality, min_distance, 0, 3, 0, 0.04 );
// cvFindCornerSubPix( tri_ref[t], puntos, numFeatTri,
// cvSize(win_size,win_size), cvSize(-1,-1),
// cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
// for(i=0; i<numFeatTri; i++)
// {
// CFeaturePtr feat = CFeature::Create();
// feat->x = puntos[i].x;
// feat->y = puntos[i].y;
// feat->ID = 0;
// feat->type = featKLT;
// feat_patch_ref[t].push_back(feat);
// CFeaturePtr feat2 = feat;
// feat2.make_unique();
// feat_patch_vid[t].push_back(feat2);
// patch.cross( feat_patch_ref[t][i]->x, feat_patch_ref[t][i]->y, 255, '+' );
// }
// cvReleaseImage( &eig );
// cvReleaseImage( &temp );
////SURF
//fExt.options.featsType = featHarris;
//fExt.options.harrisOptions.tile_image = false;
//fExt.options.patchSize=0;
//fExt.detectFeatures( tri_ref[t], feat_patch_ref[t], 0, numFeatTri);
//fExt.computeDescriptors( tri_ref[t], feat_patch_ref[t], descSURF );
//win2.showImageAndPoints( tri_ref[t], feat_patch_ref[t] );
//mrpt::system::pause();
//..........TO FINISH
//patch.saveToFile( format("D:\\ref%d+p.bmp", (t+1) ) );
//}
cvCopy( aux1.img, grey, 0 );
//cvFindCornerSubPix( grey, points[0], count,
// cvSize(win_size,win_size), cvSize(-1,-1),
// cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
//CHECK FOR A FRAME WITH ENOUGH DISPARITY AND TRACK FEATURES
img_seq_data aux;
CV_SWAP( prev_grey, grey, swap_temp );
frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); //Take one frame each 5
frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); //Take one frame each 5
frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); //Take one frame each 5
//frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); //Take one frame each 5
//frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); //Take one frame each 5
ASSERT_(frame);
ASSERT_(frame->depth==IPL_DEPTH_8U);
//if( !frame ) break;
//if (f==200) break;
aux.img = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
cvCvtColor( frame, aux.img, CV_BGR2GRAY );
IplImage *frame_aux = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
cvCopy( aux.img, frame_aux );
cvCopy( aux.img, grey, 0 );
if( count > 0 )
{
cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );
flags |= CV_LKFLOW_PYR_A_READY;
//for( i = 0; i < count; i++)
//{
// flow_klt[i]->x = points[1][i].x;
// flow_klt[i]->y = points[1][i].y;
// }
////CALCULATE AVERAGE DISTANCE OF FLOW
//for( i=0; i<count; i++ )
//{
// distance += sqrt( square( boot[i]->x - flow_klt[i]->x ) + square( boot[i]->y - flow_klt[i]->y ) );
//}
//distance /= count;
for( i=0; i<count; i++ )
{
distance += sqrt( square( points[0][i].x - points[1][i].x ) + square( points[0][i].y - points[1][i].y ) );
}
distance /= count; //Average distance of all features followed
cout<< "Average distance: " <<distance <<endl;
}
else
cout<< "Error tracking features...\n";
for( i = 0; i < count; i++)
{
flow_klt[i]->x = points[1][i].x;
flow_klt[i]->y = points[1][i].y;
}
flow_klt.saveToTextFile("D:\\flow_klt.txt");
win1.showImageAndPoints(aux.img, flow_klt);
//mrpt::system::pause();
img_aux.loadFromIplImage(aux.img);
//RANSAC
ofstream ransac_log;
ransac_log.open ("d:\\ransac_log.txt");
vector<CMatrixTemplateNumeric<double> > detected_homographies;
vector< vector<size_t> > plane_ind;
const double DIST_THRESHOLD = 0.2;
const size_t min_points = 15;
CTicTac tictac;
tictac.Tic();
ransac_homographies( boot, flow_klt, detected_homographies, DIST_THRESHOLD, min_points, plane_ind );
cout<<"RANSAC Performance in " <<tictac.Tac() <<"s." <<endl;
for (j=0;j<plane_ind.size();j++)
{
ransac_log << endl << detected_homographies[j] << endl ; // (0,0) <<" " << detected_homographies[0](0,1) <<" " << detected_homographies[0](0,2) <<endl;
for (i=0;i<plane_ind[j].size();i++)
{
v_init1[0] = boot[plane_ind[j][i] ]->x;
v_init1[1] = boot[plane_ind[j][i] ]->y;
v_init1[2] = 1;
detected_homographies[j].multiply_Ab( v_init1, v_init2 );
//dist = sqrt( square( v_init2[0] - v_init1[0] ) + square(v_init2[1] - v_init1[1] ) );
v_init3[0] = flow_klt[plane_ind[j][i] ]->x;
v_init3[1] = flow_klt[plane_ind[j][i] ]->y;
v_init3[2] = sqrt( square( v_init2[0] - v_init3[0] ) + square(v_init2[1] - v_init3[1] ) );
ransac_log << v_init1 << v_init2 << v_init3 <<endl ;
}
}
ransac_log.close();
im2.loadFromIplImage(aux.img);
TColor color;
for(i=0; i<detected_homographies.size(); i++){
//CImage imgColor(1,1,3);
im2.colorImage( imgColor ); // Create a colorimage
//k = i%4;
//switch (k){
// case 0:
// color = TColor::blue;
// break;
// case 1:
// color = TColor::red;
// break;
// case 2:
// color = TColor::green;
// break;
// case 3:
// color = TColor::black;
// break;
// default:
// color = TColor::white;
//}
//color = TColor(25*(10-i),25*(20-i),65*(i-4));
color = TColor::blue;
for(j=0;j<plane_ind[i].size(); j++)
imgColor.cross(round(flow_klt[ plane_ind[i][j] ]->x),round(flow_klt[ plane_ind[i][j] ]->y),color,'+');
win2.showImage(imgColor);
mrpt::system::pause();
}
//EXPAND THE TRIANGLE
//float dir, dist;
//CFeaturePtr next;
//center.x =100; center.y =100;
//while ( distance<5 )
//{
// next = flow_klt.nearest(center.x, center.y, dist);
//
//}
////SWAP THE TRIANGLE
//while ( distance>5 )
//{
//}
//CHECK LOCAL PLANARITY (It takes three vertex in a square and verifies if the rest of points in the square fulfill the homography)
CArrayDouble<3> vs1, vs2, vs3;
vs1[0] = boot[seed_feat[v1]->ID]->x; vs1[1] = boot[seed_feat[v1]->ID]->y; vs1[2] = 1;
vs2[0] = boot[seed_feat[v2]->ID]->x; vs2[1] = boot[seed_feat[v2]->ID]->y; vs2[2] = 1;
vs3[0] = boot[seed_feat[v3]->ID]->x; vs3[1] = boot[seed_feat[v3]->ID]->y; vs3[2] = 1;
bool seed_plane = false;
while ( !seed_plane )
{
//Build affine transformation
D.fill(0);
dett = (-vs1[0]*vs3[1]+vs1[0]*vs2[1]+vs3[1]*vs2[0]-vs2[1]*vs3[0]-vs2[0]*vs1[1]+vs3[0]*vs1[1]); //This is the square root of minus det(B) -> sqtr(-det(B))
D.set_unsafe(0,0, (-vs3[1]+vs2[1])/dett);
D.set_unsafe(0,2, -(vs1[1]-vs3[1])/dett);
D.set_unsafe(0,4, (-vs2[1]+vs1[1])/dett);
D.set_unsafe(1,0, -(vs2[0]-vs3[0])/dett);
D.set_unsafe(1,2, (vs1[0]-vs3[0])/dett);
D.set_unsafe(1,4, -(vs1[0]-vs2[0])/dett);
D.set_unsafe(2,0, (-vs2[1]*vs3[0]+vs3[1]*vs2[0])/dett);
D.set_unsafe(2,2, -(vs1[0]*vs3[1]-vs3[0]*vs1[1])/dett);
D.set_unsafe(2,4, (vs1[0]*vs2[1]-vs2[0]*vs1[1])/dett);
D.set_unsafe(3,1, (-vs3[1]+vs2[1])/dett);
D.set_unsafe(3,3, -(vs1[1]-vs3[1])/dett);
D.set_unsafe(3,5, (-vs2[1]+vs1[1])/dett);
D.set_unsafe(4,1, -(vs2[0]-vs3[0])/dett);
D.set_unsafe(4,3, (vs1[0]-vs3[0])/dett);
D.set_unsafe(4,5, -(vs1[0]-vs2[0])/dett);
D.set_unsafe(5,1, (-vs2[1]*vs3[0]+vs3[1]*vs2[0])/dett);
D.set_unsafe(5,3, -(vs1[0]*vs3[1]-vs3[0]*vs1[1])/dett);
D.set_unsafe(5,5, (vs1[0]*vs2[1]-vs2[0]*vs1[1])/dett);
D.saveToTextFile("d:\\D.txt");
C[0] = flow_klt[seed_feat[v1]->ID]->x;
C[1] = flow_klt[seed_feat[v1]->ID]->y;
C[2] = flow_klt[seed_feat[v2]->ID]->x;
C[3] = flow_klt[seed_feat[v2]->ID]->y;
C[4] = flow_klt[seed_feat[v3]->ID]->x;
C[5] = flow_klt[seed_feat[v3]->ID]->y;
D.multiply_Ab(C,A);
TForm.set_unsafe(0,0, A[0]);
TForm.set_unsafe(0,1, A[1]);
TForm.set_unsafe(0,2, A[2]);
TForm.set_unsafe(1,0, A[3]);
TForm.set_unsafe(1,1, A[4]);
TForm.set_unsafe(1,2, A[5]);
TForm.set_unsafe(2,0, 0);
TForm.set_unsafe(2,1, 0);
TForm.set_unsafe(2,2, 1);
CArrayDouble<3> pto, pto_r;
pto[2] = 1;
distance = 0;
for(i=0; i<seed_feat.size(); i++)
{
if( !(i==v1 || i==v2 || i==v3 ) )
{
pto[0] = boot[ seed_feat[i]->ID ]->x; pto[1] = boot[ seed_feat[i]->ID ]->y;
TForm.multiply_Ab( pto, pto_r );
distance += sqrt( square( pto_r[0] - flow_klt[seed_feat[i]->ID]->x ) + square(pto_r[1] - flow_klt[seed_feat[i]->ID]->y ) );
}
}
distance /= ( seed_feat.size() - 3 ); //Distancia media de los píxeles del entorno
if ( distance < 2 )
seed_plane = true;
// followPatch(TForm, aux.tri_list[t], aux.img); // followPatch(TForm, aux.tri_list[t], aux.ind_list[t], aux.img);
correlations(2*f-1,t) = ncc_vector(data_img[0].tri_list[t], aux.tri_list[t]);
cout<< "Affine-correlation\tFrame: " <<f <<"\tTriangle: " <<t+1 <<endl <<correlations(2*f-1,t) <<endl;
// DRAW PATCH_TFORM
patch_tform = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
for( i=0; i<data_img[0].tri_list[t].size(); i++ )
{
patch_tform->imageData[data_img[0].ind_list[t][i]] = aux.tri_list[t][i];
}
}
f=1;
/* while(true)
{
////TEST NCC-VECTOR
//double test = ncc_vector( data_img[0].tri_list[0], data_img[0].tri_list[0] );
//cout<< "NCC Test: " <<test <<endl;
CV_SWAP( prev_grey, grey, swap_temp );
img_seq_data aux;
//data_img[f].tri.resize(mesh.getRowCount);
//data_img[f].img = (IplImage*) im2->getAsIplImage();
frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); frame = cvQueryFrame(video); //Take one frame each 5
ASSERT_(frame);
ASSERT_(frame->depth==IPL_DEPTH_8U);
if( !frame ) break;
if (f==200) break;
aux.img = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
cvCvtColor( frame, aux.img, CV_BGR2GRAY );
IplImage *frame_aux = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 1);
//cvCvtColor( frame, frame_aux, CV_BGR2GRAY );
cvCopy( aux.img, frame_aux );
//MRPT TRACKFEATURES-WRAPER
//im2.loadFromIplImage(frame_aux);
//trackFeatures( im1, im2, flow_klt2, 11, 11 );
cvCopy( aux.img, grey, 0 );
//cvCvtColor( image, grey, CV_BGR2GRAY );
if( count > 0 )
{
cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );
flags |= CV_LKFLOW_PYR_A_READY;
// ARRAY CONVERSION OPENCV->MRPT
//CFeatureList::iterator itFeat;
//for( i = 0; i < count ; i++ )
//{
// CFeaturePtr feat = CFeature::Create();
// feat->x = points[1][i].x;
// feat->y = points[1][i].y;
// feat->ID = i;
// feat->type = featKLT;
// flow_klt.push_back(feat);
//} // end for
for( i = 0; i < count; i++)
{
flow_klt[i]->x = points[1][i].x;
flow_klt[i]->y = points[1][i].y;
}
win1.showImageAndPoints(aux.img, flow_klt);
mrpt::system::pause();
//CALCULATE AVERAGE DISTANCE OF FLOW
distance = 0;
for( i=0; i<count; i++ )
{
distance += sqrt( square( boot[i]->x - flow_klt[i]->x ) + square( boot[i]->y - flow_klt[i]->y ) );
}
distance /= count;
for( i = k = 0; i < count; i++ )
{
if( !status[i] )
continue;
//points[1][k++] = points[1][i];
//cvCircle( image, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);
}
//count = k;
}
//if( add_remove_pt && count < MAX_COUNT )
//{
// points[1][count++] = cvPointTo32f(pt);
// cvFindCornerSubPix( grey, points[1] + count - 1, 1,
// cvSize(win_size,win_size), cvSize(-1,-1),
// cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
// add_remove_pt = 0;
//}
// CV_SWAP( prev_grey, grey, swap_temp );
CV_SWAP( prev_pyramid, pyramid, swap_temp );
CV_SWAP( points[0], points[1], swap_points );
//// Draw Image and mesh. //frame_aux
//for( i=0; i<mesh.getRowCount(); i++ )
//{
// pt1 = cvPoint( flow_klt[mesh(i,0)-1]->x, flow_klt[mesh(i,0)-1]->y );
// pt2 = cvPoint( flow_klt[mesh(i,1)-1]->x, flow_klt[mesh(i,1)-1]->y );
// pt3 = cvPoint( flow_klt[mesh(i,2)-1]->x, flow_klt[mesh(i,2)-1]->y );
// cvLine(frame_aux, pt1, pt2, color, 1, 8, 0);
// cvLine(frame_aux, pt1, pt3, color, 1, 8, 0);
// cvLine(frame_aux, pt3, pt2, color, 1, 8, 0);
//}
////cvShowImage( "Video", frame_aux );
////im2.loadFromIplImage(frame_aux);
//win1.showImage(frame_aux);
////mrpt::system::pause();
// Check if all features have been followed
CFeatureList::iterator it;
for(it=flow_klt.begin(); it!=flow_klt.end(); it++)
{
if( (*it)->x < 0 )
{
cout<< "Feature " <<(*it)->ID << " couldn't be followed." <<endl;
mrpt::system::pause();
}
}
aux.tri_list.resize( mesh.getRowCount() );
aux.ind_list.resize( mesh.getRowCount() );
//loadPatches(flow_klt, mesh, &(aux.mask), aux.tri_list, aux.ind_list, aux.img);
patch_tform = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
patch_LM = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
for(t=0; t<mesh.getRowCount(); t++)
{
//if(f==1) //HISTOGRAM CALCULATION
//{
// vector_double hist = aux1.tri_list[t].histogram( 0, 255, 256, false);
// double media = 0; double length = hist.sumAll(); //=aux1.tri_list[0].size();
// for( i=0; i<256; i++)
// {
// media = media + i*hist[i]/length;
// }
// double desviacion, varianza = 0;
// for( i=0; i<256; i++)
// {
// varianza = varianza + square(i-media)*(hist[i]/length);
// }
// desviacion = sqrt(varianza);
// cout<< "\nHistogram T" <<t << "\taverage: " << media <<" \tdesviacion: " << desviacion <<endl;
//}//END HISTOGRAM
////TRANSFORM DATA TYPE TO PERFORM CORRELATION MEASUREMENT
//for(t=0; t<mesh.getRowCount; t++)
//{
// image = cvCreateMatHeader(1, data_img[f].tri_list[t].size(), CV_8UC1 );
// cvCreateData(image);
// for(k=0;k<data_img[f].tri_list[t].size();k++)
// {
// cvSet2D(image, 0, k, data_img[i].tri_list[t][k]);
// }
//}
//cvMatchTemplate(image, const CvArr* templ, result, CV_TM_CCORR_NORMED);
//cvRelease(&image);
// AFFINE MATRIX CALCULATION
//B.fill(0);
//for(h=0; h<3; h++)
//{
// B(2*h,0) = boot[mesh(t,h)-1]->x;
// B(2*h,1) = boot[mesh(t,h)-1]->y;
// B(2*h,2) = 1;
// B(2*h+1,3) = boot[mesh(t,h)-1]->x;
// B(2*h+1,4) = boot[mesh(t,h)-1]->y;
// B(2*h+1,5) = 1;
// C[2*h] = flow_klt[mesh(t,h)-1]->x;
// C[2*h+1] = flow_klt[mesh(t,h)-1]->y;
//}
//B.saveToTextFile("d:\\B.txt");
//cout << B <<endl << C << endl;
//B.inv(D); //D.loadFromTextFile("d:\\invB.txt");
D.fill(0);
dett = (-po1[t][0]*po3[t][1]+po1[t][0]*po2[t][1]+po3[t][1]*po2[t][0]-po2[t][1]*po3[t][0]-po2[t][0]*po1[t][1]+po3[t][0]*po1[t][1]); //This is the square root of minus det(B) -> sqtr(-det(B))
D.set_unsafe(0,0, (-po3[t][1]+po2[t][1])/dett);
D.set_unsafe(0,2, -(po1[t][1]-po3[t][1])/dett);
D.set_unsafe(0,4, (-po2[t][1]+po1[t][1])/dett);
D.set_unsafe(1,0, -(po2[t][0]-po3[t][0])/dett);
D.set_unsafe(1,2, (po1[t][0]-po3[t][0])/dett);
D.set_unsafe(1,4, -(po1[t][0]-po2[t][0])/dett);
D.set_unsafe(2,0, (-po2[t][1]*po3[t][0]+po3[t][1]*po2[t][0])/dett);
D.set_unsafe(2,2, -(po1[t][0]*po3[t][1]-po3[t][0]*po1[t][1])/dett);
D.set_unsafe(2,4, (po1[t][0]*po2[t][1]-po2[t][0]*po1[t][1])/dett);
D.set_unsafe(3,1, (-po3[t][1]+po2[t][1])/dett);
D.set_unsafe(3,3, -(po1[t][1]-po3[t][1])/dett);
D.set_unsafe(3,5, (-po2[t][1]+po1[t][1])/dett);
D.set_unsafe(4,1, -(po2[t][0]-po3[t][0])/dett);
D.set_unsafe(4,3, (po1[t][0]-po3[t][0])/dett);
D.set_unsafe(4,5, -(po1[t][0]-po2[t][0])/dett);
D.set_unsafe(5,1, (-po2[t][1]*po3[t][0]+po3[t][1]*po2[t][0])/dett);
D.set_unsafe(5,3, -(po1[t][0]*po3[t][1]-po3[t][0]*po1[t][1])/dett);
D.set_unsafe(5,5, (po1[t][0]*po2[t][1]-po2[t][0]*po1[t][1])/dett);
D.saveToTextFile("d:\\D.txt");
for(i=0; i<3; i++)
{
C[2*i] = flow_klt[mesh(t,i)-1]->x;
C[2*i+1] = flow_klt[mesh(t,i)-1]->y;
}
D.multiply_Ab(C,A);
TForm.set_unsafe(0,0, A[0]);
TForm.set_unsafe(0,1, A[1]);
TForm.set_unsafe(0,2, A[2]);
TForm.set_unsafe(1,0, A[3]);
TForm.set_unsafe(1,1, A[4]);
TForm.set_unsafe(1,2, A[5]);
TForm.set_unsafe(2,0, 0);
TForm.set_unsafe(2,1, 0);
TForm.set_unsafe(2,2, 1);
followPatch(TForm, aux.tri_list[t], aux.img); // followPatch(TForm, aux.tri_list[t], aux.ind_list[t], aux.img);
correlations(2*f-1,t) = ncc_vector(data_img[0].tri_list[t], aux.tri_list[t]);
cout<< "Affine-correlation\tFrame: " <<f <<"\tTriangle: " <<t+1 <<endl <<correlations(2*f-1,t) <<endl;
// DRAW PATCH_TFORM
patch_tform = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
for( i=0; i<data_img[0].tri_list[t].size(); i++ )
{
patch_tform->imageData[data_img[0].ind_list[t][i]] = aux.tri_list[t][i];
}
//cvShowImage( "Patch-TForm", patch_tform );
//patch.loadFromIplImage(patch_tform);
tri_vid[t] = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
cvCopy( patch_tform, tri_vid[t], 0 );
patch.loadFromIplImage(patch_tform);
//win2.showImage(patch_tform);
//patch.saveToFile( format("D:\\patch%d.bmp", t ) );
win2.showImageAndPoints( patch_tform, boot );
mrpt::system::pause();
//DRAW TRANSFORMED PATCHES
for( i=0; i<data_img[0].tri_list[t].size(); i++ )
patch_tform->imageData[data_img[0].ind_list[t][i]] = aux.tri_list[t][i];
pt1 = cvPoint( boot[mesh(t,0)-1]->x, boot[mesh(t,0)-1]->y);
pt2 = cvPoint( boot[mesh(t,1)-1]->x, boot[mesh(t,1)-1]->y);
pt3 = cvPoint( boot[mesh(t,2)-1]->x, boot[mesh(t,2)-1]->y);
cvLine(patch_tform, pt1, pt2, color, 1, 8, 0);
cvLine(patch_tform, pt1, pt3, color, 1, 8, 0);
cvLine(patch_tform, pt3, pt2, color, 1, 8, 0);
if( t==(mesh.getRowCount()-1) )
{
im2.loadFromIplImage(patch_tform);
for( i=0; i<mesh.getRowCount(); i++ )
im2.textOut( (boot[mesh(i,0)-1]->x + boot[mesh(i,1)-1]->x + boot[mesh(i,2)-1]->x)/3-20, (boot[mesh(i,0)-1]->y + boot[mesh(i,1)-1]->y + boot[mesh(i,2)-1]->y)/3-10, format("%.2f", correlations(2*f-1,i) ), 255 );
win2.showImageAndPoints( im2, boot ); //im2 and boot has different scales, since the origins are 0 & 1 respectively, therefore the mesh is shifted 1pixel in each axis
mrpt::system::pause();
} //END DRAWING
//FOLLOW PATCH-FEATURES
countT = feat_patch_ref[t].size();
for(i=0; i<countT; i++)
{
pointsT[0][i].x = feat_patch_vid[t][i]->x;
pointsT[0][i].y = feat_patch_vid[t][i]->y;
}
cvCalcOpticalFlowPyrLK( tri_ref[t], tri_vid[t], prev_pyramidT, pyramidT,
pointsT[0], pointsT[1], countT, cvSize(win_size,win_size), 3, statusT, 0,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flagsT );
//flagsT |= CV_LKFLOW_PYR_A_READY;
distance2 = 0;
for(i=0; i<countT; i++)
{
//CFeaturePtr feat = CFeature::Create();
//feat->x = pointsT[1][i].x;
//feat->y = pointsT[1][i].y;
//feat->ID = i;
//feat->type = featKLT;
//feat_patch_vid[t].push_back(feat);
feat_patch_vid[t][i]->x = pointsT[1][i].x;
feat_patch_vid[t][i]->y = pointsT[1][i].y;
dist = feat_patch_ref[t][i]->x - feat_patch_vid[t][i]->x ;
distance2 += square( feat_patch_ref[t][i]->x - feat_patch_vid[t][i]->x ) + square( feat_patch_ref[t][i]->y - feat_patch_vid[t][i]->y );
patch.line( feat_patch_ref[t][i]->x, feat_patch_ref[t][i]->y, feat_patch_vid[t][i]->x, feat_patch_vid[t][i]->y, 255 );
patch.cross( feat_patch_vid[t][i]->x, feat_patch_vid[t][i]->y, 255, '+' );
}
correlations(2*f,t) = distance2 / countT;
cout<< "distance " <<correlations(2*f,t) <<endl;
win3.showImageAndPoints( tri_vid[t], feat_patch_vid[t] );
patch.textOut( (boot[mesh(t,0)-1]->x + boot[mesh(t,1)-1]->x + boot[mesh(t,2)-1]->x)/3+60, (boot[mesh(t,0)-1]->y + boot[mesh(t,1)-1]->y + boot[mesh(t,2)-1]->y)/3, format( "Distance2: %f", correlations(2*f,t) ), 255 );
patch.saveToFile( format("D:\\vid%d+p.bmp", (t+1) ) );
mrpt::system::pause();
CV_SWAP( tri_ref[t], tri_vid[t], swap_temp ); //Reference changes to the previous patch.
//CV_SWAP( pointsT[0], pointsT[1], swap_points );
//CV_SWAP( prev_pyramidT, pyramidT, swap_temp );
//LEVENBERG-MARQUARDT
//if( correlations(2*f-1,t)<0.85 && correlations(2*f-1,t)>0.6 ) //Then perform Levenberg-Marquardt algorithm
//{
// //% [ A D ]
// //% [u v] = [x y 1] * [ B E ] Construct Affine Transformation
// //% [ C F ]
// initial_x.resize(8);
// initial_x[0] = A[0];
// initial_x[1] = A[1];
// initial_x[2] = A[2];
// initial_x[3] = A[3];
// initial_x[4] = A[4];
// initial_x[5] = A[5];
// initial_x[6] = 0;
// initial_x[7] = 0;
// // Increments Vector = [ 0.0001 0.0001 .5 0.0001 0.0001 .5 0.000001 0.000001 ]
// increments_x.resize(8, 0.0001 );
// increments_x[2] = 0.5; increments_x[5] = 0.5; increments_x[6] = 0.000001; increments_x[7] = 0.000001;
// optimal_x.resize(8);
// //CMyLevMarq::execute(optimal_x, initial_x, CostFunction, increments_x, data_img, info );
// CTicTac tictac;
// tictac.Tic();
// CMyLevMarq::execute(optimal_x, initial_x, CostFunction, increments_x, aux, info, false, 200, 1e-3, 1e-10, 1e-10, false );
// correlations(2*f,t) = 1 - sqrt(info.final_sqr_err);
// //cout<< "LM-correlation\tFrame: " <<f <<"\tTriangle: " <<t+1 <<endl <<correlations(2*f,t) <<endl;
// cout<< "LM-correlation" <<endl <<correlations(2*f,t) <<endl;
// cout<< tictac.Tac() <<" seconds\t" << info.iterations_executed <<" iterations\n" <<endl;
// TForm.set_unsafe(0,0, optimal_x[0]);
// TForm.set_unsafe(0,1, optimal_x[1]);
// TForm.set_unsafe(0,2, optimal_x[2]);
// TForm.set_unsafe(1,0, optimal_x[3]);
// TForm.set_unsafe(1,1, optimal_x[4]);
// TForm.set_unsafe(1,2, optimal_x[5]);
// TForm.set_unsafe(2,0, optimal_x[6]);
// TForm.set_unsafe(2,1, optimal_x[7]);
// TForm.set_unsafe(2,2, 1);
// // //DRAW IMAGE AND POINTS
// //vector_double pp1(3), pp2(3), pp3(3);
// //vector_float xx(3), yy(3);
// //TForm.multiply_Ab(po1[t],pp1);
// //pp1[0] = pp1[0]/pp1[2]; pp1[1] = pp1[1]/pp1[2];
// //TForm.multiply_Ab(po2[t],pp2);
// //pp2[0] = pp2[0]/pp2[2]; pp2[1] = pp2[1]/pp2[2];
// //TForm.multiply_Ab(po3[t],pp3);
// //pp3[0] = pp3[0]/pp3[2]; pp3[1] = pp3[1]/pp3[2];
// //xx[0] = pp1[0]; xx[1] = pp2[0]; xx[2] = pp3[0];
// //yy[0] = pp1[1]; yy[1] = pp2[1]; yy[2] = pp3[1];
// ////data_img[f].img = (IplImage*) im2->getAsIplImage();
// //win3.showImageAndPoints( aux.img, xx, yy, TColor::red );
// //mrpt::system::pause();
// vector_byte triangle;
// triangle.resize(data_img[0].tri_list[t].size());
// followPatch(TForm, triangle, aux.img);
// //cout<< ncc( data_img[0].tri_list[t], triangle ) <<endl;
// // DRAW PATCH_LM
// patch_tform = cvCreateImage(cvSize(data_img[0].img->width, data_img[0].img->height), data_img[0].img->depth, 1);
// for(i=0; i<data_img[0].tri_list[t].size(); i++)
// {
// patch_tform->imageData[data_img[0].ind_list[t][i]] = triangle[i];
// }
// patch.loadFromIplImage(patch_tform);
// win3.showImage(patch);
// mrpt::system::pause();
// ////DRAW LEVENBERG-MARQUARDT TRANSFORMED PATCHES
// //for( i=0; i<data_img[0].tri_list[t].size(); i++ )
// // patch_LM->imageData[data_img[0].ind_list[t][i]] = triangle[i];
// //
// //pt1 = cvPoint( boot[mesh(t,0)-1]->x, boot[mesh(t,0)-1]->y);
// //pt2 = cvPoint( boot[mesh(t,1)-1]->x, boot[mesh(t,1)-1]->y);
// //pt3 = cvPoint( boot[mesh(t,2)-1]->x, boot[mesh(t,2)-1]->y);
// //cvLine(patch_LM, pt1, pt2, color, 1, 8, 0);
// //cvLine(patch_LM, pt1, pt3, color, 1, 8, 0);
// //cvLine(patch_LM, pt3, pt2, color, 1, 8, 0);
// //
// //if( t==(mesh.getRowCount()-1) )
// //{
// // im2.loadFromIplImage(patch_LM);
// // for( i=0; i<mesh.getRowCount(); i++ )
// // {
// // char* cstr = new char [20];
// // sprintf( cstr, "%.2f", correlations(2*f-1,i) );
// // im2.textOut( (boot[mesh(i,0)-1]->x + boot[mesh(i,1)-1]->x + boot[mesh(i,2)-1]->x)/3-20, (boot[mesh(i,0)-1]->y + boot[mesh(i,1)-1]->y + boot[mesh(i,2)-1]->y)/3-10, cstr, 255 );
// // delete cstr;
// // }
// // win2.showImageAndPoints( im2, boot ); //im2 and boot has different scales, since the origins are 0 & 1 respectively, therefore the mesh is shifted 1pixel in each axis
// // mrpt::system::pause();
// //} //END DRAWING
//} //End If -> Levenberg-Marquardt
}
data_img.push_back( aux );
f++;
}*/
cvReleaseCapture( &video );
correlations.saveToTextFile( "D:\\correlations.txt" );
}
catch(std::exception &e)
{
std::cout << e.what() << std::endl;
}
return 0;
}
