From 614ec085f4ae39d13bb5d85abcac19d3a1e76ce1 Mon Sep 17 00:00:00 2001 From: Guido Ranzuglia granzuglia Date: Mon, 4 Jun 2012 16:13:35 +0000 Subject: [PATCH] moved to plugins_unsupported the filter_mutualinfo not-XML version. --- .../filter_mutualinfo/alignset.cpp | 357 +++++++++ .../filter_mutualinfo/alignset.h | 78 ++ .../filter_mutualinfo/filter_mutualinfo.cpp | 315 ++++++++ .../filter_mutualinfo/filter_mutualinfo.h | 73 ++ .../filter_mutualinfo/filter_mutualinfo.pro | 21 + .../filter_mutualinfo/levmarmethods.cpp | 239 ++++++ .../filter_mutualinfo/levmarmethods.h | 49 ++ .../filter_mutualinfo/mutual.cpp | 130 ++++ .../filter_mutualinfo/mutual.h | 26 + .../filter_mutualinfo/parameters.cpp | 300 ++++++++ .../filter_mutualinfo/parameters.h | 65 ++ .../filter_mutualinfo/pointCorrespondence.cpp | 22 + .../filter_mutualinfo/pointCorrespondence.h | 26 + .../filter_mutualinfo/pointOnLayer.h | 62 ++ .../filter_mutualinfo/shutils.h | 104 +++ .../filter_mutualinfo/solver.cpp | 717 ++++++++++++++++++ .../filter_mutualinfo/solver.h | 77 ++ 17 files changed, 2661 insertions(+) create mode 100644 src/plugins_unsupported/filter_mutualinfo/alignset.cpp create mode 100644 src/plugins_unsupported/filter_mutualinfo/alignset.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.cpp create mode 100644 src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.pro create mode 100644 src/plugins_unsupported/filter_mutualinfo/levmarmethods.cpp create mode 100644 src/plugins_unsupported/filter_mutualinfo/levmarmethods.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/mutual.cpp create mode 100644 src/plugins_unsupported/filter_mutualinfo/mutual.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/parameters.cpp create mode 100644 src/plugins_unsupported/filter_mutualinfo/parameters.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.cpp create mode 100644 src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/pointOnLayer.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/shutils.h create mode 100644 src/plugins_unsupported/filter_mutualinfo/solver.cpp create mode 100644 src/plugins_unsupported/filter_mutualinfo/solver.h diff --git a/src/plugins_unsupported/filter_mutualinfo/alignset.cpp b/src/plugins_unsupported/filter_mutualinfo/alignset.cpp new file mode 100644 index 000000000..82e06561b --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/alignset.cpp @@ -0,0 +1,357 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + + +#include "alignset.h" + +#include +#include +//#include +#include +#include +#include + +#include "shutils.h" + +using namespace std; + +AlignSet::AlignSet(): mode(COMBINE), +target(NULL), render(NULL), +vbo(0), nbo(0), cbo(0), ibo(0), error(0){ + + box.SetNull(); + correspList = new QList(); + imageRatio=1; +} + +AlignSet::~AlignSet() { + if(target) delete []target; + if(render) delete []render; + delete correspList; +} + +void AlignSet::initializeGL() { + + programs[COLOR] = createShaders("varying vec4 color; void main() { gl_Position = ftransform(); color = gl_Color; }", + "varying vec4 color; void main() { gl_FragColor = color; }"); + programs[NORMALMAP] = createShaders("varying vec3 normal; void main() { normal = gl_NormalMatrix * gl_Normal; gl_Position = ftransform(); }", + "varying vec3 normal; void main() { " + "vec3 color = normalize(normal); color = color * 0.5 + 0.5; gl_FragColor = vec4(color, 1.0); }"); + programs[COMBINE] = createShaders("varying vec3 normal; varying vec4 color; void main() { " + "normal = gl_NormalMatrix * gl_Normal; gl_Position = ftransform(); color = gl_Color; }", + "varying vec3 normal; varying vec4 color; void main() { " + "vec3 ncolor = normalize(normal); ncolor = ncolor * 0.5 + 0.5; " + "float t = color.x*color.x; gl_FragColor = (1.0-t)*color + t*(vec4(ncolor, 1.0)); }"); + programs[SPECULAR] = createShaders("varying vec3 reflection; void main() { " + "vec3 normal = normalize(gl_NormalMatrix * gl_Normal); vec4 position = gl_ModelViewMatrix * gl_Vertex; " + "reflection = reflect(position.xyz, normal); gl_Position = ftransform(); }", + "varying vec3 reflection; varying vec4 color; void main() { " + "vec4 ncolor; ncolor.xyz = normalize(reflection); ncolor.w = 1.0; gl_FragColor = ncolor * 0.5 + 0.5; }"); + programs[SILHOUETTE] = createShaders("varying vec4 color; void main() { gl_Position = ftransform(); color = gl_Color; }", + "varying vec4 color; void main() { gl_FragColor = color; }"); + + programs[SPECAMB] = createShaders("varying vec3 reflection; varying vec4 color; void main() { " + "vec3 normal = normalize(gl_NormalMatrix * gl_Normal); vec4 position = gl_ModelViewMatrix * gl_Vertex; " + "reflection = reflect(position.xyz, normal); gl_Position = ftransform(); color = gl_Color; }", + "varying vec3 reflection; varying vec4 color; void main() { " + "vec3 ncolor = normalize(reflection); ncolor = ncolor * 0.5 + 0.5; " + "float t = color.x*color.x; gl_FragColor = (1.0-t)*color + t*(vec4(ncolor, 1.0)); }"); + + + // generate a new VBO and get the associated ID + glGenBuffersARB(1, &vbo); + glGenBuffersARB(1, &nbo); + glGenBuffersARB(1, &cbo); + glGenBuffersARB(1, &ibo); +} + +//resample image IF too big. +void AlignSet::resize(int max_side) { + int w = image->width(); + int h = image->height(); + if(image->isNull()) { + w = 1024; + h = 768; + } + + if(w > max_side) { + h = h*max_side/w; + w = max_side; + } + if(h > max_side) { + w = w*max_side/h; + h = max_side; + } + + wt=w; + ht=h; + + if(target) delete []target; + if(render) delete []render; + target = new unsigned char[w*h]; + render = new unsigned char[w*h]; + + + if(image->isNull()) return; + //resize image and store values into render + QImage im; + if(w != image->width() || h != image->height()) + im = image->scaled(w, h, Qt::IgnoreAspectRatio); //Qt::KeepAspectRatio); + else im = *image; + //im.save("image.jpg"); + assert(w == im.width()); + assert(h == im.height()); + QColor color; + int offset = 0; + //equalize image + int histo[256]; + memset(histo, 0, 256*sizeof(int)); + for (int y = h-1; y >= 0; y--) { + for (int x = 0; x < w; x++) { + color.setRgb(im.pixel(x, y)); + unsigned char c = (unsigned char)(color.red() * 0.3f + color.green() * 0.59f + color.blue() * 0.11f); + target[offset] = c; + histo[c]++; + offset++; + } + } +#ifdef RESCALE_HISTO + int cumulative[256]; + cumulative[0] = histo[0]; + for(int i = 1; i < 256; i++) + cumulative[i] = cumulative[i-1] + histo[i]; + + int min = 0; + int max = 255; + for(int i = 0; i < 256; i++) { + if(cumulative[i] > 20) break; + min = i; + } + + //invert cumulative.. + cumulative[255] = histo[255]; + for(int i = 254; i >= 0; i--) + cumulative[i] = cumulative[i+1] + histo[i]; + + for(int i = 255; i >= 0; i--) { + if(cumulative[i] > 20) break; + max = i; + } + assert(max > min); + //rescale between min and max (should use bresenham but I am lazy + unsigned char equa[256]; + for(int i = 0; i < 256; i++) { + if(i < min) equa[i] = 0; + if(i > max) equa[i] = 255; + equa[i] = (255*(i - min))/(max - min); + } + for(int i = 0; i < w*h; i++) + target[i] = equa[target[i]]; +#endif +} + +void AlignSet::renderScene(vcg::Shot &view, int component) { + QSize fbosize(wt,ht); + QGLFramebufferObjectFormat frmt; + frmt.setInternalTextureFormat(GL_RGBA); + frmt.setAttachment(QGLFramebufferObject::Depth); + QGLFramebufferObject fbo(fbosize,frmt); + + float _near, _far; + _near=0.1; + _far=10000; + + GlShot< vcg::Shot >::GetNearFarPlanes(view, mesh->bbox, _near, _far); + //assert(_near <= _far); + if(_near <= 0) _near = 0.1; + if(_far < _near) _far = 1000; + + +//GLenum err = glGetError(); + + //render to FBO + fbo.bind(); + + glViewport(0, 0, wt, ht); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + GlShot< vcg::Shot >::SetView(shot, 0.5*_near, 2*_far); + +// err = glGetError(); + + bool use_colors = false; + bool use_normals = false; + int program = programs[mode]; //standard pipeline + switch(mode){ + case COLOR: + use_colors = true; + break; + case NORMALMAP: + case SPECULAR: + use_normals = true; + break; + case COMBINE: + case SPECAMB: + use_colors = true; + use_normals = true; + break; + case SILHOUETTE: + break; + default: assert(0); + } + glDisable(GL_LIGHTING); + //bind indices + glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, ibo); + + //bind vertices + glEnable(GL_COLOR_MATERIAL); + glBindBufferARB(GL_ARRAY_BUFFER_ARB, vbo); + glEnableClientState(GL_VERTEX_ARRAY); // activate vertex coords array + glVertexPointer(3, GL_FLOAT, 0, 0); // last param is offset, not ptr + +// err = glGetError(); + + glUseProgram(program); + +// err = glGetError(); + + if(use_colors) { + glBindBufferARB(GL_ARRAY_BUFFER_ARB, cbo); + glEnableClientState(GL_COLOR_ARRAY); + glColorPointer(4, GL_UNSIGNED_BYTE, 0, 0); + } + if(use_normals) { + glBindBufferARB(GL_ARRAY_BUFFER_ARB, nbo); + glEnableClientState(GL_NORMAL_ARRAY); + glNormalPointer(GL_FLOAT, 0, 0); + } + +// err = glGetError(); + + + int start = 0; + int tot = 30000; + if (mesh->fn>0) + { + while(start < mesh->fn) { + glDrawElements(GL_TRIANGLES, tot*3, GL_UNSIGNED_INT, (void *)(start*3*sizeof(int))); + start += tot; + if(start + tot > mesh->fn) + tot = mesh->fn - start; + } + } + else glDrawArrays(GL_POINTS, 0, mesh->vn); + + + + render = new unsigned char[wt*ht]; + + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glPixelStorei(GL_PACK_ALIGNMENT, 1); + + switch(component) { + case 0: glReadPixels( 0, 0, wt, ht, GL_RED, GL_UNSIGNED_BYTE, render); break; + case 1: glReadPixels( 0, 0, wt, ht, GL_GREEN, GL_UNSIGNED_BYTE, render); break; + case 2: glReadPixels( 0, 0, wt, ht, GL_BLUE, GL_UNSIGNED_BYTE, render); break; + case 3: glReadPixels( 0, 0, wt, ht, GL_ALPHA, GL_UNSIGNED_BYTE, render); break; + case 4: break; + } + + //err = glGetError(); + + glDisableClientState(GL_VERTEX_ARRAY); // deactivate vertex array + if(use_colors) glDisableClientState(GL_COLOR_ARRAY); + if(use_normals) glDisableClientState(GL_NORMAL_ARRAY); + + //err = glGetError(); + + // bind with 0, so, switch back to normal pointer operation + glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0); + glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0); + + switch(mode) { + case SILHOUETTE: + case COLOR: + case COMBINE: + case NORMALMAP: glEnable(GL_LIGHTING); break; + default: break; + } + + // standard opengl pipeline is re-activated + glUseProgram(0); + + GlShot< vcg::Shot >::UnsetView(); + + glFinish(); + /*QImage l=fbo.toImage(); + l.save("rendering.jpg");*/ + fbo.release(); + +} + +void AlignSet::readRender(int component) { + QSize fbosize(wt,ht); + QGLFramebufferObjectFormat frmt; + frmt.setInternalTextureFormat(GL_RGBA); + frmt.setAttachment(QGLFramebufferObject::Depth); + QGLFramebufferObject fbo(fbosize,frmt); + + fbo.bind(); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glPixelStorei(GL_PACK_ALIGNMENT, 1); + + switch(component) { + case 0: glReadPixels( 0, 0, width(), height(), GL_RED, GL_UNSIGNED_BYTE, render); break; + case 1: glReadPixels( 0, 0, width(), height(), GL_GREEN, GL_UNSIGNED_BYTE, render); break; + case 2: glReadPixels( 0, 0, width(), height(), GL_BLUE, GL_UNSIGNED_BYTE, render); break; + case 3: glReadPixels( 0, 0, width(), height(), GL_ALPHA, GL_UNSIGNED_BYTE, render); break; + } + QImage l=fbo.toImage(); + l.save("puppo.jpg"); + fbo.release(); +} + +GLuint AlignSet::createShaderFromFiles(QString name) { + QString vert = "shaders/" + name + ".vert"; + QString frag = "shaders/" + name + ".frag"; + + const char *vs_src = ShaderUtils::importShaders(vert.toAscii().data()); + if(!vs_src) { + cerr << "Could not load shader: " << qPrintable(vert) << endl; + return 0; + } + + const char *fs_src = ShaderUtils::importShaders(frag.toAscii().data()); + if(!fs_src) { + cerr << "Could not load shader: " << qPrintable(frag) << endl; + return 0; + } + + return createShaders(vs_src, fs_src); +} + +GLuint AlignSet::createShaders(const char *vert, const char *frag) { + GLuint vs = glCreateShader(GL_VERTEX_SHADER); + glShaderSource(vs, 1, &vert, NULL); + ShaderUtils::compileShader(vs); + + GLuint fs = glCreateShader(GL_FRAGMENT_SHADER); + glShaderSource(fs, 1, &frag, NULL); + ShaderUtils::compileShader(fs); + + GLuint prog = glCreateProgram(); + glAttachShader(prog, vs); + glAttachShader(prog, fs); + + ShaderUtils::linkShaderProgram(prog); + return prog; +} + diff --git a/src/plugins_unsupported/filter_mutualinfo/alignset.h b/src/plugins_unsupported/filter_mutualinfo/alignset.h new file mode 100644 index 000000000..a9743aba0 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/alignset.h @@ -0,0 +1,78 @@ +#ifndef ALIGNSET_H +#define ALIGNSET_H + + +#include +#include +#include + +// local headers +#include "common/meshmodel.h" + +// VCG headers +#include + + +//#include "fbo.h" + +#include "pointCorrespondence.h" + +class QGLFramebufferObject; + +class AlignSet { + //typedef vcg::Camera Camera; + //typedef vcg::Shot Shot; + //typedef vcg::Box3 Box; + + public: + + int wt,ht; + CMeshO* mesh; + QImage* image; + double imageRatio; + vcg::Shot shot; + vcg::Box3 box; + QList *correspList; //List that includes corresponces involving the model + double error; //alignment error in px + + GLuint vbo, nbo, cbo, ibo; // vertex buffer object (vertices, normals, colors indices) + + GLint programs[6]; + + enum RenderingMode {COMBINE=0, NORMALMAP=1, COLOR=2, SPECULAR=3, SILHOUETTE=4, SPECAMB = 5}; + RenderingMode mode; + + unsigned char *target, *render; //buffers for rendered images + + AlignSet(); + ~AlignSet(); + + void initializeGL(); + + int width() { return wt; } + int height() { return ht; } + void resize(int max_side); // resize the fbo and the images so that the longest side is max_side + double focal(); + bool setFocal(double f); //return false if unchanged + void setPixelSizeMm(double ccdWidth); + + void renderScene(vcg::Shot &shot, int component); + void readRender(int component); + + void drawMeshPoints(); + void drawImagePoints(); + + void undistortImage(); + + void resetAlign(); + + private: + + + + GLuint createShaderFromFiles(QString basename); // converted into shader/basename.vert .frag + GLuint createShaders(const char *vert, const char *frag); + +}; + +#endif diff --git a/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.cpp b/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.cpp new file mode 100644 index 000000000..0f3b28252 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.cpp @@ -0,0 +1,315 @@ +/**************************************************************************** +* MeshLab o o * +* A versatile mesh processing toolbox o o * +* _ O _ * +* Copyright(C) 2005 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program 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 2 of the License, or * +* (at your option) any later version. * +* * +* This program 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 (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ + +#include "filter_mutualinfo.h" +#include +#include "solver.h" +#include "mutual.h" + + +//#include "shutils.h" + +#include +#include + +// Constructor usually performs only two simple tasks of filling the two lists +// - typeList: with all the possible id of the filtering actions +// - actionList with the corresponding actions. If you want to add icons to your filtering actions you can do here by construction the QActions accordingly + +//AlignSet align; + + +FilterMutualInfoPlugin::FilterMutualInfoPlugin() +{ + typeList << FP_IMAGE_ALIGN; + + foreach(FilterIDType tt , types()) + actionList << new QAction(filterName(tt), this); +} + +// ST() must return the very short string describing each filtering action +// (this string is used also to define the menu entry) +QString FilterMutualInfoPlugin::filterName(FilterIDType filterId) const +{ + switch(filterId) { + case FP_IMAGE_ALIGN : return QString("Image Registration: Mutual Information"); + default : assert(0); + } + return QString(); +} + +// Info() must return the longer string describing each filtering action +// (this string is used in the About plugin dialog) + QString FilterMutualInfoPlugin::filterInfo(FilterIDType filterId) const +{ + switch(filterId) { + case FP_IMAGE_ALIGN : return QString("Register an image on a 3D model using Mutual Information. This filter is an implementation of Corsini et al. 'Image-to-geometry registration: a mutual information method exploiting illumination-related geometric properties', 2009"); + default : assert(0); + } + return QString("Unknown Filter"); +} + +// The FilterClass describes in which generic class of filters it fits. +// This choice affect the submenu in which each filter will be placed +// More than a single class can be choosen. +FilterMutualInfoPlugin::FilterClass FilterMutualInfoPlugin::getClass(QAction *a) +{ + switch(ID(a)) + { + case FP_IMAGE_ALIGN : return MeshFilterInterface::Camera; + default : assert(0); + } + return MeshFilterInterface::Generic; +} + +// This function define the needed parameters for each filter. Return true if the filter has some parameters +// it is called every time, so you can set the default value of parameters according to the mesh +// For each parameter you need to define, +// - the name of the parameter, +// - the string shown in the dialog +// - the default value +// - a possibly long string describing the meaning of that parameter (shown as a popup help in the dialog) +void FilterMutualInfoPlugin::initParameterSet(QAction *action,MeshDocument & md, RichParameterSet & parlst) +{ + QStringList rendList; + switch(ID(action)) { + case FP_IMAGE_ALIGN : + /*parlst.addParam(new RichMesh ("SourceMesh", md.mm(),&md, "Source Mesh", + "The mesh on which the image must be aligned"));*/ + /*parlst.addParam(new RichRaster ("SourceRaster", md.rm(),&md, "Source Raster", + "The mesh on which the image must be aligned"));*/ + + + rendList.push_back("Combined"); + rendList.push_back("Normal map"); + rendList.push_back("Color per vertex"); + rendList.push_back("Specular"); + rendList.push_back("Silhouette"); + rendList.push_back("Specular combined"); + + //rendList.push_back("ABS Curvature"); + parlst.addParam(new RichEnum("RenderingMode", 0, rendList, tr("Rendering mode:"), + QString("Rendering mode used for Mutual Information maximization"))); + + parlst.addParam(new RichShotf ("Shot", vcg::Shotf(),"Set Shot", "The shot that will be used as a starting position")); + + parlst.addParam(new RichBool("Estimate Focal",false,"Estimate focal length","Estimate focal length")); + parlst.addParam(new RichBool("Fine",true,"Fine Alignment","Fine alignment")); + + parlst.addParam(new RichInt("Num. of Iterations",100,"Max iterations","Maximum number of iterations")); + parlst.addParam(new RichFloat("Tolerance",0.1,"Tolerance","Threshold to stop convergence")); + + parlst.addParam(new RichFloat("Expected Variance",2.0,"Expected Variance","Expected Variance")); + parlst.addParam(new RichInt("Background weight",2,"Background weight","Weight of background pixels")); + + + + /*parlst.addParam(new RichBool ("UpdateNormals", + true, + "Recompute normals", + "Toggle the recomputation of the normals after the random displacement.\n\n" + "If disabled the face normals will remains unchanged resulting in a visually pleasant effect.")); + parlst.addParam(new RichAbsPerc("Displacement", + m.cm.bbox.Diag()/100.0,0,m.cm.bbox.Diag(), + "Max displacement", + "The vertex are displaced of a vector whose norm is bounded by this value"));*/ + break; + + default : assert(0); + } +} + +// The Real Core Function doing the actual mesh processing. +// Move Vertex of a random quantity +bool FilterMutualInfoPlugin::applyFilter(QAction */*filter*/, MeshDocument &md, RichParameterSet & par, vcg::CallBackPos *cb) +{ + Solver solver; + MutualInfo mutual; + if (md.rasterList.size()==0) + { + Log(0, "You need a Raster Model to apply this filter!"); + return false; + } + else + align.image=&md.rm()->currentPlane->image; + + align.mesh=&md.mm()->cm; + + solver.optimize_focal=par.getBool("Estimate Focal"); + solver.fine_alignment=par.getBool("Fine"); + solver.variance=par.getFloat("Expected Variance"); + solver.tolerance=par.getFloat("Tolerance"); + solver.maxiter=par.getInt("Num. of Iterations"); + + mutual.bweight=par.getInt("Background weight"); + + int rendmode= par.getEnum("RenderingMode"); + + switch(rendmode){ + case 0: + align.mode=AlignSet::COMBINE; + break; + case 1: + align.mode=AlignSet::NORMALMAP; + break; + case 2: + align.mode=AlignSet::COLOR; + break; + case 3: + align.mode=AlignSet::SPECULAR; + break; + case 4: + align.mode=AlignSet::SILHOUETTE; + break; + case 5: + align.mode=AlignSet::SPECAMB; + break; + default: + align.mode=AlignSet::COMBINE; + break; + } + + this->glContext->makeCurrent(); + if (this->initGL() == false) + return false; + + vcg::Point3f *vertices = new vcg::Point3f[align.mesh->vn]; + vcg::Point3f *normals = new vcg::Point3f[align.mesh->vn]; + vcg::Color4b *colors = new vcg::Color4b[align.mesh->vn]; + unsigned int *indices = new unsigned int[align.mesh->fn*3]; + + for(int i = 0; i < align.mesh->vn; i++) { + vertices[i] = align.mesh->vert[i].P(); + normals[i] = align.mesh->vert[i].N(); + colors[i] = align.mesh->vert[i].C(); + } + + for(int i = 0; i < align.mesh->fn; i++) + for(int k = 0; k < 3; k++) + indices[k+i*3] = align.mesh->face[i].V(k) - &*align.mesh->vert.begin(); + + glBindBufferARB(GL_ARRAY_BUFFER_ARB, align.vbo); + glBufferDataARB(GL_ARRAY_BUFFER_ARB, align.mesh->vn*sizeof(vcg::Point3f), + vertices, GL_STATIC_DRAW_ARB); + glBindBufferARB(GL_ARRAY_BUFFER_ARB, align.nbo); + glBufferDataARB(GL_ARRAY_BUFFER_ARB, align.mesh->vn*sizeof(vcg::Point3f), + normals, GL_STATIC_DRAW_ARB); + glBindBufferARB(GL_ARRAY_BUFFER_ARB, align.cbo); + glBufferDataARB(GL_ARRAY_BUFFER_ARB, align.mesh->vn*sizeof(vcg::Color4b), + colors, GL_STATIC_DRAW_ARB); + glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0); + + glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, align.ibo); + glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB, align.mesh->fn*3*sizeof(unsigned int), + indices, GL_STATIC_DRAW_ARB); + glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0); + + + // it is safe to delete after copying data to VBO + delete []vertices; + delete []normals; + delete []colors; + delete []indices; + + align.shot=par.getShotf("Shot"); + + align.shot.Intrinsics.ViewportPx[0]=int((double)align.shot.Intrinsics.ViewportPx[1]*align.image->width()/align.image->height()); + align.shot.Intrinsics.CenterPx[0]=(int)(align.shot.Intrinsics.ViewportPx[0]/2); + + if (solver.fine_alignment) + solver.optimize(&align, &mutual, align.shot); + else + solver.iterative(&align, &mutual, align.shot); + //align.renderScene(align.shot, 3); + //align.readRender(0); + + md.rm()->shot=align.shot; + float ratio=(float)md.rm()->currentPlane->image.height()/(float)align.shot.Intrinsics.ViewportPx[1]; + md.rm()->shot.Intrinsics.ViewportPx[0]=md.rm()->currentPlane->image.width(); + md.rm()->shot.Intrinsics.ViewportPx[1]=md.rm()->currentPlane->image.height(); + md.rm()->shot.Intrinsics.PixelSizeMm[1]/=ratio; + md.rm()->shot.Intrinsics.PixelSizeMm[0]/=ratio; + md.rm()->shot.Intrinsics.CenterPx[0]=(int)((float)md.rm()->shot.Intrinsics.ViewportPx[0]/2.0); + md.rm()->shot.Intrinsics.CenterPx[1]=(int)((float)md.rm()->shot.Intrinsics.ViewportPx[1]/2.0); + + this->glContext->doneCurrent(); + //emit md.rasterSetChanged(); + return true; +} + +bool FilterMutualInfoPlugin::initGL() +{ + GLenum err = glewInit(); + Log(0, "GL Initialization"); + if (GLEW_OK != err) { + Log(0, "GLEW initialization error!"); + return false; + } + + if (!glewIsSupported("GL_EXT_framebuffer_object")) { + Log(0, "Graphics hardware does not support FBOs"); + return false; + } + if (!glewIsSupported("GL_ARB_vertex_shader") || !glewIsSupported("GL_ARB_fragment_shader") || + !glewIsSupported("GL_ARB_shader_objects") || !glewIsSupported("GL_ARB_shading_language")) { + //QMessageBox::warning(this, "Danger, Will Robinson!", +// "Graphics hardware does not fully support Shaders"); + } + + if (!glewIsSupported("GL_ARB_texture_non_power_of_two")) { + Log(0,"Graphics hardware does not support non-power-of-two textures"); + return false; + } + if (!glewIsSupported("GL_ARB_vertex_buffer_object")) { + Log(0, "Graphics hardware does not support vertex buffer objects"); + return false; + } + + glEnable(GL_NORMALIZE); + glDepthRange (0.0, 1.0); + + glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST); + glEnable(GL_POLYGON_SMOOTH); + glShadeModel(GL_SMOOTH); + glDisable(GL_POLYGON_SMOOTH); + + //AlignSet &align = Autoreg::instance().align; + align.initializeGL(); + align.resize(800); + //assert(glGetError() == 0); + + Log(0, "GL Initialization done"); + return true; + +} + +QString FilterMutualInfoPlugin::filterScriptFunctionName( FilterIDType filterID ) +{ + switch(filterID) { + case FP_IMAGE_ALIGN : return QString("imagealignment"); + default : assert(0); + } + return QString(); +} + +Q_EXPORT_PLUGIN(FilterMutualInfoPlugin) diff --git a/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.h b/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.h new file mode 100644 index 000000000..94a910343 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.h @@ -0,0 +1,73 @@ +/**************************************************************************** +* MeshLab o o * +* A versatile mesh processing toolbox o o * +* _ O _ * +* Copyright(C) 2005 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program 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 2 of the License, or * +* (at your option) any later version. * +* * +* This program 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 (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ +/**************************************************************************** + History +$Log: sampleplugins.h,v $ +Revision 1.2 2006/11/29 00:59:21 cignoni +Cleaned plugins interface; changed useless help class into a plain string + +Revision 1.1 2006/09/25 09:24:39 e_cerisoli +add sampleplugins + +****************************************************************************/ + +#ifndef FILTER_MUTUALINFO_H +#define FILTER_MUTUALINFO_H + +#include + +#include +#include "alignset.h" + + + +class QScriptEngine; + +class FilterMutualInfoPlugin : public QObject, public MeshFilterInterface +{ + Q_OBJECT + Q_INTERFACES(MeshFilterInterface) + +public: + enum { FP_IMAGE_ALIGN } ; + + FilterMutualInfoPlugin(); + + virtual QString pluginName(void) const { return "FilterMutualInfoPlugin"; } + + QString filterName(FilterIDType filter) const; + QString filterInfo(FilterIDType filter) const; + void initParameterSet(QAction *,MeshDocument & md, RichParameterSet & /*parent*/); + bool applyFilter(QAction *filter, MeshDocument &md, RichParameterSet & /*parent*/, vcg::CallBackPos * cb) ; + int postCondition( QAction* ) const {return MeshModel::MM_UNKNOWN;}; + FilterClass getClass(QAction *a); + QString filterScriptFunctionName(FilterIDType filterID); + + bool initGL(); +private: + AlignSet align; + +}; + + +#endif diff --git a/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.pro b/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.pro new file mode 100644 index 000000000..892e4fcfa --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/filter_mutualinfo.pro @@ -0,0 +1,21 @@ +include (../../shared.pri) + +HEADERS += filter_mutualinfo.h \ + alignset.h \ + levmarmethods.h \ + mutual.h \ + parameters.h \ + pointCorrespondence.h \ + pointOnLayer.h \ + shutils.h \ + solver.h + +SOURCES += filter_mutualinfo.cpp \ + alignset.cpp \ + levmarmethods.cpp \ + mutual.cpp \ + parameters.cpp \ + pointCorrespondence.cpp \ + solver.cpp + +TARGET = filter_mutualinfo diff --git a/src/plugins_unsupported/filter_mutualinfo/levmarmethods.cpp b/src/plugins_unsupported/filter_mutualinfo/levmarmethods.cpp new file mode 100644 index 000000000..4b94bcf3d --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/levmarmethods.cpp @@ -0,0 +1,239 @@ +/**************************************************************************** +LEVMAR METHODS + Interface class between the levmar.lib and the project structure. + +*****************************************************************************/ + +#include "levmarmethods.h" //base header + +#include + +/********************************************************************** +Calibrate extrinsics (if p_foc is false) or focal (if p_foc is true) +**********************************************************************/ +bool LevmarMethods::calibrate( vcg::Shot* shot,std::list* corr, bool p_foc) +{ + bool my_ret_val=false; + + //if(corr->size() >= MIN_POINTS_FOR_CALIBRATE){ + + double p[7]; + + Shot2Levmar(shot,p,p_foc); + + LevmarData* data = new LevmarData(); + double* x = new double[corr->size()*2]; + double opts[LM_OPTS_SZ]; + double info[LM_INFO_SZ]; + + if(createDataSet(corr,shot,data,x,opts,info)) + { + int n= corr->size()*2; + if(!p_foc){ + int m=6; + + //my_ret_val = dlevmar_dif(estimateExtr, p,x,m,n,1000,opts,info,NULL,NULL,data); OKKIO + } + else{ + int m=1; + + //my_ret_val = dlevmar_dif(estimateFocal, p,x,m,n,100000,opts,info,NULL,NULL,data); OKKIO + } + + Levmar2Shot(shot,p,p_foc); + vcg::Matrix44f rot = shot->Extrinsics.Rot(); + } + + delete data; + delete[] x; + + return my_ret_val; +} + + +/********************************************* +CREATE DATA SET +Modify data,x, opts and info(?) +*********************************************/ +//TOGLIERE SHOT DAI PARAMETRI! +bool LevmarMethods::createDataSet(std::list* corr,vcg::Shot* s, LevmarData* data, double* x,double opts[LM_OPTS_SZ],double info[LM_INFO_SZ]) +{ + bool my_ret_val=false; + // OKKIO!!! +// +// //lascia lo spazio dei punti come per Tsai +// vcg::Point3f *p1; +// vcg::Point2d *p2; +// int count=0; +// +// data->points3d = new vcg::Point3d*[corr->size()]; +// data->levmarCam = s; +// +// +// std::list::iterator it_c; +// LevmarCorrelation* c; +// +// double ratio = s->Intrinsics.ViewportPx.X()/(double) s->Intrinsics.ViewportPx.Y(); +// +// for ( it_c= corr->begin() ; it_c !=corr->end(); it_c++ ){ +// c=&*it_c; +// p1=&(c->point3d); +// p2=&(c->point2d); +// +// if(p1!=NULL && p2!=NULL) +// { +// (data->points3d)[count] = p1; +//// cd.Xf[count] = (p2->X()+1)/2.0 * cp.Cx*2.0; +//// cd.Yf[count] = ((-p2->Y())+1)/2.0 * cp.Cy*2.0; +// +// x[count*2] = ((p2->X()/ratio) +1)/2.0 * s->Intrinsics.CenterPx.X()*2.0; +// x[count*2+1] = ((p2->Y())+1)/2.0 * s->Intrinsics.CenterPx.Y()*2.0; +// count++; +// } +// //if(count>=MAX_POINTS) break; +// +// }//all corrs +// +// assert(count==corr->size()); +// +// opts[0] = 1E-6; +// opts[1] = 1E-15; +// opts[2] = 1E-15; +// opts[3] = 1E-15; +// opts[4]= LM_DIFF_DELTA; +// +// //qDebug("all points: %i",count); +// if(count>0 ) my_ret_val = true; +// + return my_ret_val; +} + + +/********************************************* +SHOT 2 TSAI +Transformation of the camera data between levmar structure and vcg structure +*********************************************/ +void LevmarMethods::Shot2Levmar(vcg::Shot* shot, double* p, bool p_foc){ + + if(!p_foc){ + float alpha, beta, gamma; + vcg::Matrix44f rot = shot->Extrinsics.Rot(); + rot.ToEulerAngles(alpha, beta, gamma); + + vcg::Point3f tVect = shot->Extrinsics.Tra(); + + p[0] = alpha; + p[1] = beta; + p[2] = gamma; + p[3] = tVect[0]; + p[4] = tVect[1]; + p[5] = tVect[2]; + } + else + p[0]= shot->Intrinsics.FocalMm; + + //Cam2Levmar(shot); +} + +/********************************************* +TSAI 2 SHOT +Transformation of the camera data between levmar structure and vcg structure +*********************************************/ +void LevmarMethods::Levmar2Shot(vcg::Shot* shot, double *p,bool p_foc) { + + //if(p_foc) + // shot->Intrinsics.FocalMm = cc.f;//*cp.sx;// *SCALE_FACTOR; + ///* old ones + //shot->Intrinsics.DistorCenterPx[0] = cc.p1; + //shot->Intrinsics.DistorCenterPx[1] = cc.p2; + // + //shot->Intrinsics.DistorCenterPx[0] = shot->Intrinsics.CenterPx.X()+(cc.p1/shot->Intrinsics.PixelSizeMm.X()); + //shot->Intrinsics.DistorCenterPx[1] = shot->Intrinsics.CenterPx.Y()+(cc.p2/shot->Intrinsics.PixelSizeMm.Y()); + //*/ + //shot->Intrinsics.DistorCenterPx[0] = cp.Cx; + //shot->Intrinsics.DistorCenterPx[1] = cp.Cy; + + //shot->Intrinsics.k[0]=cc.kappa1; + if(!p_foc){ + + //* ROTATION */ + vcg::Matrix44::ScalarType> mat; + mat.SetIdentity(); + mat.FromEulerAngles(p[0],p[1],p[2]); + + shot->Extrinsics.SetRot(mat); + + //* TRANSLATION */ + //vcg::Point3d tl = shot->Extrinsics.Tra(); + // + //tl = vcg::Inverse(shot->Extrinsics.Rot())* vcg::Point3d(-cc.Tx,cc.Ty,cc.Tz); + + shot->Extrinsics.SetTra(vcg::Point3f(p[3], p[4], p[5])); + } + else + shot->Intrinsics.FocalMm=p[0]; +} + +void LevmarMethods::Cam2Levmar(vcg::Shot *s){ + + //cp.Ncx = s->Intrinsics.ViewportPx.X(); // [sel] Number of sensor elements in camera's x direction // + //cp.Nfx = s->Intrinsics.ViewportPx.X(); // [pix] Number of pixels in frame grabber's x direction // + //cp.dx = s->Intrinsics.PixelSizeMm.X();//*SCALE_FACTOR; // [mm/sel] X dimension of camera's sensor element (in mm) // + //cp.dy = s->Intrinsics.PixelSizeMm.Y();//*SCALE_FACTOR; // [mm/sel] Y dimension of camera's sensor element (in mm) // + // + //cp.dpx = cp.dx * cp.Nfx/cp.Ncx; // [mm/pix] Effective X dimension of pixel in frame grabber // + //cp.dpy = cp.dy; // [mm/pix] Effective Y dimension of pixel in frame grabber // + + //cp.Cx = s->Intrinsics.CenterPx.X(); // [pix] Z axis intercept of camera coordinate system // + //cp.Cy = s->Intrinsics.CenterPx.Y(); // [pix] Z axis intercept of camera coordinate system // + // + //cp.sx = 1.0; // [] Scale factor to compensate for any error in dpx // + + //cc.f = s->Intrinsics.FocalMm;// *SCALE_FACTOR; + //cc.kappa1 = s->Intrinsics.k[0]; +} + +//Estimate only extrinsics. +void LevmarMethods::estimateExtr(double *p, double *x, int m, int n, void *data) +{ + vcg::Point3f** ptr = ((LevmarData*) data)->points3d; + vcg::Shot* levmarCam = ((LevmarData*) data)->levmarCam; + + vcg::Matrix44f matrix; +#ifdef USE_QUATERNION + vcg::Quaterniond quaternion(p[0], p[1], p[2], p[3]); + quaternion.ToMatrix(matrix); + levmarCam.Extrinsics.SetRot(matrix); + levmarCam.Extrinsics.SetTra(vcg::Point3d(p[4], p[5], p[6])); +#else + matrix.SetIdentity(); + matrix.FromEulerAngles(p[0], p[1], p[2]); + levmarCam->Extrinsics.SetRot(matrix); + levmarCam->Extrinsics.SetTra(vcg::Point3f(p[3], p[4], p[5])); +#endif + + for (int i = 0; i < n/2; i++) + { + vcg::Point2f point2d = levmarCam->Project(*ptr[i]); + x[i*2] = point2d.X(); + x[i*2 + 1] = point2d.Y(); + } +} + +//Estimate only the focal. +void LevmarMethods::estimateFocal(double *p, double *x, int m, int n, void *data) +{ + vcg::Point3f** ptr = ((LevmarData*) data)->points3d; + vcg::Shot* levmarCam = ((LevmarData*) data)->levmarCam; + + levmarCam->Intrinsics.FocalMm = p[0]; + + for (int i = 0; i < n/2; i++) + { + vcg::Point2f point2d = levmarCam->Project(*ptr[i]); + x[i*2] = point2d.X(); + x[i*2 + 1] = point2d.Y(); + } + + +} \ No newline at end of file diff --git a/src/plugins_unsupported/filter_mutualinfo/levmarmethods.h b/src/plugins_unsupported/filter_mutualinfo/levmarmethods.h new file mode 100644 index 000000000..484d6dbf7 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/levmarmethods.h @@ -0,0 +1,49 @@ +/** Levmar Methods -- Interface with the levamr lib + +The class is static, so a simple call to calibrate() is +sufficient to get a calibrated shot.
+ +*/ + +#include +#include +#include +#include + +#include + +#include "../../external/levmar-2.3/lm.h" + + +struct LevmarCorrelation { + vcg::Point3f point3d; + vcg::Point2d point2d; +}; + +struct LevmarData{ + vcg::Point3f** points3d; + vcg::Shot* levmarCam; +}; + + +class LevmarMethods +{ +public: + //Calibration of the shot according to the 2D and 3D points + static bool calibrate( vcg::Shot* shot,std::list* corr,bool p_focal); + + ///Transformation of the camera data between tsai structure and vcg structure + ///True if you want calibrate the focal, false if you want calibrate the extrinsics + static void Shot2Levmar(vcg::Shot*,double* p,bool p_foc); + ///Transformation of the camera data between tsai structure and vcg structure + ///True if you have calibrated the focal, false if you had calibrated the extrinsics + static void Levmar2Shot(vcg::Shot*, double *p, bool p_foc); + +private: + static void Cam2Levmar(vcg::Shot*); + static bool createDataSet(std::list* corr,vcg::Shot* s, LevmarData* data, double* x,double opts[LM_OPTS_SZ],double info[LM_INFO_SZ]); + static void estimateExtr(double *p, double *x, int m, int n, void *data); + static void estimateFocal(double *p, double *x, int m, int n, void *data); + +}; + diff --git a/src/plugins_unsupported/filter_mutualinfo/mutual.cpp b/src/plugins_unsupported/filter_mutualinfo/mutual.cpp new file mode 100644 index 000000000..a11704816 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/mutual.cpp @@ -0,0 +1,130 @@ +#include +#include +#include +#include + +#include +#include /*debug*/ +#include "mutual.h" + +using namespace std; +MutualInfo::MutualInfo(unsigned int _nbins, int _bweight, bool _use_background): + bweight(_bweight), use_background(_use_background), + histo2D(NULL), histoA(NULL), histoB(NULL) { + + setBins(_nbins); +} + +MutualInfo::~MutualInfo() { + delete []histo2D; + delete []histoA; + delete []histoB; +} + +void MutualInfo::setBins(unsigned int _nbins) { + nbins = _nbins; + assert(!(nbins & (nbins-1))); + + if(histo2D) delete []histo2D; + if(histoA) delete []histoA; + if(histoB) delete []histoB; + histo2D = new unsigned int[nbins*nbins]; + histoA = new unsigned int[nbins]; + histoB = new unsigned int[nbins]; +} + +double MutualInfo::info(int width, int height, + unsigned char *target, unsigned char *render, + int startx, int endx, + int starty, int endy) { + histogram(width, height, target, render, startx, endx, starty, endy); + + memset(histoA, 0, nbins*sizeof(int)); + memset(histoB, 0, nbins*sizeof(int)); + double n = 0.0; + + int i = 0; + for(unsigned int y = 0; y < nbins; y++) { + unsigned int &b = histoB[y]; + for(unsigned int x = 0; x < nbins; x++) { + int ab = histo2D[i++]; + histoA[x] += ab; + b += ab; +// if(ab != 0) cout << "1"; +// else cout << "0"; + } +// cout << endl; + n += b; + } + //cout << endl; + double ILOG2 = 1/log(2.0); + //assert(n > 0); + if(n == 0) n = 1; + double m = 0.0; + for(unsigned int y = 0; y < nbins; y++) { + double b = histoB[y]; + if(b == 0) continue; + for(unsigned int x = 0; x < nbins; x++) { + double ab = histo2D[x + nbins*y]; + if(ab == 0) continue; + double a = histoA[x]; + m += ab * log((n*ab)/(a*b))*ILOG2; + } + } + m /= n; + + #ifdef USE_MUTUAL + for(int x = 0; x < bins; x++) { + double a = histoA[x]/n; + double b = histoB[x]/n; + if(a > 0) + m -= a*log(a); + if(b > 0) m -= b*log(b); + } + #endif + return m; +} + +void MutualInfo::histogram(int width, int height, + unsigned char *target, unsigned char *render, + int startx, int endx, + int starty, int endy) { + if(endx == 0) endx = width; + if(endy == 0) endy = height; + memset(histo2D, 0, nbins*nbins*sizeof(int)); + int side = 256/nbins; + assert(!(side & (side-1))); + + //k is the log2 of side + int k = 0; + while ( side>>=1 ) { ++k; } + + int bins = nbins; + int s = 0; + while ( bins>>=1) { ++s; } + + for(int y = starty; y < endy; y++) { + int offset = width*y + startx; + for(int x = startx; x < endx; x++, offset++) { + unsigned char a = target[offset]>>k; //instead of /side; + unsigned char b = render[offset]>>k; //instead of /side; + histo2D[a + (b< first row of histo2D + if(bweight != 0) { + for(unsigned int i = 0; i < nbins; i++) + histo2D[i] /= bweight; + } else + memset(histo2D, 0, nbins*sizeof(int)); + + /*unsigned char buffer[nbins*nbins*4]; + for(int i = 0; i < nbins*nbins; i++) { + buffer[i*4] = buffer[i*4+1] = buffer[i*4+2] = (unsigned char)(10*log(histo2D[i])); + buffer[i*4+3] = 0; + } + QImage im(buffer, nbins, nbins, QImage::Format_ARGB32); + im.save("prova.jpg");*/ +} + diff --git a/src/plugins_unsupported/filter_mutualinfo/mutual.h b/src/plugins_unsupported/filter_mutualinfo/mutual.h new file mode 100644 index 000000000..147f13ba6 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/mutual.h @@ -0,0 +1,26 @@ +#ifndef MUTUAL_INFORMATION_H +#define MUTUAL_INFORMATION_H + +class MutualInfo { + public: + int bweight; + bool use_background; + //bweight: weithg of boundary pixels in mutual information (1/bweight is weighting function) + MutualInfo(unsigned int nbins = 128, int bweight = 2, bool use_background = true); + ~MutualInfo(); + + void setBins(unsigned int nbins); + double info(int width, int height, unsigned char *target, unsigned char *render, + int startx = 0, int endx = 0, int starty = 0, int endy = 0); + void histogram(int width, int height, unsigned char *target, unsigned char *render, + int startx = 0, int endx = 0, int starty = 0, int endy = 0); + + private: + unsigned int nbins; + unsigned int *histo2D; //matrix nbisXnbins + unsigned int *histoA; //vector nbins + unsigned int *histoB; +}; + + +#endif diff --git a/src/plugins_unsupported/filter_mutualinfo/parameters.cpp b/src/plugins_unsupported/filter_mutualinfo/parameters.cpp new file mode 100644 index 000000000..27d4cac4a --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/parameters.cpp @@ -0,0 +1,300 @@ +#include + +#include "common/meshmodel.h" +#include "parameters.h" + +using namespace std; +using namespace vcg; + +Parameters::Parameters(bool _use_focal, Shot &_reference, int w, int h, Box &_box, CMeshO &mesh): + use_focal(_use_focal), max_norm(false), reference(_reference), box(_box) +{ + srand(1); + rx = w/(double)reference.Intrinsics.ViewportPx[0]; + ry = h/(double)reference.Intrinsics.ViewportPx[1]; + pointSamples = NULL; + initScale(mesh); +} + + +Parameters::Parameters(bool _use_focal, Shot &_reference, int w, int h, Box &_box): + use_focal(_use_focal), max_norm(false), reference(_reference), box(_box) +{ + srand(1); + rx = w/(double)reference.Intrinsics.ViewportPx[0]; + ry = h/(double)reference.Intrinsics.ViewportPx[1]; + pointSamples = NULL; + initScale(); +} + + +Parameters::~Parameters() +{ + /*if(pointSamples) + delete pointSamples;*/ +} + +int Parameters::size() +{ + //number of used parameters + if(use_focal) return 7; + return 6; +} + + +double Parameters::norm() +{ + double dist = 0.0; + for(int i = 0; i < 7; i++) + dist += p[i]*p[i]; + return sqrt(dist); +} + + +void Parameters::reset() +{ + for(int i = 0; i < 7; i++) + { + p[i] = 0.0; + scale[i] = 1.0; + } +} + + +double Parameters::random(double max, double min) +{ + assert(max >= min); + double r = (double)rand() / ((double)(RAND_MAX)+(double)(1)); + return min + r*(max - min); +} + + +void Parameters::randomDir(int n, double *p, double len) +{ + double dist = 0.0; + for(int i = 0; i < n; i++) + { + p[i] = random(1, -1); + dist += p[i]*p[i];; + } + dist = len/sqrt(dist); + for(int i = 0; i < n; i++) + p[i] *= dist; +} + + +void Parameters::rattle(double amount) +{ + double r[7]; + randomDir(size(), r, amount); + for(int i = 0; i < size(); i++) + p[i] += r[i]; +} + + +void Parameters::initScale(CMeshO &mesh) +{ + reset(); + pointSamples = new CMeshO(); + clusteringGrid.Init(mesh.bbox, 10000, mesh.bbox.Diag()*0.02f ); + clusteringGrid.AddPointSet(mesh); + clusteringGrid.ExtractPointSet(*pointSamples); + const double step = 0.1; + for(int i = 0; i < size(); i++) + { + p[i] = step; + Shot test = toShot(false); + double diff = pixelDiff(test, mesh)/step; + if(diff <= 0) + { + scale[i] = 1; + cerr << "WARNING: parameter " << i << " does not change the image. " << endl; + } + else + scale[i] = 1/diff; + p[i] = 0.0; + } + +} + + +void Parameters::initScale() +{ + reset(); + for(int i = 0; i < size(); i++) + { + p[i] = 1.0; + Shot test = toShot(false); + scale[i] = 1/pixelDiff(test); + p[i] = 0.0; + } +} + + +double Parameters::pixelDiff(CMeshO &mesh) +{ + Shot s = toShot(); + return pixelDiff(s, mesh); +} + + +double Parameters::pixelDiff(Shot &test, CMeshO &mesh) +{ + double maxdist = 0.0; + double avedist = 0.0; + int count = 0; + CMeshO::VertexIterator vi; + for(vi = pointSamples->vert.begin(); vi != pointSamples->vert.end(); ++vi ) + { + vcg::Point3f c; + c.Import((*vi).P()); + Point2f diff = pixelDiff(test, c); + double dd = diff.Norm(); + if(dd <= 0) continue; //outside of viewpoint + if(dd > maxdist) maxdist = dd; + avedist += dd*dd; + count++; + } + //for(int i = 0; i < nsamples; i++) + //{ + // double r = (double)rand() / ((double)(RAND_MAX)+(double)(1)); + // double g = (double)rand() / ((double)(RAND_MAX)+(double)(1)); + // r = r*16000*16000+g*16000; + // int v = ((int)r)%(mesh.vert.size()); + // vcg::Point3d c; + // c.Import(mesh.vert[v].P()); + // Point2d diff = pixelDiff(test, c); + // double dd = diff.Norm(); + // if(dd <= 0) continue; //outside of viewpoint + // if(dd > maxdist) maxdist = dd; + // avedist += dd*dd; + // count++; + //} + if(max_norm) return maxdist; + return sqrt(avedist/count); +} + + +double Parameters::pixelDiff() +{ + Shot s = toShot(); + return pixelDiff(s); +} + + +double Parameters::pixelDiff(Shot &test) +{ + double maxdist = 0.0; + double avedist = 0.0; + + for(int i = 0; i < 8; i++) + { + double dd = pixelDiff(test, box.P(i)).Norm(); + if(dd < 0) continue; //outside of viewpoint + if(dd > maxdist) maxdist = dd; + avedist += dd*dd; + } + if(max_norm) return maxdist; + return sqrt(avedist/8); +} + + +vcg::Point2f Parameters::pixelDiff(Shot &test, vcg::Point3f p) +{ + vcg::Point2f before = reference.Project(p)*rx; + if(before[0] < 0 || before[0] > reference.Intrinsics.ViewportPx[0] || before[1] < 0 || before[1] > reference.Intrinsics.ViewportPx[1]) + return vcg::Point2f(0, 0); + vcg::Point2f after = test.Project(p)*rx; + return (after - before); +} + + +vcg::Shot Parameters::toShot(bool scale) +{ + double _p[7]; + scramble(_p, scale); + + Similarity > s; + s.SetIdentity(); + s.rot.FromEulerAngles(_p[3], _p[4], _p[5]); + s.tra = vcg::Point3f(_p[0], _p[1], _p[2]); + + Shot shot = reference; + if(use_focal) + { + double fov = reference.Intrinsics.FocalMm; + double h = reference.Intrinsics.ViewportPx[0]*reference.Intrinsics.PixelSizeMm[0]; + //double f = h/fov; + //f += _p[6]/100; + //shot.Intrinsics.FocalMm = h/f; + + //double newfov = fov*exp(0.1*_p[6]); + double newfov = fov + _p[6]; + shot.Intrinsics.FocalMm = newfov; + if(shot.Intrinsics.FocalMm <= 1) shot.Intrinsics.FocalMm = 1; + } + else + shot.Intrinsics.FocalMm = reference.Intrinsics.FocalMm; + Matrix44f rot = shot.Extrinsics.Rot(); + Matrix44f irot = Inverse(rot); + + Point3f tra = shot.Extrinsics.Tra(); + + //rotation in camera space, remove it and we are in model space + s.rot = irot*s.rot*rot; + + Matrix44f isrot = Inverse(s.rot); + Point3f center = box.Center(); + + shot.Extrinsics.SetRot(rot *s.rot); + shot.Extrinsics.SetTra(irot*s.tra + isrot*(tra - center) + center); + + return shot; +} + + +void Parameters::scramble(double *_p, bool rescale) +{ + if(rescale) + { + for (int i = 0; i < size(); i++) + _p[i] = p[i]*scale[i]; + } + else + { + for (int i = 0; i < size(); i++) + _p[i] = p[i]; + } + if(use_focal) + { + Point3f center = box.Center(); + //initial distance to the center of the object + double dist = (center - reference.Extrinsics.Tra()).Norm(); + //initial fov + double fov = reference.Intrinsics.FocalMm; + double h = reference.Intrinsics.ViewportPx[0]*reference.Intrinsics.PixelSizeMm[0]; + + /*double f = h/fov; + f += _p[6]/100; + double newfov = h/f;*/ + //double newfov = fov*exp(0.1*_p[6]); + double newfov = fov + _p[6]; + double ratio = (newfov - fov)/fov; + + Point3f fcenter = reference.ConvertWorldToCameraCoordinates(center); + fcenter[0] = 0; fcenter[1] = 0; + fcenter = reference.ConvertCameraToWorldCoordinates(fcenter); + + Point3f view = reference.Extrinsics.Tra(); + //view = center + (view - center)*ratio; + Point3f diff = view - (center + (view - center)*ratio); + + _p[2] += ratio*dist; + + /* double z = (_p[2] + _p[6])/2; + double f = (_p[2] - _p[6])/2; + _p[2] = z; + _p[6] = f; */ + } +} + diff --git a/src/plugins_unsupported/filter_mutualinfo/parameters.h b/src/plugins_unsupported/filter_mutualinfo/parameters.h new file mode 100644 index 000000000..840a45b47 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/parameters.h @@ -0,0 +1,65 @@ +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#include +#include +#include + +#include + +class CMeshO; + +class Parameters +{ + typedef vcg::Shot Shot; + typedef vcg::Box3 Box; + +public: + double p[7]; + double scale[7]; + bool use_focal; + bool max_norm; //use max distance as pixel diff (instead of rms) + + vcg::Shot reference; + Box box; + //vcg::Point3d center; + double rx, ry; //ratio between fbo width and height and reference values + + CMeshO* pointSamples; + vcg::tri::Clustering > clusteringGrid; + + Parameters(): use_focal(true), max_norm(false), pointSamples(NULL) {} + // Parameters(bool _use_focal, Shot &_reference, int w, int h, vcg::Box3d &_box); + Parameters(bool _use_focal, Shot &_reference, int w, int h, vcg::Box3f &_box, CMeshO &mesh); + Parameters(bool _use_focal, Shot &_reference, int w, int h, vcg::Box3f &_box); + + ~Parameters(); + + int size(); //number of used parameters + double &operator[](int n) { return p[n]; } + double norm(); + void reset(); + double random(double max = 1.0, double min = 0.0); + void randomDir(int n, double *p, double len = 1.0); + void rattle(double amount); + + void initScale(CMeshO &mesh); + void initScale(); + + //estimate pixeldifference + double pixelDiff(CMeshO &mesh); + double pixelDiff(vcg::Shot &test, CMeshO &mesh); + + double pixelDiff(); + double pixelDiff(vcg::Shot &test); + + vcg::Point2f pixelDiff(vcg::Shot &test, vcg::Point3f p); + + vcg::Shot toShot(bool scale = true); + //void ShotAndSim(vcg::Shot &shot, vcg::Similarity > &s); + +private: + void scramble(double *_p, bool rescale); +}; + +#endif diff --git a/src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.cpp b/src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.cpp new file mode 100644 index 000000000..ffb49713c --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.cpp @@ -0,0 +1,22 @@ +#include "pointCorrespondence.h" + +PointCorrespondence::PointCorrespondence() +{ + pointList = new QList(); + numofItems=0; +} + +PointCorrespondence::~PointCorrespondence() +{ + delete pointList; +} + +void PointCorrespondence::addPoint(PointOnLayer &newPt) +{ + pointList->append(newPt); + numofItems++; +} + +PointOnLayer PointCorrespondence::getPointAt(int i){ + return pointList->at(i); +} diff --git a/src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.h b/src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.h new file mode 100644 index 000000000..1ce3115fd --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/pointCorrespondence.h @@ -0,0 +1,26 @@ +#ifndef POINTCORRESPONDENCE_H +#define POINTCORRESPONDENCE_H + +#include +#include "pointOnLayer.h" + +class PointCorrespondence +{ + +public: + + PointCorrespondence(); + ~PointCorrespondence(); + + int numofItems; + + void addPoint(PointOnLayer &newPt); + PointOnLayer getPointAt(int i); + +private: + + QList *pointList; +}; + + +#endif // POINTCORRESPONDENCE_H diff --git a/src/plugins_unsupported/filter_mutualinfo/pointOnLayer.h b/src/plugins_unsupported/filter_mutualinfo/pointOnLayer.h new file mode 100644 index 000000000..bee2366cd --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/pointOnLayer.h @@ -0,0 +1,62 @@ +#ifndef POINTONLAYER_H +#define POINTONLAYER_H + + +#include //String compare of class names +#include + + +/** Registration Point -- Contains 2d or 3d coordinates + +... +*/ + +enum PointType { _NO_POINT , _2D_POINT , _3D_POINT }; + +class PointOnLayer +{ +public: + + ///Constructor + PointOnLayer(int id=-1) + { init(id); pType = _NO_POINT;} + ///Constructor + PointOnLayer(int id, double a, double b) + { init(id); setPoint2D(a,b); } + ///Constructor + PointOnLayer(int id, double x, double y, double z) + { init(id); setPoint3D(x,y,z); } + ~PointOnLayer(){} + /// + void init(int id=-1){ + + layerId=id; + } + /// + void setPoint2D(double a, double b) + { + pX = a; pY = b; pZ = 0; + pType = _2D_POINT; + } + /// + void setPoint3D(double x, double y, double z) + { + + pX = x; pY = y; pZ = z; + pType = _3D_POINT; + } + /// + int getLayerId(){return layerId;} + PointType getType(){return pType;} + + +public: //Members + /// + double pX,pY,pZ; + /// + int layerId; + /// + PointType pType; +}; + +#endif // POINTONLAYER_H diff --git a/src/plugins_unsupported/filter_mutualinfo/shutils.h b/src/plugins_unsupported/filter_mutualinfo/shutils.h new file mode 100644 index 000000000..172c839c3 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/shutils.h @@ -0,0 +1,104 @@ +/**************************************************************************** +* AutoReg Tool o o * +* Automatic Registration of Images on Approximate Geometry o o * +* _ O _ * +* Copyright(C) 2008 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program 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 2 of the License, or * +* (at your option) any later version. * +* * +* This program 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 (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ + +#ifndef SHADERUTILS_H +#define SHADERUTILS_H + +// Standard headers +#include +#include + +#include + + +namespace ShaderUtils +{ + +const char *importShaders(const char *filename) +{ + FILE *fp; + char *text = NULL; + + fp = fopen(filename, "rt"); + + if (fp != NULL) + { + fseek(fp, 0, SEEK_END); + size_t count = ftell(fp); + fseek(fp, 0, 0); + + if (count > 0) + { + text = new char[count+1]; + count = fread(text, sizeof(char), count, fp); + text[count] = '\0'; + } + + fclose(fp); + } + + return text; +} + +// Compile shader and provide verbose output +void compileShader(GLuint shader) +{ + static char shlog[2048]; + GLint status; + int length; + + glCompileShader(shader); + glGetShaderiv(shader, GL_COMPILE_STATUS, &status); + if (status != GL_TRUE) + { + glGetShaderInfoLog(shader, 2048, &length, shlog); + std::cout << std::endl; + std::cout << shlog << std::endl; + } +// else +// std::cout << " OK!" << std::endl; +} + +// Link shader program and provide verbose output +void linkShaderProgram(GLuint program) +{ + static char proglog[2048]; + GLint status; + int length; + + glLinkProgram(program); + glGetProgramiv(program, GL_LINK_STATUS, &status); + if (status != GL_TRUE) + { + glGetProgramInfoLog(program, 2048, &length, proglog); + std::cout << std::endl; + std::cout << proglog << std::endl; + } + //else +// std::cout << " OK!" << std::endl; +} + +} /* end namespace */ + +#endif /* SHADERUTILS_H */ + diff --git a/src/plugins_unsupported/filter_mutualinfo/solver.cpp b/src/plugins_unsupported/filter_mutualinfo/solver.cpp new file mode 100644 index 000000000..440ef3536 --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/solver.cpp @@ -0,0 +1,717 @@ +#include +#include +#include "filter_mutualinfo.h" +#include +#include "../../external/newuoa/include/newuoa.h" + + +#include "solver.h" +#include "mutual.h" + +#include "pointCorrespondence.h" +//#include +#include "levmarmethods.h" + +using namespace std; +using namespace vcg; + +Solver::Solver(): + align(NULL), mutual(NULL), optimize_focal(true), + variance(2), tolerance(0.01), maxiter(600), mIweight(1) { + + start = 1e20; end = 0; + f_evals = 0; +} + +//function that evaluate error +// it is a weighted function. +// mIweight=1 consider only the MI evaluetion function; +// mIweight=0 consider only the error function through the correspondences; +double Solver::operator()(int ndim, double *x) { + f_evals++; + + //For test + f_evals_total++; + + for(int i = 0; i < ndim; i++) { + p[i] = x[i]; + //cout << p[i] << "\t"; + } + //cout << endl; + static int iter = 0; + iter++; +/* double orig = p.scale[6]; + //p.scale[6] *= pow(iter/(double)maxiter, 4); + double v = 4*(iter/(double)maxiter) - 2; + p.scale[6] *= v / sqrt(1 + v*v) + 1; + double forig = p.reference.Intrinsics.FocalMm; + p.reference.Intrinsics.FocalMm += p[6]*(orig - p.scale[6]); + cout << "v: " << v << " Scale: " << p.scale[6] << endl;*/ + + //cout << iter << " " << v / sqrt(1 + v*v) << endl; + + Shot shot = p.toShot(); + //p.scale[6] = orig; + //p.reference.Intrinsics.FocalMm = forig; + + align->shot = shot; + //align->renderScene(shot); + + int w = align->width(); + int h = align->height(); + + double info = 0; + +#define TEST +#ifndef TEST + switch(align->mode) { + + case AlignSet::NORMALMAP: + case AlignSet::COMBINE: + case AlignSet::SPECULAR: + align->readRender(1); + info += 2 - mutual->info(w, h, align->target, align->render); + case AlignSet::COLOR: + case AlignSet::SILHOUETTE: { + align->readRender(0); + info += 2 - mutual->info(w, h, align->target, align->render); + } + } +#else + int wstep = w;//w/4; + int hstep = h;//h/4; + + if (mIweight!=0){ + switch(align->mode) { + + case AlignSet::NORMALMAP: + case AlignSet::COMBINE: + case AlignSet::SPECULAR: + case AlignSet::SPECAMB: + align->renderScene(shot,1); + for(int i = 0; i < w; i+= wstep) { + int iend = i + wstep; + if(iend > w) iend = w; + for(int j =0; j < h; j+= hstep) { + int jend = j+hstep; + if(jend > h) jend = h; + + double m = 2 - mutual->info(w, h, align->target, align->render, i, iend, j, jend); + info += m; + } + } + case AlignSet::COLOR: + case AlignSet::SILHOUETTE: { + align->renderScene(shot,0); + for(int i = 0; i < w; i+= wstep) { + int iend = i + wstep; + if(iend > w) iend = w; + for(int j =0; j < h; j+= hstep) { + int jend = j+hstep; + if(jend > h) jend = h; + double m = 2 - mutual->info(w, h, align->target, align->render, i, iend, j, jend); + info += m; + } + } + } + } + } +#endif + + if(start == 0) start = info; + if(start == 1e20) + start = info; + end = info; + //return info; + + double k = mIweight; + double error =0; + if(align->correspList->size()>0) + error = calculateError2(shot); + align->error = error; + + double result = k*info + (1-k)*error; + + /*myfile << "error " << error <<"\n"; + myfile << "MI " << info <<"\n"; + myfile << "result " << result <<"\n \n";*/ + + myfile << f_evals_total <<" "; + myfile << (1-k)*error <<" "; + myfile << k*info <<" "; + myfile << result <<" "; + myfile << endl; + + return result; +} + +int Solver::optimize(AlignSet *_align, MutualInfo *_mutual, Shot &shot) { + align = _align; + mutual = _mutual; + + p = Parameters(optimize_focal, shot, align->width(), align->height(), + align->box, *align->mesh); + + f_evals = 0; + start = 1e20; + end = 0; + //TODO optimize fbo binding here + end = min_newuoa(p.size(), p.p, *this, + variance, tolerance, maxiter); + + align->shot = p.toShot(); + return f_evals; +} + +int Solver::iterative(AlignSet *_align, MutualInfo *_mutual, Shot &shot) { + //QString fileName = _align->projectName; + //fileName.append(".txt"); + ////myfile.open ("result.txt"); + //myfile.open (fileName.toLatin1()); + //assert(myfile.is_open()); + ////myfile << " k=" << mIweight << "\n"; + //assert(myfile.badbit); + + f_evals = 0; + f_evals_total =0; + double var = variance; + double tol = tolerance; + double niter = maxiter; + maxiter = niter/4; + variance = 6; + tolerance = variance/10; + while(f_evals < niter && variance > 0.1) { + f_evals += optimize(_align, _mutual, shot); + shot = _align->shot; + //cout << "Iter: " << f_evals << endl; + //get max p: + double max = 0; + for(int i = 0; i < p.size(); i++) + if(fabs(p[i]) > max) max = fabs(p[i]); + //cout << "Max: " << max << endl; + variance = max/8; + if(variance > 20) variance = 20; + tolerance = variance/10; + } + variance = var; + tolerance = tol; + maxiter = niter; + + //myfile.close(); + + return f_evals; +} + +//int Solver::convergence(AlignSet *_align, MutualInfo *_mutual, Shot &shot, QTextStream &stream) { +// p = Parameters(optimize_focal, shot, _align->width(), _align->height(), +// _align->box, _align->mesh); +// +// double mindist = 10; +// double maxdist = 60; +// int niter = 300; +// int seed = 0; +// int n = 6; +// if(p.use_focal) n = 7; +// Shot initial = shot; +// for(int i = 0; i < niter; i++) { +// srand(seed++); +// double len = mindist + i*(maxdist-mindist)/(niter); +// p.randomDir(n, p.p, len); +// +// double max_step = len; +// double initial_dist = p.pixelDiff(_align->mesh); +// Shot test = p.toShot(); +// int evals = iterative(_align, _mutual, test); +// double final_dist = p.pixelDiff(initial, _align->mesh); +// stream << initial_dist << " " << final_dist << " " << len << " " << p.norm() +// << " " << evals; +// +// for(int i = 0; i < n; i++) +// stream << " " << p[i]; +// stream << endl; +// p.reference = initial; +// } +// _align->shot = initial; +// return 0; +//} +// +//int Solver::graphs(AlignSet *_align, MutualInfo *_mutual, Shot &shot, QTextStream &stream) { +// align = _align; +// mutual = _mutual; +// +// p = Parameters(optimize_focal, shot, align->width(), align->height(), +// align->box, align->mesh); +// +// double maxdist = 10; +// int niter = 30; +// const int naxis = 30; +// double step = 2*maxdist/niter; +// //n directions of norm 1 +// int n = 6; +// if(p.use_focal) n = 7; +// double *dirs = new double[naxis * n]; +// +// for(int k =0 ; k < naxis; k++) { +// p.randomDir(n, &(dirs[k*n])); +// if(dirs[k*n+2] < 0) { +// for(int i = 0; i < n; i++) +// dirs[k*n+i] *= -1; +// } +// } +// for(double j = -maxdist; j <= maxdist; j += step) { +// stream << j << " "; +// for(int k = 0; k < naxis; k++) { +// +// for(int i = 0; i < n; i++) { +// p[i] = dirs[k*n+i]*j; +// } +// +// Shot shot = p.toShot(); +// +// align->shot = shot; +// align->renderScene(shot); +// +// int w = align->width(); +// int h = align->height(); +// +// double info = 0; +// +// switch(align->mode) { +// case AlignSet::NORMALMAP: +// case AlignSet::COMBINE: +// case AlignSet::SPECULAR: +// case AlignSet::SPECAMB: +// align->readRender(1); +// info += 2 - mutual->info(w, h, align->target, align->render); +// case AlignSet::COLOR: +// case AlignSet::SILHOUETTE: { +// align->readRender(0); +// info += 2 - mutual->info(w, h, align->target, align->render); +// } +// } +// +// //Per considerare k (il peso delle MI) +// double weight = mIweight; +// double error =0; +// if(align->correspList->size()>0 && weight!=1) +// error = calculateError2(shot); +// info = weight*info + (1-weight)*error; +// +// stream << info << " "; +// } +// stream << endl; +// for(int i = 0; i < n; i++) +// p[i] = 0; +// Shot shot = p.toShot(); +// align->shot = shot; +// align->renderScene(shot); +// //delete []dirs; +// } +// return 0; +//} + +//For Ponchio's Levmar +void Solver::value(double *_p, double *x, int m, int n, void *data) { + Solver &solver = *(Solver *)data; + solver.f_evals++; + Parameters &p = solver.p; + AlignSet *align = solver.align; + MutualInfo *mutual = solver.mutual; + + for(int i = 0; i < m; i++) { + //cout << _p[i] << " "; + p[i] = _p[i]; + } + //cout << endl; + + Shot shot = p.toShot(); + + align->shot = shot; + //align->renderScene(shot); + + int w = align->width(); + int h = align->height(); + + for(int i = 0; i < n; i++) x[i] = 0; + int blocks = (int)sqrt((double)n); + + int wstep = w/blocks+1; + int hstep = h/blocks+1; + + switch(align->mode) { + + case AlignSet::NORMALMAP: + case AlignSet::COMBINE: + case AlignSet::SPECULAR: + case AlignSet::SPECAMB: + align->renderScene(shot,1); + for(int i = 0; i < w; i+= wstep) { + int iend = i + wstep; + if(iend > w) iend = w; + for(int j =0; j < h; j+= hstep) { + int jend = j+hstep; + if(jend > h) jend = h; + double info = 2 - mutual->info(w, h, align->target, align->render, i, iend, j, jend); + x[j + 3*i] = info; + //cout << "info: " << info << endl; + } + } + case AlignSet::COLOR: + case AlignSet::SILHOUETTE: { + align->renderScene(shot,0); + for(int i = 0; i < w; i+= wstep) { + int iend = i + wstep; + if(iend > w) iend = w; + for(int j =0; j < h; j+= hstep) { + int jend = j+hstep; + if(jend > h) jend = h; + double info = 2 - mutual->info(w, h, align->target, align->render, i, iend, j, jend); + x[j + 3*i] += info; + } + } + } + } + double totinfo = 0; + for(int i = 0; i < n; i++) + totinfo += x[i]; + + double &start = solver.start; + double &end = solver.end; + if(start == 0) start = totinfo; + if(start == 1e20) start = totinfo; + end = totinfo; + +} + +int Solver::levmar(AlignSet *_align, MutualInfo *_mutual, Shot &shot) { + align = _align; + mutual = _mutual; + + p = Parameters(optimize_focal, shot, align->width(), align->height(), + align->box, *align->mesh); + + f_evals = 0; + start = 1e20; + end = 0; + + double *_p = new double[p.size()]; + for(int i = 0; i < p.size(); i++) _p[i] = p[i]; + int sides = 16; + double *x = new double[sides]; + for(int i = 0; i < sides; i++) x[i] = 0; + + opts[0] = 1; + opts[1] = 1e-8; + opts[2] = 1e-8; + opts[3] = 1e-8; + opts[4] = 1; + + //double opts[5]; //0 -> initial mu + //1 -> minimum JTe + //2 -> minimum Dp + //3 -> minimum sum(d*d) + //4 -> delta for finite differe + + int niter=0; + /*int niter = dlevmar_dif(Solver::value, _p, x, p.size(), sides, + maxiter, opts, info, + NULL, NULL, this); OKKIO */ + + //std::cout << "DEBUG: terminated because "; + switch((int)info[6]) { + case 1: std::cout << "stopped by small gradient J^T e\n"; break; + case 2: std::cout << "stopped by small Dp\n"; break; + case 3: std::cout << "stopped by itmax\n"; break; + case 4: std::cout << "singular matrix. Restart from current p with increased mu \n"; break; + case 5: std::cout << "no further error reduction is possible. Restart with increased mu\n"; break; + case 6: std::cout << "stopped by small ||e||_2 \n"; break; + } + //std::cout << "iterations: " << info[5] << " distseval: " << info[7] << " jaceval: " << info[8] << std::endl; + + for(int i = 0; i < p.size(); i++) p[i] = _p[i]; + align->shot = p.toShot(); + + delete []x; + delete _p; + + + return niter; +} + + +//bool Solver::tsai(AlignSet *_align, Shot &shot){ +// align = _align; +// QList *correspList = align->correspList; +// +// std::list *corrs = new std::list(); +// for( int i=0; icount(); i++){ +// PointCorrespondence *corr = correspList->at(i); +// PointOnLayer currentPointOnLayer1= corr->getPointAt(0); +// PointOnLayer currentPointOnLayer2= corr->getPointAt(1); +// PointType type1 = currentPointOnLayer1.getType(); +// TsaiCorrelation *corrTsai = new TsaiCorrelation(); +// if(type1==_3D_POINT){ +// vcg::Point3d currentPoint3d(currentPointOnLayer1.pX, currentPointOnLayer1.pY,currentPointOnLayer1.pZ ); +// corrTsai->point3d = currentPoint3d; +// vcg::Point2d currentPoint2d(currentPointOnLayer2.pX, currentPointOnLayer2.pY); +// corrTsai->point2d = currentPoint2d; +// } +// else{ +// vcg::Point3d currentPoint3d(currentPointOnLayer2.pX, currentPointOnLayer2.pY,currentPointOnLayer2.pZ ); +// corrTsai->point3d = currentPoint3d; +// vcg::Point2d currentPoint2d(currentPointOnLayer1.pX, currentPointOnLayer1.pY); +// corrTsai->point2d = currentPoint2d; +// } +// qDebug("Point3d %f %f %f",(float)corrTsai->point3d.X(),(float)corrTsai->point3d.Y(),(float)(float)corrTsai->point3d.Z()); +// qDebug("Point2d %f %f %f",(float)corrTsai->point2d.X(),(float)corrTsai->point2d.Y()); +// +// corrs->push_back(*corrTsai); +// } +// +// +// vcg::Camera &cam = shot.Intrinsics; +// +// //DEBUG +// qDebug("\n TEST BEFORE CALIBRATION \n"); +// qDebug("Focal %f",cam.FocalMm); +// qDebug("ViewportPx.X %i",cam.ViewportPx.X()); +// qDebug("ViewportPx.Y %i",cam.ViewportPx.Y()); +// qDebug("CenterPx.X %f",cam.CenterPx[0]); +// qDebug("CenterPx.Y %f",cam.CenterPx[1]); +// qDebug("DistorntedCenterPx.X %f",cam.DistorCenterPx[0]); +// qDebug("DistorntedCenterPx.Y %f",cam.DistorCenterPx[1]); +// qDebug("PixelSizeMm.X %f",cam.PixelSizeMm[0]); +// qDebug("PixelSizeMm.Y %f",cam.PixelSizeMm[1]); +// qDebug("k1 %f",cam.k[0]); +// qDebug("k2 %f",cam.k[1]); +// qDebug("Tra %f %f %f",shot.Extrinsics.Tra().X(),shot.Extrinsics.Tra().Y(),shot.Extrinsics.Tra().Z()); +// for(int i=0;i<4;i++) +// qDebug("Rot %f %f %f %f", (shot.Extrinsics.Rot())[i][0],(shot.Extrinsics.Rot())[i][1],(shot.Extrinsics.Rot())[i][2],(shot.Extrinsics.Rot())[i][3] ); +// +// //bool result = TsaiMethods::calibrate(&shot,corrs,optimize_focal); +// bool result= false; //OKKIO!!! +// qDebug("End calibration"); +// +// align->error = calculateError(corrs,shot); +// +// //DA SPEIGARE PERCHE' +// //cam.CenterPx[0] = cam.CenterPx[0] + (cam.CenterPx[0] - cam.DistorCenterPx[0]); +// //cam.CenterPx[1] = cam.CenterPx[1] + (cam.CenterPx[1] - cam.DistorCenterPx[1]); +// +// //TEMPORANEO +// //resetView(); +// +//// //PAOLO BRIVIO +//// Shot idShot; +//// idShot.Intrinsics = shot.Intrinsics; +//// idShot.Extrinsics.SetTra(Point3d(0,0,1000)); +//// //idShot = shot; +//// Shot2Track(shot,idShot,track); +//// //shot.Extrinsics.SetIdentity(); +//// shot = idShot; +// +//// //PAOLO BRIVIO +//// Shot idShot; +//// idShot.Intrinsics = shot.Intrinsics; +//// +//// Shot2Track(shot,previousShot,track); +//// shot = idShot; +// +// +// //DEBUG +// //vcg::Camera &cam = shot.Intrinsics; +// qDebug("\n TEST AFTER CALIBRATION \n"); +// qDebug("Focal %f",cam.FocalMm); +// qDebug("ViewportPx.X %i",cam.ViewportPx.X()); +// qDebug("ViewportPx.Y %i",cam.ViewportPx.Y()); +// qDebug("CenterPx.X %f",cam.CenterPx[0]); +// qDebug("CenterPx.Y %f",cam.CenterPx[1]); +// qDebug("DistortedCenterPx.X %f",cam.DistorCenterPx[0]); +// qDebug("DistortedCenterPx.Y %f",cam.DistorCenterPx[1]); +// qDebug("PixelSizeMm.X %f",cam.PixelSizeMm[0]); +// qDebug("PixelSizeMm.Y %f",cam.PixelSizeMm[1]); +// qDebug("k1 %f",cam.k[0]); +// qDebug("k2 %f",cam.k[1]); +// qDebug("Tra %f %f %f",shot.Extrinsics.Tra().X(),shot.Extrinsics.Tra().Y(),shot.Extrinsics.Tra().Z()); +// for(int i=0;i<4;i++) +// qDebug("Rot %f %f %f %f", (shot.Extrinsics.Rot())[i][0],(shot.Extrinsics.Rot())[i][1],(shot.Extrinsics.Rot())[i][2],(shot.Extrinsics.Rot())[i][3] ); +// +// align->shot=shot; +// return result; +//} + +template +double Solver::calculateError(std::list *corrs, Shot &shot){ + typename std::list::iterator it_c; + Correlation* c; + + double ratio = shot.Intrinsics.ViewportPx.X()/(double) shot.Intrinsics.ViewportPx.Y(); + vcg::Point3f *p1; + vcg::Point2d *p2; + int count=0; + + double error = 0; +// OKKIO!! + // for ( it_c= corrs->begin() ; it_c !=corrs->end(); it_c++ ){ + // c=&*it_c; + // p1=&(c->point3d); + // p2=&(c->point2d); + + // if(p1!=NULL && p2!=NULL) + // { + // //Adjust 2D point + // vcg::Point2d p2adj(((p2->X()/ratio) +1)/2.0 * shot.Intrinsics.CenterPx.X()*2.0,((p2->Y())+1)/2.0 * shot.Intrinsics.CenterPx.Y()*2.0); + // //Project 3D point + // vcg::Point2f p1proj = shot.Project(*p1); + // //Calculate disntance + // float dist = vcg::Distance(p1proj,p2adj); + // error += dist; + // count++; + // } + //}//all corrs + + ////Normalize error + return error /= count; +} + +double Solver::calculateError2( Shot &shot){ + //E' una pezza, andrebbe meglio pensato. Va a beccare direttamente le strutture dati PointCorrespondence di base. + //align già è sicuramente settato perchè lo chiami da optimize (poi dovrai distinguere le due cose, p.e. fare un optimize2) + QList *correspList = align->correspList; + double error = 0; + int count=0; + + for( int i=0; icount(); i++){ + PointCorrespondence *corr = correspList->at(i); + PointOnLayer currentPointOnLayer1= corr->getPointAt(0); + PointOnLayer currentPointOnLayer2= corr->getPointAt(1); + PointType type1 = currentPointOnLayer1.getType(); + vcg::Point3f currentPoint1(currentPointOnLayer1.pX, currentPointOnLayer1.pY,currentPointOnLayer1.pZ ); + vcg::Point3f currentPoint2(currentPointOnLayer2.pX, currentPointOnLayer2.pY,currentPointOnLayer2.pZ ); + vcg::Point2f p2adj; + vcg::Point2f p1proj; + if(type1==_3D_POINT){ + //Adjust 2D point + p2adj = vcg::Point2f(((currentPoint2.X()/align->imageRatio) +1)/2.0 * shot.Intrinsics.CenterPx.X()*2.0,((currentPoint2.Y())+1)/2.0 * shot.Intrinsics.CenterPx.Y()*2.0); + //Project 3D point + p1proj = shot.Project(currentPoint1); + } + else{ + //Adjust 2D point + p2adj = vcg::Point2f(((currentPoint1.X()/align->imageRatio) +1)/2.0 * shot.Intrinsics.CenterPx.X()*2.0,((currentPoint1.Y())+1)/2.0 * shot.Intrinsics.CenterPx.Y()*2.0); + //Project 3D point + p1proj = shot.Project(currentPoint2); + } + + float dist = vcg::Distance(p1proj,p2adj); + error += dist; + count++; + } + + //Normalize error + return error /= count; + +} + +bool Solver::levmar(AlignSet *_align, Shot &shot){ + align = _align; + QList *correspList = align->correspList; + + std::list *corrs = new std::list(); + for( int i=0; icount(); i++){ + PointCorrespondence *corr = correspList->at(i); + PointOnLayer currentPointOnLayer1= corr->getPointAt(0); + PointOnLayer currentPointOnLayer2= corr->getPointAt(1); + PointType type1 = currentPointOnLayer1.getType(); + LevmarCorrelation *corrLevmar = new LevmarCorrelation(); + if(type1==_3D_POINT){ + vcg::Point3f currentPoint3d(currentPointOnLayer1.pX, currentPointOnLayer1.pY,currentPointOnLayer1.pZ ); + corrLevmar->point3d = currentPoint3d; + vcg::Point2d currentPoint2d(currentPointOnLayer2.pX, currentPointOnLayer2.pY); + corrLevmar->point2d = currentPoint2d; + } + else{ + vcg::Point3f currentPoint3d(currentPointOnLayer2.pX, currentPointOnLayer2.pY,currentPointOnLayer2.pZ ); + corrLevmar->point3d = currentPoint3d; + vcg::Point2d currentPoint2d(currentPointOnLayer1.pX, currentPointOnLayer1.pY); + corrLevmar->point2d = currentPoint2d; + } + qDebug("Point3d %f %f %f",(float)corrLevmar->point3d.X(),(float)corrLevmar->point3d.Y(),(float)(float)corrLevmar->point3d.Z()); + qDebug("Point2d %f %f %f",(float)corrLevmar->point2d.X(),(float)corrLevmar->point2d.Y()); + + corrs->push_back(*corrLevmar); + } + + vcg::Camera &cam = shot.Intrinsics; + + //DEBUG + qDebug("\n TEST BEFORE CALIBRATION \n"); + qDebug("Focal %f",cam.FocalMm); + qDebug("ViewportPx.X %i",cam.ViewportPx.X()); + qDebug("ViewportPx.Y %i",cam.ViewportPx.Y()); + qDebug("CenterPx.X %f",cam.CenterPx[0]); + qDebug("CenterPx.Y %f",cam.CenterPx[1]); + qDebug("DistorntedCenterPx.X %f",cam.DistorCenterPx[0]); + qDebug("DistorntedCenterPx.Y %f",cam.DistorCenterPx[1]); + qDebug("PixelSizeMm.X %f",cam.PixelSizeMm[0]); + qDebug("PixelSizeMm.Y %f",cam.PixelSizeMm[1]); + qDebug("k1 %f",cam.k[0]); + qDebug("k2 %f",cam.k[1]); + qDebug("Tra %f %f %f",shot.Extrinsics.Tra().X(),shot.Extrinsics.Tra().Y(),shot.Extrinsics.Tra().Z()); + for(int i=0;i<4;i++) + qDebug("Rot %f %f %f %f", (shot.Extrinsics.Rot())[i][0],(shot.Extrinsics.Rot())[i][1],(shot.Extrinsics.Rot())[i][2],(shot.Extrinsics.Rot())[i][3] ); + + Shot previousShot = shot; + //First calculate only extrinsics + bool result = LevmarMethods::calibrate(&shot,corrs,false); + //If user wants calibrate the focal (these have to be two different steps) + if(optimize_focal) + result = LevmarMethods::calibrate(&shot,corrs,optimize_focal); + + qDebug("End calibration"); + + align->error = calculateError(corrs,shot); + + //DA SPEIGARE PERCHE' + //cam.CenterPx[0] = cam.CenterPx[0] + (cam.CenterPx[0] - cam.DistorCenterPx[0]); + //cam.CenterPx[1] = cam.CenterPx[1] + (cam.CenterPx[1] - cam.DistorCenterPx[1]); + + // //PAOLO BRIVIO + // Shot idShot; + // idShot.Intrinsics = shot.Intrinsics; + // idShot.Extrinsics.SetTra(Point3d(0,0,1000)); + // //idShot = shot; + // Shot2Track(shot,idShot,track); + // //shot.Extrinsics.SetIdentity(); + // shot = idShot; + + // //PAOLO BRIVIO + // Shot idShot; + // idShot.Intrinsics = shot.Intrinsics; + // + // Shot2Track(shot,previousShot,track); + // shot = idShot; + + + //DEBUG + //vcg::Camera &cam = shot.Intrinsics; + qDebug("\n TEST AFTER CALIBRATION \n"); + qDebug("Focal %f",cam.FocalMm); + qDebug("ViewportPx.X %i",cam.ViewportPx.X()); + qDebug("ViewportPx.Y %i",cam.ViewportPx.Y()); + qDebug("CenterPx.X %f",cam.CenterPx[0]); + qDebug("CenterPx.Y %f",cam.CenterPx[1]); + qDebug("DistortedCenterPx.X %f",cam.DistorCenterPx[0]); + qDebug("DistortedCenterPx.Y %f",cam.DistorCenterPx[1]); + qDebug("PixelSizeMm.X %f",cam.PixelSizeMm[0]); + qDebug("PixelSizeMm.Y %f",cam.PixelSizeMm[1]); + qDebug("k1 %f",cam.k[0]); + qDebug("k2 %f",cam.k[1]); + qDebug("Tra %f %f %f",shot.Extrinsics.Tra().X(),shot.Extrinsics.Tra().Y(),shot.Extrinsics.Tra().Z()); + for(int i=0;i<4;i++) + qDebug("Rot %f %f %f %f", (shot.Extrinsics.Rot())[i][0],(shot.Extrinsics.Rot())[i][1],(shot.Extrinsics.Rot())[i][2],(shot.Extrinsics.Rot())[i][3] ); + + align->shot=shot; + return result; +} + diff --git a/src/plugins_unsupported/filter_mutualinfo/solver.h b/src/plugins_unsupported/filter_mutualinfo/solver.h new file mode 100644 index 000000000..c45992b2f --- /dev/null +++ b/src/plugins_unsupported/filter_mutualinfo/solver.h @@ -0,0 +1,77 @@ +#ifndef SOLVER_H +#define SOLVER_H + +#include +#include "alignset.h" + +#include "parameters.h" +#include "../../external/levmar-2.3/lm.h" + +#include +#include + +//These values changed from levmar definitions +#define LM_INIT_MU 1E-03 +#define LM_STOP_THRESH 1E-17 +#define LM_CONV_THRESH 1E-05 +#define LM_DIFF_DELTA 1E-06 +#define LM_MAX_ITER 100 + + +class AlignSet; +class MutualInfo; + +class Solver { + typedef vcg::Shot Shot; + typedef vcg::Box3 Box; + + public: + AlignSet *align; + MutualInfo *mutual; + Parameters p; + + bool optimize_focal; //if true also focal value is optimized + bool fine_alignment; //if true also focal value is optimized + + double variance; // 1/10th of the expected variance in the parameters + double tolerance; // newuoa terminates when minimum is closest then tolerance + int maxiter; // max number of function evaluations in NEWUOA + + double mIweight; //need to weight the MI function and the error function of the correspondences + + double start, end; //starting and ending value of the function + int f_evals; //number of function evaluations + int f_evals_total; //total number of function evaluations (for test) + + std::ofstream myfile; //for debugging + + Solver(); + double operator()(int ndim, double *x); + int optimize(AlignSet *align, MutualInfo *mutual, Shot &shot); + int iterative(AlignSet *align, MutualInfo *mutual, Shot &shot); + //int globalConvergence(AlignSet *align, MutualInfo *mutual, Shot &shot); + //int convergence(AlignSet *align, MutualInfo *mutual, Shot &shot, QTextStream &stream); + //int graphs(AlignSet *align, MutualInfo *mutual, Shot &shot, QTextStream &stream); + + static void value(double *p, double *x, int m, int n, void *data); + int levmar(AlignSet *align, MutualInfo *mutual, Shot &shot); //never used + + bool tsai(AlignSet *align, Shot &shot); + bool levmar(AlignSet *align,Shot &shot); + + private: + double opts[5]; //0 -> initial mu + //1 -> minimum JTe + //2 -> minimum Dp + //3 -> minimum sum(d*d) + //4 -> delta for finite differe + double info[LM_INFO_SZ]; + + template + double calculateError(std::list *corrs, Shot &shot); + + double calculateError2( Shot &shot); + +}; + +#endif