[GRASS-SVN] r44606 -
grass/branches/develbranch_6/imagery/i.ortho.photo/libes
svn_grass at osgeo.org
svn_grass at osgeo.org
Wed Dec 15 09:21:20 EST 2010
Author: mmetz
Date: 2010-12-15 06:21:20 -0800 (Wed, 15 Dec 2010)
New Revision: 44606
Modified:
grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c
Log:
fix calculation of initial camera parameters
Modified: grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c
===================================================================
--- grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c 2010-12-14 11:22:29 UTC (rev 44605)
+++ grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c 2010-12-15 14:21:20 UTC (rev 44606)
@@ -30,8 +30,9 @@
#include <signal.h>
#include <stdio.h>
#include <math.h>
-#include "orthophoto.h"
#include <grass/imagery.h>
+#include <grass/glocale.h>
+#include "orthophoto.h"
#include "matrixdefs.h"
#include "local_proto.h"
@@ -54,9 +55,6 @@
MATRIX *MO, MATRIX *MT)
{
MATRIX delta, epsilon, B, BT, C, E, N, CC, NN, UVW, XYZ, M, WT1;
- double meanx, meany;
- double X1 = 0, X2 = 0, x1 = 0, x2 = 0, Z1 = 0, Y1 = 0, Y2 = 0, y1 =
- 0, y2 = 0, dist_grnd = 0, dist_photo = 0;
double x, y, z, X, Y, Z, Xp, Yp, CFL;
double lam, mu, nu, U, V, W;
double xbar, ybar;
@@ -187,14 +185,13 @@
WT1.x[5][5] = (Q1 / (init_info->kappa_var * init_info->kappa_var));
}
else { /* set from mean values of active control points */
- double meanX, meanY, meanZ;
- int nz;
-
+ double dist_grnd, dist_photo;
+ double meanx, meany, meanX, meanY, meanZ;
+
/* set intial XC and YC from mean values of control points */
- meanx = meany = 0;
- meanX = meanY = meanZ = 0;
- *ZC = 0;
- n = nz = 0;
+ meanx = meany = meanX = meanY = meanZ = 0;
+ x = y = X = Y = 0;
+ n = 0;
first = 1;
for (i = 0; i < cpz->count; i++) {
if (cpz->status[i] <= 0)
@@ -202,49 +199,29 @@
/* set initial XC, YC */
if (first) {
- X1 = *(cpz->e2);
- x1 = *(cpz->e1);
- Y1 = *(cpz->n2);
- y1 = *(cpz->n1);
- Z1 = *(cpz->z2);
+ X = *((cpz->e2)++);
+ x = *((cpz->e1)++);
+ Y = *((cpz->n2)++);
+ y = *((cpz->n1)++);
+ (cpz->z2)++;
first = 0;
}
- if (!first) {
- X2 = *(cpz->e2);
- x2 = *(cpz->e1);
- Y2 = *(cpz->n2);
- y2 = *(cpz->n1);
- Z1 = *(cpz->z2);
- dist_photo =
- sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
- dist_grnd =
- sqrt(((X1 - X2) * (X1 - X2)) + ((Y1 - Y2) * (Y1 - Y2)));
-
- /* set initial ZC from: */
- /* scale ~= dist_photo/dist_grnd ~= (CFL)/(Z1 - Zc) */
- /* Zc ~= Z1 + CFL(dist_grnd)/(dist_photo) */
-
- if (dist_grnd != 0 && dist_photo != 0) {
- meanZ += Z1 + (CFL * (dist_grnd) / (dist_photo));
- nz++;
- }
+ else {
+ n++;
+ meanx += *((cpz->e1)++);
+ meany += *((cpz->n1)++);
+ meanX += *((cpz->e2)++);
+ meanY += *((cpz->n2)++);
+ meanZ += *((cpz->z2)++);
}
-
- n++;
- meanx += *((cpz->e1)++);
- meany += *((cpz->n1)++);
- meanX += *((cpz->e2)++);
- meanY += *((cpz->n2)++);
- (cpz->z2)++;
}
- *XC = meanX / n;
- *YC = meanY / n;
- *ZC = meanZ / (nz - 1);
- G_debug(2, "XC: %.2f, YC: %.2f, ZC: %.2f", *XC, *YC, *ZC);
- meanx = (meanx - x1) / (n - 1);
- meany = (meany - y1) / (n - 1);
- meanX = (meanX - X1) / (n - 1);
- meanY = (meany - Y1) / (n - 1);
+ meanx /= n;
+ meany /= n;
+ meanX /= n;
+ meanY /= n;
+ meanZ /= n;
+ *XC = meanX;
+ *YC = meanY;
/* reset pointers */
for (i = 0; i < cpz->count; i++) {
@@ -256,17 +233,28 @@
(cpz->n2)--;
(cpz->z2)--;
}
+
+ /* initial ZC and Kappa must be derived from the same two distinct points
+ * first control point: x1, y1, X1, Y1
+ * mean values (or last control point: x2, y2, X2, Y2)
+ */
+ /* set initial ZC from: */
+ /* scale ~= dist_photo/dist_grnd ~= (CFL)/(meanZ - ZC) */
+ /* ZC ~= meanZ + CFL(dist_grnd)/(dist_photo) */
+ dist_photo =
+ sqrt(((meanx - x) * (meanx - x)) + ((meany - y) * (meany - y)));
+ dist_grnd =
+ sqrt(((meanX - X) * (meanX - X)) + ((meanY - Y) * (meanY - Y)));
+
+ *ZC = meanZ + (CFL * (dist_grnd) / (dist_photo));
+
/* set initial rotations to zero (radians) */
*Omega = *Phi = 0.0;
/* find an initial kappa */
- /*
- kappa1 = atan2((y2 - y1), (x2 - x1));
- kappa2 = atan2((Y2 - Y1), (X2 - X1));
- */
- kappa1 = atan2((meany - y1), (meanx - x1));
- kappa2 = atan2((meanY - Y1), (meanX - X1));
+ kappa1 = atan2((meany - y), (meanx - x));
+ kappa2 = atan2((meanY - Y), (meanX - X));
*Kappa = (kappa2 - kappa1);
/* set initial weights */
@@ -284,6 +272,8 @@
WT1.x[4][4] = (Q1 / (phi_var * phi_var));
WT1.x[5][5] = (Q1 / (kappa_var * kappa_var));
}
+ G_debug(1, "Initial XC: %.2f, YC: %.2f, ZC: %.2f", *XC, *YC, *ZC);
+ G_debug(1, "Initial Omega %.2f, Phi %.2f, Kappa: %.2f", *Omega, *Phi, *Kappa);
#ifdef DEBUG
fprintf(debug, "\nINITIAL CAMERA POSITION:\n");
@@ -522,6 +512,11 @@
*Phi = epsilon.x[4][0];
*Kappa = epsilon.x[5][0];
+ G_debug(1, "Final XC: %.2f, YC: %.2f, ZC: %.2f", *XC, *YC, *ZC);
+ G_debug(1, "Final Omega %.2f, Phi %.2f, Kappa: %.2f", *Omega, *Phi, *Kappa);
+ if (*ZC < 0)
+ G_warning(_("Potential BUG in ortholib: camera altitude < 0"));
+
/* Compute Orientation Matrix M from (Omega, Phi, Kappa); */
sw = sin(*Omega);
cw = cos(*Omega);
More information about the grass-commit
mailing list