124 lines
4.3 KiB
C++
124 lines
4.3 KiB
C++
|
|
|
|
/*----------------------------------------------------------------------
|
|
* Predice la posizione corrente dei punti-immagine di un oggetto
|
|
* tridimensionale piano, nota la posizione iniziale dei punti-immagine
|
|
* e la posizione corrente di un sottoinsieme dei punti-immagine.
|
|
*
|
|
* DESCRIZIONE DEL PROBLEMA
|
|
* Sono dati k punti bidimensionali che rappresentano la proiezione
|
|
* sul piano-immagine di k punti tridimensionali appartenenti a uno
|
|
* stesso oggetto; i punti tridimensionali sono complanari, e le loro
|
|
* coordinate 3D NON sono note.
|
|
* Successivamente, l'oggetto viene sottoposto a rotazioni e traslazioni,
|
|
* e possibilmente l'immagine subisce uno zoom.
|
|
* A seguito di questa trasformazione, viene rilevata la posizione sul
|
|
* piano-immagine di un sottoinsieme dei k punti iniziali.
|
|
* Con questi dati, si vuole predire la posizione corrente sul
|
|
* piano-immagine dei rimanenti punti.
|
|
* L'algoritmo richiede le seguenti precondizioni:
|
|
* - i punti tridimensionali giacciono sullo stesso piano,
|
|
* non necessariamente parallelo al piano-immagine
|
|
* - il piano dei punti tridimensionali non degenera in una
|
|
* retta se osservato dal punto di vista della camera
|
|
* - i punti di cui e' nota la posizione corrente sono almeno 4
|
|
* - i punti di cui e' nota la posizione corrente non sono
|
|
* allineati, ovvero non giacciono su un'unica retta
|
|
---------------------------------------------------------------------*/
|
|
#include <memory>
|
|
|
|
#include "predict3d.h"
|
|
#include "metnum.h"
|
|
|
|
using namespace Predict3D;
|
|
using namespace MetNum;
|
|
|
|
/*----------------------------------------------------------------------
|
|
* Calcola la posizione dei punti non visibili data la posizione
|
|
* iniziale di tutti i punti e la posizione corrente dei punti visibili.
|
|
* PARAMETRI DI INGRESSO
|
|
* k numero di punti
|
|
* initial posizione iniziale dei punti nell'immagine
|
|
* current posizione corrente dei punti visibili;
|
|
* il punto current[i] corrisponde al punto initial[i]
|
|
* visible indicatore di visibiltia'; se visible[i] e' true,
|
|
* il punto i e' visibile e current[i] ne indica
|
|
* la posizione corrente; altrimenti il valore di
|
|
* current[i] non e' definito
|
|
* PARAMETRI DI USCITA
|
|
* current posizione corrente di tutti i punti
|
|
* VALORE DI RITORNO
|
|
* true la funzione non ha incontrato problemi
|
|
* false si e' verificato un errore; le cause di errore
|
|
* possibili sono:
|
|
* - k < 4
|
|
* - il numero di punti visibili e' < 4
|
|
* - i punti visibili sono allineati
|
|
----------------------------------------------------------------------------*/
|
|
bool Predict3D::Predict(int k, Point initial[], Point current[],
|
|
bool visible[]) {
|
|
/* Definizione e allocazione delle variabili */
|
|
int n, m, kvis;
|
|
double **x;
|
|
int i, ii, j, l;
|
|
kvis = 0;
|
|
for (i = 0; i < k; i++)
|
|
if (visible[i]) kvis++;
|
|
if (kvis < 4) return false;
|
|
n = 3 * kvis + 1;
|
|
m = kvis + 9;
|
|
|
|
x = AllocMatrix(m, n);
|
|
if (x == 0) return false;
|
|
std::unique_ptr<double[]> y(new double[n]);
|
|
std::unique_ptr<double[]> c(new double[m]);
|
|
|
|
/* Costruzione dei coefficienti del sistema */
|
|
|
|
for (j = 0; j < n - 1; j++) y[j] = 0.0;
|
|
y[n - 1] = 1.0;
|
|
|
|
for (l = 0; l < m; l++)
|
|
for (j = 0; j < n; j++) x[l][j] = 0.0;
|
|
|
|
for (l = 0; l < m; l++) c[l] = 0.0;
|
|
|
|
ii = 0;
|
|
for (i = 0; i < k; i++) {
|
|
if (!visible[i]) continue;
|
|
j = 3 * ii;
|
|
x[0][j] = x[3][j + 1] = x[6][j + 2] = -initial[i].x;
|
|
x[1][j] = x[4][j + 1] = x[7][j + 2] = -initial[i].y;
|
|
x[2][j] = x[5][j + 1] = x[8][j + 2] = -1.0;
|
|
l = ii + 9;
|
|
x[l][j] = current[i].x;
|
|
x[l][j + 1] = current[i].y;
|
|
x[l][j + 2] = 1.0;
|
|
ii++;
|
|
}
|
|
x[9][n - 1] = 1.0;
|
|
|
|
/* Soluzione del sistema */
|
|
|
|
int status = Approx(n, m, x, y.get(), c.get());
|
|
if (status != 0) {
|
|
FreeMatrix(m, x);
|
|
return false;
|
|
}
|
|
|
|
/* Applicazione della soluzione */
|
|
for (i = 0; i < k; i++) {
|
|
if (visible[i]) continue;
|
|
double x = initial[i].x;
|
|
double y = initial[i].y;
|
|
double uw1 = x * c[0] + y * c[1] + c[2];
|
|
double vw1 = x * c[3] + y * c[4] + c[5];
|
|
double w1 = x * c[6] + y * c[7] + c[8];
|
|
current[i].x = uw1 / w1;
|
|
current[i].y = vw1 / w1;
|
|
}
|
|
|
|
FreeMatrix(m, x);
|
|
|
|
return true;
|
|
}
|