[GRASS-SVN] r44472 -
grass/branches/develbranch_6/imagery/i.ortho.photo/libes
svn_grass at osgeo.org
svn_grass at osgeo.org
Mon Nov 29 03:05:42 EST 2010
Author: mmetz
Date: 2010-11-29 00:05:42 -0800 (Mon, 29 Nov 2010)
New Revision: 44472
Modified:
grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_cameras.c
grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_elev.c
grass/branches/develbranch_6/imagery/i.ortho.photo/libes/m_mult.c
grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthophoto.h
grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c
Log:
ortholib: optimize, simplify, fix bugs
Modified: grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_cameras.c
===================================================================
--- grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_cameras.c 2010-11-28 22:22:49 UTC (rev 44471)
+++ grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_cameras.c 2010-11-29 08:05:42 UTC (rev 44472)
@@ -36,7 +36,7 @@
strcat(buf, ";ls");
if (!full)
strcat(buf, " -C");
- if (ls = popen(buf, "r")) {
+ if ((ls = popen(buf, "r"))) {
while (G_getl(buf, sizeof buf, ls)) {
any = 1;
fprintf(temp, "%s", buf);
Modified: grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_elev.c
===================================================================
--- grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_elev.c 2010-11-28 22:22:49 UTC (rev 44471)
+++ grass/branches/develbranch_6/imagery/i.ortho.photo/libes/ls_elev.c 2010-11-29 08:05:42 UTC (rev 44472)
@@ -33,7 +33,7 @@
G__file_name(buf + strlen(buf), element, " ", " ");
strcat(buf, ";ls");
strcat(buf, " -C");
- if (ls = popen(buf, "r")) {
+ if ((ls = popen(buf, "r"))) {
while (G_getl(buf, sizeof buf, ls)) {
any = 1;
fprintf(temp, "%s", buf);
Modified: grass/branches/develbranch_6/imagery/i.ortho.photo/libes/m_mult.c
===================================================================
--- grass/branches/develbranch_6/imagery/i.ortho.photo/libes/m_mult.c 2010-11-28 22:22:49 UTC (rev 44471)
+++ grass/branches/develbranch_6/imagery/i.ortho.photo/libes/m_mult.c 2010-11-29 08:05:42 UTC (rev 44472)
@@ -11,7 +11,6 @@
{
register int i, j, k, nr, nc, ncols;
char message[256];
- static MATRIX m;
if (a->nrows == 0)
return error("*: arg1 not defined\n");
@@ -31,13 +30,12 @@
nc = b->ncols;
for (i = 0; i < nr; i++)
for (j = 0; j < nc; j++) {
- m.x[i][j] = 0.0;
+ c->x[i][j] = 0.0;
for (k = 0; k < ncols; k++)
- m.x[i][j] += (a->x[i][k] * b->x[k][j]);
+ c->x[i][j] += (a->x[i][k] * b->x[k][j]);
}
- m.nrows = nr;
- m.ncols = nc;
- m_copy(c, &m);
+ c->nrows = nr;
+ c->ncols = nc;
return 1;
}
Modified: grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthophoto.h
===================================================================
--- grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthophoto.h 2010-11-28 22:22:49 UTC (rev 44471)
+++ grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthophoto.h 2010-11-29 08:05:42 UTC (rev 44472)
@@ -1,5 +1,6 @@
#include <grass/gis.h>
#include <grass/imagery.h>
+#include "mat.h"
/* #define DEBUG 1 */
@@ -57,6 +58,8 @@
int *status;
};
+/* Ortho_Control_Points is identical to Ortho_Photo_Points
+ * Why ? */
struct Ortho_Control_Points
{
int count;
@@ -104,6 +107,7 @@
int con_equation_stat;
double E12[3], N12[3], E21[3], N21[3], Z12[3], Z21[3];
double XC, YC, ZC, omega, phi, kappa;
+ MATRIX M, MI;
};
/* conz_points.c */
@@ -121,13 +125,13 @@
struct Ortho_Camera_File_Ref *,
struct Ortho_Camera_Exp_Init *, double *,
double *, double *, double *, double *,
- double *);
+ double *, MATRIX *, MATRIX *);
int I_ortho_ref(double, double, double, double *, double *, double *,
struct Ortho_Camera_File_Ref *, double, double, double,
- double, double, double);
+ MATRIX);
int I_inverse_ortho_ref(double, double, double, double *, double *, double *,
struct Ortho_Camera_File_Ref *, double, double,
- double, double, double, double);
+ double, MATRIX);
/* ref_points.c */
int I_new_ref_point(struct Ortho_Photo_Points *, double, double, double,
double, int);
Modified: grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c
===================================================================
--- grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c 2010-11-28 22:22:49 UTC (rev 44471)
+++ grass/branches/develbranch_6/imagery/i.ortho.photo/libes/orthoref.c 2010-11-29 08:05:42 UTC (rev 44472)
@@ -32,29 +32,26 @@
#include <math.h>
#include "orthophoto.h"
#include <grass/imagery.h>
-#include "mat.h"
#include "matrixdefs.h"
#include "local_proto.h"
-#define MAX_ITERS 10 /* Max iteration is normal equation solution */
-#define CONV_LIMIT 1.0 /* meters */
+#define MAX_ITERS 1000 /* Max iteration is normal equation solution */
+#define CONV_LIMIT 0.1 /* meters */
#ifdef DEBUG
FILE *debug;
char msg[120];
#endif
-static int floating_exception;
-static void catch(int);
-
/* Compute the ortho rectification parameters */
/* XC,YC,ZC, Omega, Phi, Kappa */
int I_compute_ortho_equations(struct Ortho_Control_Points *cpz,
struct Ortho_Camera_File_Ref *cam_info,
struct Ortho_Camera_Exp_Init *init_info,
double *XC, double *YC, double *ZC,
- double *Omega, double *Phi, double *Kappa)
+ double *Omega, double *Phi, double *Kappa,
+ MATRIX *MO, MATRIX *MT)
{
MATRIX delta, epsilon, B, BT, C, E, N, CC, NN, UVW, XYZ, M, WT1;
double meanx, meany;
@@ -74,21 +71,16 @@
Q1 = (double)1.0;
- /*
- * floating_exception = 0;
- * sigfpe = signal (SIGFPE, catch);
- */
-
/* DEBUG */
#ifdef DEBUG
debug = fopen("ortho_compute.rst", "w");
if (debug == NULL) {
- sprintf(msg, "Cant open debug file ortho_analyze.rst\n");
+ sprintf(msg, "Cant open debug file ortho_compute.rst\n");
G_fatal_error(msg);
}
#endif
- /* Need 4 active control points */
+ /* Need at least 4 active control points */
active = 0;
for (i = 0; i < cpz->count; i++) {
if (cpz->status[i] > 0)
@@ -148,7 +140,7 @@
UVW.nrows = 3;
UVW.ncols = 1;
zero(&UVW);
- /* Oreintaiton Matrix M=[3,3] functions of (omega,phi,kappa) */
+ /* Orientation Matrix M=[3,3] functions of (omega,phi,kappa) */
M.nrows = 3;
M.ncols = 3;
zero(&M);
@@ -160,7 +152,7 @@
/******************** Start the solution *****************************/
- /* set Xp, Yp, and CFL form cam_info */
+ /* set Xp, Yp, and CFL from cam_info */
Xp = cam_info->Xp;
Yp = cam_info->Yp;
CFL = cam_info->CFL;
@@ -171,7 +163,7 @@
#endif
/* use initial estimates for XC,YC,ZC,omega,phi,kappa
- * and initial standard deviations if proveded by i.ortho.photo
+ * and initial standard deviations if provided by i.ortho.photo
*
* otherwise set from mean value of all active control points
* - init_info is generated by photo.init (file INIT_EXP)
@@ -194,12 +186,15 @@
WT1.x[4][4] = (Q1 / (init_info->phi_var * init_info->phi_var));
WT1.x[5][5] = (Q1 / (init_info->kappa_var * init_info->kappa_var));
}
- else { /* set from mean values of active control points */
-
-
+ else { /* set from mean values of active control points */
+ double meanX, meanY, meanZ;
+ int nz;
+
/* set intial XC and YC from mean values of control points */
meanx = meany = 0;
- n = 0;
+ meanX = meanY = meanZ = 0;
+ *ZC = 0;
+ n = nz = 0;
first = 1;
for (i = 0; i < cpz->count; i++) {
if (cpz->status[i] <= 0)
@@ -219,20 +214,37 @@
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++;
+ }
}
n++;
- meanx += *((cpz->e2)++);
- meany += *((cpz->n2)++);
- ((cpz->e1)++);
- ((cpz->n1)++);
+ meanx += *((cpz->e1)++);
+ meany += *((cpz->n1)++);
+ meanX += *((cpz->e2)++);
+ meanY += *((cpz->n2)++);
+ (cpz->z2)++;
}
- *XC = meanx / n;
- *YC = meany / n;
+ *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);
/* reset pointers */
for (i = 0; i < cpz->count; i++) {
@@ -242,20 +254,19 @@
(cpz->e2)--;
(cpz->n1)--;
(cpz->n2)--;
+ (cpz->z2)--;
}
- /* set initial ZC from: */
- /* scale ~= dist_photo/dist_grnd ~= (CFL)/(Z1 - Zc) */
- /* Zc ~= Z1 + CFL(dist_grnd)/(dist_photo) */
-
- *ZC = Z1 + (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));
*Kappa = (kappa2 - kappa1);
/* set initial weights */
@@ -362,12 +373,12 @@
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))++;
+ x = *((cpz->e1)++);
+ y = *((cpz->n1)++);
+ z = *((cpz->z1)++);
+ X = *((cpz->e2)++);
+ Y = *((cpz->n2)++);
+ Z = *((cpz->z2)++);
if (cpz->status[i] <= 0)
continue;
@@ -389,7 +400,7 @@
mu = XYZ.x[1][0];
nu = XYZ.x[2][0];
- /* Compute image space coordiantes */
+ /* Compute image space coordinates */
m_mult(&M, &XYZ, &UVW);
/* just an abbreviation */
@@ -397,7 +408,6 @@
V = UVW.x[1][0];
W = UVW.x[2][0];
-
/* Form Partial derivatives of Normal Equations */
xbar = x - Xp;
ybar = y - Yp;
@@ -454,7 +464,7 @@
m_mult(&BT, &E, &C);
/* CC += C */
m_add(&CC, &C, &CC);
- } /* end summation loop over all active control points */
+ } /* end summation loop over all active control points */
/* reset pointers */
for (i = 0; i < cpz->count; i++) {
@@ -479,7 +489,6 @@
CC.x[3][0], CC.x[4][0], CC.x[5][0]);
#endif
-
/* Add weigth matrix of unknowns to NN */
m_add(&NN, &WT1, &NN);
/* Solve for delta */
@@ -503,7 +512,7 @@
delta.x[4][0], delta.x[5][0]);
#endif
- } /* end ITERATION loop */
+ } /* end ITERATION loop */
/* This is the solution */
*XC = epsilon.x[0][0];
@@ -513,6 +522,45 @@
*Phi = epsilon.x[4][0];
*Kappa = epsilon.x[5][0];
+ /* Compute Orientation Matrix M from (Omega, Phi, Kappa); */
+ sw = sin(*Omega);
+ cw = cos(*Omega);
+ sp = sin(*Phi);
+ cp = cos(*Phi);
+ sk = sin(*Kappa);
+ ck = cos(*Kappa);
+
+ MO->nrows = 3;
+ MO->ncols = 3;
+ zero(MO);
+
+ MO->x[0][0] = cp * ck;
+ MO->x[0][1] = cw * sk + (sw * sp * ck);
+ MO->x[0][2] = sw * sk - (cw * sp * ck);
+ MO->x[1][0] = -(cp * sk);
+ MO->x[1][1] = cw * ck - (sw * sp * sk);
+ MO->x[1][2] = sw * ck + (cw * sp * sk);
+ MO->x[2][0] = sp;
+ MO->x[2][1] = -(sw * cp);
+ MO->x[2][2] = cw * cp;
+
+ /* Compute Transposed Orientation Matrix (Omega, Phi, Kappa); */
+
+ MT->nrows = 3;
+ MT->ncols = 3;
+ zero(MT);
+
+ /* M Transposed */
+ MT->x[0][0] = cp * ck;
+ MT->x[1][0] = cw * sk + (sw * sp * ck);
+ MT->x[2][0] = sw * sk - (cw * sp * ck);
+ MT->x[0][1] = -(cp * sk);
+ MT->x[1][1] = cw * ck - (sw * sp * sk);
+ MT->x[2][1] = sw * ck + (cw * sp * sk);
+ MT->x[0][2] = sp;
+ MT->x[1][2] = -(sw * cp);
+ MT->x[2][2] = cw * cp;
+
#ifdef DEBUG
fclose(debug);
#endif
@@ -520,24 +568,16 @@
return (1);
}
-static void catch(int n)
-{
- floating_exception = 1;
- signal(n, catch);
-}
-
/* given ground coordinates (e1,n1,z1) and the solution from above */
/* compute the photo coordinate (e2,n2) position */
int I_ortho_ref(double e1, double n1, double z1,
double *e2, double *n2, double *z2,
struct Ortho_Camera_File_Ref *cam_info,
- double XC, double YC, double ZC,
- double Omega, double Phi, double Kappa)
+ double XC, double YC, double ZC, MATRIX M)
{
- MATRIX UVW, XYZ, M;
+ MATRIX UVW, XYZ;
double U, V, W;
double Xp, Yp, CFL;
- double sw, cw, sp, cp, sk, ck;
/* Initialize and zero the matrices */
/* Object Space Coordinates */
@@ -548,35 +588,13 @@
UVW.nrows = 3;
UVW.ncols = 1;
zero(&UVW);
- /* Orientation Matrix */
- M.nrows = 3;
- M.ncols = 3;
- zero(&M);
/************************ Start the work ******************************/
- /* set Xp, Yp, and CFL form cam_info */
+ /* set Xp, Yp, and CFL from cam_info */
Xp = cam_info->Xp;
Yp = cam_info->Yp;
CFL = cam_info->CFL;
- /* Compute Orientation Matrix M from (Omega, Phi, Kappa); */
- sw = sin(Omega);
- cw = cos(Omega);
- sp = sin(Phi);
- cp = cos(Phi);
- sk = sin(Kappa);
- ck = cos(Kappa);
-
- M.x[0][0] = cp * ck;
- M.x[0][1] = cw * sk + (sw * sp * ck);
- M.x[0][2] = sw * sk - (cw * sp * ck);
- M.x[1][0] = -(cp * sk);
- M.x[1][1] = cw * ck - (sw * sp * sk);
- M.x[1][2] = sw * ck + (cw * sp * sk);
- M.x[2][0] = sp;
- M.x[2][1] = -(sw * cp);
- M.x[2][2] = cw * cp;
-
/* ObjSpace (&XYZ, XC,YC,ZC, X,Y,Z); */
XYZ.x[0][0] = e1 - XC;
XYZ.x[1][0] = n1 - YC;
@@ -601,13 +619,11 @@
int I_inverse_ortho_ref(double e1, double n1, double z1,
double *e2, double *n2, double *z2,
struct Ortho_Camera_File_Ref *cam_info,
- double XC, double YC, double ZC,
- double Omega, double Phi, double Kappa)
+ double XC, double YC, double ZC, MATRIX M)
{
- MATRIX UVW, XYZ, M;
+ MATRIX UVW, XYZ;
double lam, mu, nu;
double Xp, Yp, CFL;
- double sw, cw, sp, cp, sk, ck;
/* Initialize and zero matrices */
/* Object Space Coordinates */
@@ -618,36 +634,13 @@
UVW.nrows = 3;
UVW.ncols = 1;
zero(&UVW);
- /* Orientation Matrix */
- M.nrows = 3;
- M.ncols = 3;
- zero(&M);
/********************** Start the work ********************************/
- /* set Xp, Yp, and CFL form cam_info */
+ /* set Xp, Yp, and CFL from cam_info */
Xp = cam_info->Xp;
Yp = cam_info->Yp;
CFL = cam_info->CFL;
- /* Compute Orientation Matrix (Omega, Phi, Kappa); */
- sw = sin(Omega);
- cw = cos(Omega);
- sp = sin(Phi);
- cp = cos(Phi);
- sk = sin(Kappa);
- ck = cos(Kappa);
-
- /* M Transposed */
- M.x[0][0] = cp * ck;
- M.x[1][0] = cw * sk + (sw * sp * ck);
- M.x[2][0] = sw * sk - (cw * sp * ck);
- M.x[0][1] = -(cp * sk);
- M.x[1][1] = cw * ck - (sw * sp * sk);
- M.x[2][1] = sw * ck + (cw * sp * sk);
- M.x[0][2] = sp;
- M.x[1][2] = -(sw * cp);
- M.x[2][2] = cw * cp;
-
/* ImageSpace */
UVW.x[0][0] = e1 - Xp;
UVW.x[1][0] = n1 - Yp;
More information about the grass-commit
mailing list