/* +---------------------------------------------------------------------------+ | The Mobile Robot Programming Toolkit (MRPT) C++ library | | | | http://www.mrpt.org/ | | | | Copyright (C) 2005-2011 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; }