[GRASS-SVN] r54285 - grass/trunk/imagery/i.ortho.photo/lib
svn_grass at osgeo.org
svn_grass at osgeo.org
Fri Dec 14 14:03:48 PST 2012
Author: mmetz
Date: 2012-12-14 14:03:48 -0800 (Fri, 14 Dec 2012)
New Revision: 54285
Removed:
grass/trunk/imagery/i.ortho.photo/lib/ask_camera.c
grass/trunk/imagery/i.ortho.photo/lib/ls_cameras.c
grass/trunk/imagery/i.ortho.photo/lib/ls_elev.c
Modified:
grass/trunk/imagery/i.ortho.photo/lib/cam_info.c
grass/trunk/imagery/i.ortho.photo/lib/camera.c
grass/trunk/imagery/i.ortho.photo/lib/conz_points.c
grass/trunk/imagery/i.ortho.photo/lib/m_mult.c
grass/trunk/imagery/i.ortho.photo/lib/orthophoto.h
grass/trunk/imagery/i.ortho.photo/lib/orthoref.c
Log:
ortho lib update
Deleted: grass/trunk/imagery/i.ortho.photo/lib/ask_camera.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/ask_camera.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/ask_camera.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -1,80 +0,0 @@
-/*** TODO: make non-interactive and merge with main imagery library ***/
-
-
-/*************************************************************
-* I_ask_camera_old (prompt,camera)
-* I_ask_camera_new (prompt,camera)
-* I_ask_camera_any (prompt,camera)
-*
-* prompt the user for an camera reference file name
-*************************************************************/
-#include <string.h>
-#include "orthophoto.h"
-#include <grass/ortholib.h>
-
-static int ask_camera(char *, char *);
-
-int I_ask_camera_old(char *prompt, char *camera)
-{
- while (1) {
- if (*prompt == 0)
- prompt = "Select an camera reference file";
- if (!ask_camera(prompt, camera))
- return 0;
- if (I_find_camera(camera))
- return 1;
- fprintf(stderr, "\n** %s - not found **\n\n", camera);
- }
-}
-
-int I_ask_camera_new(char *prompt, char *camera)
-{
- while (1) {
- if (*prompt == 0)
- prompt = "Enter a new camera reference file name";
- if (!ask_camera(prompt, camera))
- return 0;
- if (!I_find_camera(camera))
- return 1;
- fprintf(stderr, "\n** %s - exists, select another name **\n\n",
- camera);
- }
-}
-
-int I_ask_camera_any(char *prompt, char *camera)
-{
- if (*prompt == 0)
- prompt = "Enter a new or existing camera reference file";
- return ask_camera(prompt, camera);
-}
-
-static int ask_camera(char *prompt, char *camera)
-{
- char buf[1024];
-
- while (1) {
- fprintf(stderr, "\n%s\n", prompt);
- fprintf(stderr, "Enter 'list' for a list of existing camera files\n");
- fprintf(stderr, "Enter 'list -f' for a verbose listing\n");
- fprintf(stderr, "Hit RETURN %s\n", G_get_ask_return_msg());
- fprintf(stderr, "> ");
- if (!G_gets(buf))
- continue;
-
- G_squeeze(buf);
- fprintf(stderr, "<%s>\n", buf);
- if (*buf == 0)
- return 0;
-
- if (strcmp(buf, "list") == 0)
- I_list_cameras(0);
- else if (strcmp(buf, "list -f") == 0)
- I_list_cameras(1);
- else if (G_legal_filename(buf) < 0)
- fprintf(stderr, "\n** <%s> - illegal name **\n\n", buf);
- else
- break;
- }
- strcpy(camera, buf);
- return 1;
-}
Modified: grass/trunk/imagery/i.ortho.photo/lib/cam_info.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/cam_info.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/cam_info.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -1,4 +1,5 @@
#include <string.h>
+#include <grass/glocale.h>
#include "orthophoto.h"
#include <grass/ortholib.h>
@@ -17,38 +18,38 @@
char fid_id[30];
double Xf, Yf;
- G_getl(buf, IN_BUF, fd);
+ G_getl2(buf, IN_BUF, fd);
G_strip(buf);
- if (sscanf(buf, "CAMERA NAME %s \n", cam_name) == 1)
+ if (sscanf(buf, "CAMERA NAME %[^\n]", cam_name) == 1)
strcpy(cam_info->cam_name, cam_name);
- G_getl(buf, IN_BUF, fd);
+ G_getl2(buf, IN_BUF, fd);
G_strip(buf);
- if (sscanf(buf, "CAMERA ID %s \n", cam_id) == 1)
+ if (sscanf(buf, "CAMERA ID %[^\n]", cam_id) == 1)
strcpy(cam_info->cam_id, cam_id);
- G_getl(buf, IN_BUF, fd);
+ G_getl2(buf, IN_BUF, fd);
G_strip(buf);
if (sscanf(buf, "CAMERA XP %lf \n", &Xp) == 1)
cam_info->Xp = Xp;
- G_getl(buf, IN_BUF, fd);
+ G_getl2(buf, IN_BUF, fd);
G_strip(buf);
if (sscanf(buf, "CAMERA YP %lf \n", &Yp) == 1)
cam_info->Yp = Yp;
- G_getl(buf, IN_BUF, fd);
+ G_getl2(buf, IN_BUF, fd);
G_strip(buf);
if (sscanf(buf, "CAMERA CFL %lf \n", &CFL) == 1)
cam_info->CFL = CFL;
- G_getl(buf, IN_BUF, fd);
+ G_getl2(buf, IN_BUF, fd);
G_strip(buf);
if (sscanf(buf, "NUM FID %d \n", &num_fid) == 1)
cam_info->num_fid = num_fid;
for (n = 0; n < cam_info->num_fid; n++) {
- G_getl(buf, IN_BUF, fd);
+ G_getl2(buf, IN_BUF, fd);
G_strip(buf);
if (sscanf(buf, "%s %lf %lf", fid_id, &Xf, &Yf) == 3) {
strcpy(cam_info->fiducials[n].fid_id, fid_id);
@@ -95,7 +96,8 @@
fd = I_fopen_cam_file_old(camera);
if (fd == NULL) {
- G_warning("unable to open camera file %s in %s", camera, G_mapset());
+ G_warning(_("Unable to open camera file '%s' in '%s'"),
+ camera, G_mapset());
return 0;
}
@@ -103,7 +105,8 @@
stat = I_read_cam_info(fd, cam_info);
fclose(fd);
if (stat < 0) {
- G_warning("bad format in camera file %s in %s", camera, G_mapset());
+ G_warning(_("Bad format in camera file '%s' in '%s'"),
+ camera, G_mapset());
return 0;
}
@@ -118,7 +121,8 @@
fd = I_fopen_cam_file_new(camera);
if (fd == NULL) {
- G_warning("unable to open camera file %s in %s", camera, G_mapset());
+ G_warning(_("Unable to open camera file '%s' in '%s'"),
+ camera, G_mapset());
return 0;
}
Modified: grass/trunk/imagery/i.ortho.photo/lib/camera.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/camera.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/camera.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -18,7 +18,7 @@
if (!fd)
return 0;
- fprintf(fd, "%s", camera);
+ fprintf(fd, "%s\n", camera);
return 0;
}
Modified: grass/trunk/imagery/i.ortho.photo/lib/conz_points.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/conz_points.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/conz_points.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -182,7 +182,7 @@
n2 = con_cp->n2[i];
z2 = con_cp->z2[i];
- I_georef(e1, n1, &e0, &n0, E12, N12);
+ I_georef(e1, n1, &e0, &n0, E12, N12, 1);
/* I_new_con_point (photo_cp, e0,n0,z1,e2,n2,z2,status); */
I_new_con_point(photo_cp, e0, n0, z1, e2, n2, z2, status);
}
Deleted: grass/trunk/imagery/i.ortho.photo/lib/ls_cameras.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/ls_cameras.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/ls_cameras.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -1,65 +0,0 @@
-
-/*************************************************************
-* I_list_cameras (full)
-*************************************************************/
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <grass/imagery.h>
-#include <grass/ortholib.h>
-
-static char *tempfile = NULL;
-
-int I_list_cameras(int full)
-{
- char *element;
- char buf[1024];
- char title[50];
- FILE *ls, *temp;
- int any;
-
- if (tempfile == NULL)
- tempfile = G_tempfile();
-
- element = "camera";
- G__make_mapset_element(element);
-
- temp = fopen(tempfile, "w");
- if (temp == NULL)
- G_fatal_error("can't open any temp files");
- fprintf(temp, "Available cameras\n");
- fprintf(temp, "---------------------------------\n");
-
- any = 0;
- strcpy(buf, "cd ");
- G_file_name(buf + strlen(buf), element, "", G_mapset());
- strcat(buf, ";ls");
- if (!full)
- strcat(buf, " -C");
- if (ls = popen(buf, "r")) {
- while (G_getl(buf, sizeof buf, ls)) {
- any = 1;
- fprintf(temp, "%s", buf);
- if (full) {
- I_get_cam_title(buf, title, sizeof title);
- if (*title)
- fprintf(temp, " (%s)", title);
- fprintf(temp, "\n");
- }
- else
- fprintf(temp, "\n");
- }
- pclose(ls);
- }
- if (!any)
- fprintf(temp, "no camera files available\n");
- fprintf(temp, "---------------------------------\n");
- fclose(temp);
- sprintf(buf, "$GRASS_PAGER %s", tempfile);
- G_system(buf);
- unlink(tempfile);
- fprintf(stderr, "hit RETURN to continue -->");
- G_gets(buf);
-
- return 0;
-}
Deleted: grass/trunk/imagery/i.ortho.photo/lib/ls_elev.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/ls_elev.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/ls_elev.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -1,59 +0,0 @@
-
-/*************************************************************
-* I_list_elev (full)
-*************************************************************/
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <grass/imagery.h>
-
-static char *tempfile = NULL;
-
-int I_list_elev(int full)
-{
- char *element;
- char buf[1024];
- FILE *ls, *temp;
- int any;
-
- if (tempfile == NULL)
- tempfile = G_tempfile();
-
- element = "cell";
- G__make_mapset_element(element);
-
- temp = fopen(tempfile, "w");
- if (temp == NULL)
- G_fatal_error("can't open any temp files");
- fprintf(temp, "Available raster maps:\n");
- fprintf(temp, "---------------------------------\n");
-
- any = 0;
- strcpy(buf, "cd ");
- G_file_name(buf + strlen(buf), element, " ", " ");
- strcat(buf, ";ls");
- strcat(buf, " -C");
- if (ls = popen(buf, "r")) {
- while (G_getl(buf, sizeof buf, ls)) {
- any = 1;
- fprintf(temp, "%s", buf);
- fprintf(temp, "\n");
- }
- pclose(ls);
- }
- if (!any)
- fprintf(temp, "no raster maps available\n");
- fprintf(temp, "---------------------------------\n");
- fclose(temp);
- sprintf(buf, "$GRASS_PAGER %s", tempfile);
- G_system(buf);
- unlink(tempfile);
- fprintf(stderr, "hit RETURN to continue -->");
- G_gets(buf);
-
-/******/
- G_list_element("cell", "cell", G_mapset(), NULL);
-
-
- return 0;
-}
Modified: grass/trunk/imagery/i.ortho.photo/lib/m_mult.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/m_mult.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/m_mult.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -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/trunk/imagery/i.ortho.photo/lib/orthophoto.h
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/orthophoto.h 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/orthophoto.h 2012-12-14 22:03:48 UTC (rev 54285)
@@ -1,5 +1,6 @@
#include <grass/gis.h>
#include <grass/imagery.h>
+#include "mat.h"
/* #define DEBUG 1 */
@@ -15,8 +16,8 @@
int nfiles;
struct Ortho_Image_Group_Ref_Files
{
- char name[30];
- char mapset[30];
+ char name[GNAME_MAX];
+ char mapset[GMAPSET_MAX];
} *file;
struct Ortho_Ref_Color
{
@@ -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/trunk/imagery/i.ortho.photo/lib/orthoref.c
===================================================================
--- grass/trunk/imagery/i.ortho.photo/lib/orthoref.c 2012-12-14 21:36:47 UTC (rev 54284)
+++ grass/trunk/imagery/i.ortho.photo/lib/orthoref.c 2012-12-14 22:03:48 UTC (rev 54285)
@@ -30,36 +30,31 @@
#include <signal.h>
#include <stdio.h>
#include <math.h>
-#include "orthophoto.h"
#include <grass/imagery.h>
-#include "mat.h"
+#include <grass/glocale.h>
+#include "orthophoto.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;
- 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;
@@ -74,21 +69,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 +138,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 +150,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 +161,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,11 +184,13 @@
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 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 = meanX = meanY = meanZ = 0;
+ x = y = X = Y = 0;
n = 0;
first = 1;
for (i = 0; i < cpz->count; i++) {
@@ -207,55 +199,52 @@
/* set initial XC, YC */
if (first) {
- X1 = *(cpz->e2);
- x1 = *(cpz->e1);
- Y1 = *(cpz->n2);
- y1 = *(cpz->n1);
- Z1 = *(cpz->z2);
+ X = cpz->e2[i];
+ x = cpz->e1[i];
+ Y = cpz->n2[i];
+ y = cpz->n1[i];
first = 0;
}
- if (!first) {
- X2 = *(cpz->e2);
- x2 = *(cpz->e1);
- Y2 = *(cpz->n2);
- y2 = *(cpz->n1);
- dist_photo =
- sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
- dist_grnd =
- sqrt(((X1 - X2) * (X1 - X2)) + ((Y1 - Y2) * (Y1 - Y2)));
- }
+ else {
+ /* 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++;
- meanx += *((cpz->e2)++);
- meany += *((cpz->n2)++);
- ((cpz->e1)++);
- ((cpz->n1)++);
+ n++;
+ }
+ }
}
- *XC = meanx / n;
- *YC = meany / n;
+ if (!n) /* Poorly placed Control Points */
+ return -1;
- /* reset pointers */
- for (i = 0; i < cpz->count; i++) {
- if (cpz->status[i] <= 0)
- continue;
- (cpz->e1)--;
- (cpz->e2)--;
- (cpz->n1)--;
- (cpz->n2)--;
- }
+ meanx /= n;
+ meany /= n;
+ meanX /= n;
+ meanY /= n;
+ meanZ /= n;
+ *XC = meanX;
+ *YC = meanY;
+ *ZC = meanZ;
- /* 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 - y), (meanx - x));
+ kappa2 = atan2((meanY - Y), (meanX - X));
*Kappa = (kappa2 - kappa1);
/* set initial weights */
@@ -273,6 +262,9 @@
WT1.x[4][4] = (Q1 / (phi_var * phi_var));
WT1.x[5][5] = (Q1 / (kappa_var * kappa_var));
}
+ 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");
@@ -362,34 +354,33 @@
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];
- /* Compute image space coordiantes */
+ /* Compute image space coordinates */
m_mult(&M, &XYZ, &UVW);
/* just an abbreviation */
@@ -397,7 +388,6 @@
V = UVW.x[1][0];
W = UVW.x[2][0];
-
/* Form Partial derivatives of Normal Equations */
xbar = x - Xp;
ybar = y - Yp;
@@ -454,18 +444,8 @@
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++) {
- (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++)
@@ -479,7 +459,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 +482,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 +492,51 @@
*Phi = epsilon.x[4][0];
*Kappa = epsilon.x[5][0];
+ 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 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 from Omega, Phi, Kappa */
+
+ MT->nrows = 3;
+ MT->ncols = 3;
+ zero(MT);
+
+ /* 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);
+ 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 +544,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,36 +564,14 @@
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); */
+ /* 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;
@@ -601,13 +595,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,44 +610,21 @@
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 */
+ /* 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