[GRASS-SVN] r44652 -
grass/branches/develbranch_6/imagery/i.ortho.photo/libes
svn_grass at osgeo.org
svn_grass at osgeo.org
Wed Dec 22 04:38:28 EST 2010
Author: mmetz
Date: 2010-12-22 01:38:28 -0800 (Wed, 22 Dec 2010)
New Revision: 44652
Modified:
grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c
Log:
fix handling of pointers to control point coordinates; more robust initial camera position estimation
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-22 00:06:51 UTC (rev 44651)
+++ grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c 2010-12-22 09:38:28 UTC (rev 44652)
@@ -199,22 +199,37 @@
/* set initial XC, YC */
if (first) {
- X = *((cpz->e2)++);
- x = *((cpz->e1)++);
- Y = *((cpz->n2)++);
- y = *((cpz->n1)++);
- (cpz->z2)++;
+ X = cpz->e2[i];
+ x = cpz->e1[i];
+ Y = cpz->n2[i];
+ y = cpz->n1[i];
first = 0;
}
else {
- n++;
- meanx += *((cpz->e1)++);
- meany += *((cpz->n1)++);
- meanX += *((cpz->e2)++);
- meanY += *((cpz->n2)++);
- meanZ += *((cpz->z2)++);
+ /* set initial ZC from: */
+ /* scale ~= dist_photo/dist_grnd ~= (CFL)/(Z - ZC) */
+ /* ZC ~= Z + CFL(dist_grnd)/(dist_photo) */
+ dx = cpz->e1[i] - x;
+ dy = cpz->n1[i] - y;
+ dist_photo = sqrt(dx * dx + dy * dy);
+ dx = cpz->e2[i] - X;
+ dy = cpz->n2[i] - Y;
+ dist_grnd = sqrt(dx * dx + dy * dy);
+
+ if (dist_photo != 0 && dist_grnd != 0) {
+ meanZ += cpz->z2[i] + (CFL * (dist_grnd) / (dist_photo));
+ meanx += cpz->e1[i];
+ meany += cpz->n1[i];
+ meanX += cpz->e2[i];
+ meanY += cpz->n2[i];
+
+ n++;
+ }
}
}
+ if (!n) /* Poorly placed Control Points */
+ return -1;
+
meanx /= n;
meany /= n;
meanX /= n;
@@ -222,33 +237,8 @@
meanZ /= n;
*XC = meanX;
*YC = meanY;
+ *ZC = meanZ;
- /* reset pointers */
- for (i = 0; i < cpz->count; i++) {
- if (cpz->status[i] <= 0)
- continue;
- (cpz->e1)--;
- (cpz->e2)--;
- (cpz->n1)--;
- (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;
@@ -272,8 +262,9 @@
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);
+ G_debug(1, "INITIAL CAMERA POSITION:");
+ G_debug(1, "XC: %.2f, YC: %.2f, ZC: %.2f", *XC, *YC, *ZC);
+ G_debug(1, "Omega %.2f, Phi %.2f, Kappa: %.2f", *Omega, *Phi, *Kappa);
#ifdef DEBUG
fprintf(debug, "\nINITIAL CAMERA POSITION:\n");
@@ -363,29 +354,28 @@
fprintf(debug, "\n\t\t\tIn Summation count = %d \n", i);
#endif
- x = *((cpz->e1)++);
- y = *((cpz->n1)++);
- z = *((cpz->z1)++);
- X = *((cpz->e2)++);
- Y = *((cpz->n2)++);
- Z = *((cpz->z2)++);
-
if (cpz->status[i] <= 0)
continue;
+ x = cpz->e1[i];
+ y = cpz->n1[i];
+ z = cpz->z1[i];
+ X = cpz->e2[i];
+ Y = cpz->n2[i];
+ Z = cpz->z2[i];
+
#ifdef DEBUG
fprintf(debug,
"\n\t\t\timage:\n \t\t\tx = \t%f \n\t\t\ty = \t%f \n\t\t\tz = \t%f \n\t\t\tobject:\n \t\t\tX = \t%f \n\t\t\tY = \t%f \n\t\t\tZ = \t%f \n",
x, y, z, X, Y, Z);
#endif
-
/* Compute Obj. Space coordinates */
XYZ.x[0][0] = X - *XC;
XYZ.x[1][0] = Y - *YC;
XYZ.x[2][0] = Z - *ZC;
- /* just an abbreviations */
+ /* just an abbreviation */
lam = XYZ.x[0][0];
mu = XYZ.x[1][0];
nu = XYZ.x[2][0];
@@ -456,16 +446,6 @@
m_add(&CC, &C, &CC);
} /* end summation loop over all active control points */
- /* reset pointers */
- for (i = 0; i < cpz->count; i++) {
- (cpz->e1)--;
- (cpz->n1)--;
- (cpz->z1)--;
- (cpz->e2)--;
- (cpz->n2)--;
- (cpz->z2)--;
- }
-
#ifdef DEBUG
fprintf(debug, "\n\tN: \n");
for (i = 0; i < 6; i++)
@@ -512,12 +492,13 @@
*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);
+ G_debug(1, "FINAL CAMERA POSITION:");
+ G_debug(1, "XC: %.2f, YC: %.2f, ZC: %.2f", *XC, *YC, *ZC);
+ G_debug(1, "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); */
+ /* Compute Orientation Matrix from Omega, Phi, Kappa */
sw = sin(*Omega);
cw = cos(*Omega);
sp = sin(*Phi);
@@ -539,13 +520,13 @@
MO->x[2][1] = -(sw * cp);
MO->x[2][2] = cw * cp;
- /* Compute Transposed Orientation Matrix (Omega, Phi, Kappa); */
+ /* Compute Transposed Orientation Matrix from Omega, Phi, Kappa */
MT->nrows = 3;
MT->ncols = 3;
zero(MT);
- /* M Transposed */
+ /* Transposed Matrix */
MT->x[0][0] = cp * ck;
MT->x[1][0] = cw * sk + (sw * sp * ck);
MT->x[2][0] = sw * sk - (cw * sp * ck);
@@ -585,12 +566,12 @@
zero(&UVW);
/************************ Start the work ******************************/
- /* set Xp, Yp, and CFL from cam_info */
+ /* Set Xp, Yp, and CFL from cam_info */
Xp = cam_info->Xp;
Yp = cam_info->Yp;
CFL = cam_info->CFL;
- /* ObjSpace (&XYZ, XC,YC,ZC, X,Y,Z); */
+ /* Object Space (&XYZ, XC,YC,ZC, X,Y,Z); */
XYZ.x[0][0] = e1 - XC;
XYZ.x[1][0] = n1 - YC;
XYZ.x[2][0] = z1 - ZC;
@@ -631,19 +612,19 @@
zero(&UVW);
/********************** Start the work ********************************/
- /* set Xp, Yp, and CFL from cam_info */
+ /* Set Xp, Yp, and CFL from cam_info */
Xp = cam_info->Xp;
Yp = cam_info->Yp;
CFL = cam_info->CFL;
- /* ImageSpace */
+ /* Image Space */
UVW.x[0][0] = e1 - Xp;
UVW.x[1][0] = n1 - Yp;
UVW.x[2][0] = -CFL;
m_mult(&M, &UVW, &XYZ);
- /* Image Space */
+ /* Object Space */
lam = XYZ.x[0][0];
mu = XYZ.x[1][0];
nu = XYZ.x[2][0];
More information about the grass-commit
mailing list