This is an automated email from the git hooks/post-receive script.
sebastic-guest pushed a commit to branch upstream-master
in repository pktools.
commit e12d767e56be1886fa1f41c7099ca090fa84c5cd
Author: Pieter Kempeneers <kempe...@gmail.com>
Date: Fri Dec 7 14:26:23 2012 +0100
added pkascii2ogr, SensorModel.h and pksensormodel
---
src/algorithms/SensorModel.h | 387 +++++++++++++++++++++++++
src/apps/pkascii2ogr.cc | 122 ++++++++
src/apps/pksensormodel.cc | 676 +++++++++++++++++++++++++++++++++++++++++++
src/apps/pksensormodel.h | 81 ++++++
4 files changed, 1266 insertions(+)
diff --git a/src/algorithms/SensorModel.h b/src/algorithms/SensorModel.h
new file mode 100644
index 0000000..2323452
--- /dev/null
+++ b/src/algorithms/SensorModel.h
@@ -0,0 +1,387 @@
+/**********************************************************************
+SensorModel.h: class for sensor model (calculate position on earth from pos
and attidude information)
+Copyright (C) 2008-2012 Pieter Kempeneers
+
+This file is part of pktools
+
+pktools is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+pktools is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with pktools. If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+#ifndef _SENSORMODEL_
+#define _SENSORMODEL_
+#include <assert.h>
+#include <math.h>
+#include <vector>
+#include <iostream>
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_vector.h>
+#include <gsl/gsl_permutation.h>
+#include <gsl/gsl_linalg.h>
+#include <gsl/gsl_blas.h>
+#include <gsl/gsl_rng.h>
+#include <gsl/gsl_randist.h>
+#include <gslwrap/matrix_double.h>
+#include "ogr_spatialref.h"
+
+#ifndef PI
+#define PI 3.1415926535897932384626433832795
+#endif
+
+// #ifndef DEG2RAD
+// #define DEG2RAD(DEG) (DEG/180.0*PI)
+// #endif
+
+namespace SensorModel
+{
+ enum Type { PUSHBROOM=0, WHISKBROOM=1 , FRAME=2};
+
+ class SensorModel{
+ public:
+ SensorModel(void){
+ m_bcpos.resize(3);
+ m_bcatt.resize(3);
+ m_bcpos[0]=0;
+ m_bcpos[1]=0;
+ m_bcpos[2]=0;
+ m_bcatt[0]=0;
+ m_bcatt[1]=0;
+ m_bcatt[2]=0;
+ };
+ SensorModel(int theModel) : m_model(theModel){
+ m_bcpos.resize(3);
+ m_bcatt.resize(3);
+ m_bcpos[0]=0;
+ m_bcpos[1]=0;
+ m_bcpos[2]=0;
+ m_bcatt[0]=0;
+ m_bcatt[1]=0;
+ m_bcatt[2]=0;
+ };
+ ~SensorModel(){};
+ // double deg2rad(double angle) const{return acos(-1)*angle/180.0;};
+ static double deg2rad(double angle) {return PI*angle/180.0;};
+ static double rad2deg(double angle) {return 180*angle/PI;};
+ void setModel(int theModel){m_model=theModel;};
+ int getModel() const {return m_model;};
+ void setFOV(double fovDEG){m_fov=deg2rad(fovDEG);};
+ void setNcol(int ncol){m_ncol=ncol;};
+ void setNrow(int nrow){m_nrow=nrow;};
+ void setDx(double dx){m_dx=dx;};
+ void setDy(double dy){m_dy=dy;};
+ void setF(double fc){m_fc=fc;};
+ double getF() const {return m_fc;};
+ void setPPx(double ppx){m_ppx=ppx;};
+ void setPPy(double ppy){m_ppy=ppy;};
+ void setBoresightPos(const gsl::vector& bcpos){m_bcpos=bcpos;};
+ void setBoresightAtt(const gsl::vector& bcatt){m_bcatt=bcatt;};
+ gsl::vector getBoresightPos() const{return m_bcpos;};
+ gsl::vector getBoresightAtt() const{return m_bcatt;};
+ void setPolynome(const std::vector<double>& polynome)
{m_polynome=polynome;};
+ void setDatum(const std::string&
theDatum="WGS84"){m_spatialref.SetWellKnownGeogCS(theDatum.c_str());};
+ double getZenith(const gsl::vector& att_platform) const{
+ gsl::vector normallevel(3);
+ gsl::vector normalplatform(3);
+ normallevel[0]=0;
+ normallevel[1]=0;
+ normallevel[2]=-1;
+ gsl::matrix rotM(3,3);
+
rotM=getRz(deg2rad(att_platform[2]))*getRy(deg2rad(att_platform[1]))*getRx(deg2rad(att_platform[0]));
+ normalplatform=rotM*normallevel;
+ return rad2deg(acos(-normalplatform[2]));
+ };
+ gsl::vector getPos(const gsl::vector& pos_platform, const gsl::vector&
att_platform, int row, int column, double elevation) const{
+ gsl::vector thePosition(3);
+ gsl::vector pos_ellips(3);
+ gsl::vector ppl_deg=pos_platform;
+ gsl::vector apl_deg=att_platform;
+ ppl_deg+=m_bcpos;
+ apl_deg+=m_bcatt;
+ gsl::vector ppl_rad(3);
+ gsl::vector apl_rad(3);
+ ppl_rad[0]=deg2rad(ppl_deg[0]);
+ ppl_rad[1]=deg2rad(ppl_deg[1]);
+ ppl_rad[2]=ppl_deg[2];//add geoid elevation if necessary...
+ apl_rad[0]=deg2rad(apl_deg[0]);//roll
+ apl_rad[1]=deg2rad(apl_deg[1]);//pitch
+ apl_rad[2]=deg2rad(apl_deg[2]);//yaw
+ gsl::vector pos_ecef=pECEF(ppl_rad,apl_rad,row,column);
+ pos_ellips=ecef2geo(pos_ecef);
+ pos_ellips[2]=0;
+ thePosition=getXYatZ(elevation,ppl_deg,pos_ellips);
+ return thePosition;
+ };
+ double getDistGeo(const gsl::vector& pos1, const gsl::vector& pos2) const{
+ double lon1=pos1[0];
+ double lat1=pos1[1];
+ double lon2=pos2[0];
+ double lat2=pos2[1];
+ double result;
+ //simplified formula (spherical approximation)
+ // result=2*asin(sqrt(pow(sin((lat1-lat2)/2),2) +
cos(lat1)*cos(lat2)*pow(sin((lon1-lon2)/2),2)));
+ //using loxodromes
+ result=(pos1[1]==pos2[1]) ?
lox2(deg2rad(pos1[1]),deg2rad(pos1[0]),deg2rad(pos2[0])) :
lox1(deg2rad(pos1[1]),deg2rad(pos1[0]),deg2rad(pos2[1]),deg2rad(pos2[0]));
+ return result;
+ };
+ gsl::vector geo2ecef(const gsl::vector& pos_geo) const{
+ gsl::vector pos_ecef(3);
+ double nu=getNu(pos_geo[1]);
+ double f1=(nu+pos_geo[2])*cos(pos_geo[1]);
+ double e1=getE1();
+ pos_ecef[0]=f1*cos(pos_geo[0]);
+ pos_ecef[1]=f1*sin(pos_geo[0]);
+ pos_ecef[2]=(nu*(1-e1*e1)+pos_geo[2])*sin(pos_geo[1]);
+ return pos_ecef;
+ };
+ gsl::vector ecef2geo(const gsl::vector& pos_ecef) const{
+ gsl::vector pos_geo(3);
+ pos_geo[0]=atan2(pos_ecef[1],pos_ecef[0]);
+ while(pos_geo[0]>2*PI)
+ pos_geo[0]-=2*PI;
+ double r=sqrt(pos_ecef[0]*pos_ecef[0]+pos_ecef[1]*pos_ecef[1]);
+ double f_earth=1.0/m_spatialref.GetInvFlattening();
+ double e2=f_earth*(2-f_earth);
+ double Ae=m_spatialref.GetSemiMajor();
+ pos_geo[1]=atan(pos_ecef[2]/r);
+ double c=1;
+ double lat=1E+30;
+ int iterations=0;
+ while(sqrt((pos_geo[1]-lat)*(pos_geo[1]-lat))>1E-15){
+ ++iterations;
+ lat=pos_geo[1];
+ c=1.0/sqrt(1-e2*sin(lat)*sin(lat));
+ pos_geo[1]=atan((pos_ecef[2]+Ae*c*e2*sin(lat))/r);
+ }
+ pos_geo[2]=r/cos(pos_geo[1])-Ae*c;
+ pos_geo[0]=rad2deg(pos_geo[0]);
+ pos_geo[1]=rad2deg(pos_geo[1]);
+ return pos_geo;
+ };
+ private:
+ //line function; In this function, point 0 is the platform position, point
1 is the ellipsoid position
+ gsl::vector getXYatZ (double inZ, gsl::vector p0, gsl::vector p1) const{
+ gsl::vector posatz(3);
+ posatz[0]=p0[0]+(p0[0]-p1[0])/(p0[2]-p1[2])*(inZ-p0[2]);
+ posatz[1]=p0[1]+(p0[1]-p1[1])/(p0[2]-p1[2])*(inZ-p0[2]);
+ posatz[2]=inZ;
+ return posatz;
+ };
+ //get First eccentricity of the Earth's surface
+ double getE1() const{double
f_earth=1.0/m_spatialref.GetInvFlattening();return sqrt(f_earth*(2-f_earth));};
+ //get Second eccentricity of the Earth's surface
+ double getEearth() const{double Ae=m_spatialref.GetSemiMajor(); double
Be=m_spatialref.GetSemiMinor();return sqrt(Ae*Ae-Be*Be)/Ae;};
+ //get Radius of curvature normal to the meridian
+ double getNu(double lat) const{double sinlat=sin(lat);return
m_spatialref.GetSemiMajor()/sqrt(1-getE1()*getE1()*sinlat*sinlat);};
+ //get Ellipsoid radius in the plane of the meridian
+ double getRo(double lat) const{double e1=getE1();double
sinlat=sin(lat);return
(m_spatialref.GetSemiMajor()*(1-e1*e1))/sqrt(1-getE1()*getE1()*sinlat*sinlat);};
+ double getEccentricity() const{return
sqrt(1-m_spatialref.GetSemiMinor()*m_spatialref.GetSemiMinor()/m_spatialref.GetSemiMajor()/m_spatialref.GetSemiMajor());};
+ //Loxodromes are closely related with Mercator projection as the main
feature of this projection is that loxodromes are projected as straight lines.
To find the angle of the loxodrome between two points one has simply to project
the two points using a Mercator projection and calculate the angle of the line
joining the two resulting points. Formulas for Mercator projection on the
ellipsoid can be found in many places (for instance "Map Projections - A
Working Manual" by J.P. Snyder, U [...]
+ //project lat lon to Mercator projection
+ void getMxy(double& lon, double& lat) const{
+ // lon=m_spatialref.GetSemiMajor()*lon;
+ // double sinlat=sin(lat);
+ // double f1=(1+sinlat)/(1-sinlat);
+ // double eps=getEccentricity();
+ // double f2=pow((1-eps*sinlat)/(1+eps*sinlat),eps);
+ // lat=m_spatialref.GetSemiMajor()/2.0*log(f1*f2);
+ // //test
+ // std::cout << "Jan Mercator lat, lon: " << lat << ", " << lon <<
std::endl;
+ OGRSpatialReference oSourceSRS, oTargetSRS;
+ OGRCoordinateTransformation *poCT;
+ oSourceSRS.importFromEPSG(4326);
+ oTargetSRS.importFromEPSG(3395);
+ poCT = OGRCreateCoordinateTransformation( &oSourceSRS,
+ &oTargetSRS );
+ lon=rad2deg(lon);
+ lat=rad2deg(lat);
+ poCT->Transform(1,&lon,&lat);
+ };
+
+ gsl::vector pECEF(const gsl::vector& pos, const gsl::vector& attitude, int
row, int column) const{
+ gsl::vector A(3);
+ gsl::matrix B(3,3);
+ B.set_element(0,0,-sin(pos[1])*cos(pos[0]));
+ B.set_element(0,1,-sin(pos[0]));
+ B.set_element(0,2,-cos(pos[1])*cos(pos[0]));
+ B.set_element(1,0,-sin(pos[1])*sin(pos[0]));
+ B.set_element(1,1,cos(pos[0]));
+ B.set_element(1,2,-cos(pos[1])*sin(pos[0]));
+ B.set_element(2,0,cos(pos[1]));
+ B.set_element(2,1,0);
+ B.set_element(2,2,-sin(pos[1]));
+ A=geo2ecef(pos);
+ gsl::vector C(3);
+ A+=B*SV(row,column,attitude,pos[2]);
+ return A;
+ };
+ gsl::vector getIV(int row, int column) const{
+ gsl::vector iv(3);
+ iv[0]=0;
+ gsl::vector scanAngle(2);
+ if(getModel()==PUSHBROOM){
+ scanAngle=scanAngle_PB(row,column);
+ iv[0]=scanAngle[1];
+ iv[1]=tan(scanAngle[0]);
+ }
+ else if(getModel()==WHISKBROOM){
+ iv[0]=0;
+ iv[1]=tan(scanAngle_WB(column));
+ }
+ else//not implemented?
+ assert(0);
+ iv[2]=-1.0;
+ return iv;
+ };
+ double scanAngle_WB(int column) const{
+ return (-m_fov/2.0+column*(m_fov/(m_ncol-1)));
+ };
+ gsl::vector SV(int row, int column, const gsl::vector& attitude, double z)
const{
+ gsl::vector rv(3);
+ gsl::matrix rotM(3,3);
+ rotM=getRz(attitude[2])*getRy(attitude[1])*getRx(attitude[0]);
+ rv=rotM*getIV(row,column);
+ double height=rv[2];
+ // gsl_blas_dscal(z/height,&rv);
+ // return rv;
+ rv*=z/height;
+ return rv;
+ };
+ gsl::vector scanAngle_PB(int row, int column) const{
+ gsl::vector alpha(2);
+ double r=corr_along(column);
+ double theAx=(column<m_ppx) ?
m_dx*0.000001*(m_ppx-(column+corr_across(column))) :
m_dx*0.000001*(m_ppx-(column-corr_across(column)));
+ double theAy=m_dy*0.000001*(m_ppy-r);
+ alpha[0]=(getModel()==FRAME) ? atan(theAx/getF()) : atan(-theAx/getF());
+ alpha[1]=atan(-theAy/getF());
+ return alpha;
+ };
+ //geocentric coordinate system is right-handed, orthogogal Cartesian
system with its origin at the centre of the earth. The direct Helmert
transformation from lat/lon and ellipsoid height to geocentric x,y,z is
returned in posX, posY, posZ
+ //Euclidian distance between points on sphere
+ //Euclidian distance between two vectors
+ double getDist(const gsl::vector& v1, const gsl::vector& v2)
const{gsl::vector dv=v1;dv-=v2;return dv.norm2();};
+ double lox1(double lat1, double lon1, double lat2, double lon2) const{
+ double result=(M(lat2)-M(lat1))/cos(Az(lat1,lon1,lat2,lon2));
+ return result;
+ };
+ double lox2(double lat, double lon1, double lon2) const{
+ double eps2=getEccentricity();
+ eps2*=eps2;
+ double sin2=sin(lat);
+ sin2*=sin2;
+ return
m_spatialref.GetSemiMajor()*(lon2-lon1)*cos(lat)/sqrt(1-eps2*sin2);
+ };
+ //length of the meridian from Equator to point
+ double M(double lat) const{
+ double eps2=getEccentricity();
+ eps2*=eps2;
+ double eps4=eps2*eps2;
+ double eps6=eps4*eps2;
+ double t1=(1-eps2/4.0-3*eps4/64.0-5*eps6/256.0)*lat;
+ double t2=(3*eps2/8.0+3*eps4/32.0+45*eps6/1024.0)*sin(2*lat);
+ double t3=(15*eps4/256.0+45*eps6/1024.0)*sin(4*lat);
+ double t4=35*eps6/3072.0*sin(6*lat);
+ double semiMajor=m_spatialref.GetSemiMajor();
+ return semiMajor*(t1-t2+t3-t4);
+ };
+ double atan33(double x, double y) const{
+ double angle=atan2(y,x);
+ while(angle<0)
+ angle+=2*PI;
+ angle=PI/2.0+2*PI-angle;
+ while(angle>2*PI)
+ angle-=2*PI;
+ return angle;
+ };
+ //The azimuth from North (clockwise) of the loxodrome from point 1 to
point 2 will than be given by:
+ double Az(double lat1, double lon1, double lat2, double lon2) const{
+ double x1=lon1;
+ double x2=lon2;
+ double y1=lat1;
+ double y2=lat2;
+ getMxy(x1,y1);
+ getMxy(x2,y2);
+ return atan33(x2-x1,y2-y1);
+ };
+ double corr_along(double c) const{
+ return 0;
+ };
+ double corr_across(double c) const{
+ double result=0;
+ if(m_polynome.size()){
+ double absc=1;
+ result+=absc*m_polynome[0];
+ for(int degree=1;degree<m_polynome.size();++degree){
+ absc*=sqrt((c-m_ppx)*(c-m_ppx));
+ result+=absc*m_polynome[degree];
+ }
+ }
+ return result;
+ };
+ gsl::matrix getRx(double theta) const{
+ gsl::matrix Rx(3,3);
+ Rx.set_element(0,0,1);
+ Rx.set_element(0,1,0);
+ Rx.set_element(0,2,0);
+ Rx.set_element(1,0,0);
+ Rx.set_element(1,1,cos(theta));
+ Rx.set_element(1,2,-sin(theta));
+ Rx.set_element(2,0,0);
+ Rx.set_element(2,1,sin(theta));
+ Rx.set_element(2,2,cos(theta));
+ return Rx;
+ };
+ gsl::matrix getRy(double theta) const{
+ gsl::matrix Ry(3,3);
+ Ry.set_element(0,0,cos(theta));
+ Ry.set_element(0,1,0);
+ Ry.set_element(0,2,sin(theta));
+ Ry.set_element(1,0,0);
+ Ry.set_element(1,1,1);
+ Ry.set_element(1,2,0);
+ Ry.set_element(2,0,-sin(theta));
+ Ry.set_element(2,1,0);
+ Ry.set_element(2,2,cos(theta));
+ return Ry;
+ };
+ gsl::matrix getRz(double theta) const{
+ gsl::matrix Rz(3,3);
+ Rz.set_element(0,0,cos(theta));
+ Rz.set_element(0,1,-sin(theta));
+ Rz.set_element(0,2,0);
+ Rz.set_element(1,0,sin(theta));
+ Rz.set_element(1,1,cos(theta));
+ Rz.set_element(1,2,0);
+ Rz.set_element(2,0,0);
+ Rz.set_element(2,1,0);
+ Rz.set_element(2,2,1);
+ return Rz;
+ };
+ int m_model;
+ OGRSpatialReference m_spatialref;
+ //Ae=m_spatialref.GetSemiMajor()
+ //Be=m_spatialref.GetSemiMinor()
+ //f_earth=1.0/m_spatialref.GetInvFlattening()
+ std::string m_datum;
+ double m_ppx;
+ double m_ppy;
+ double m_fc;
+ double m_fov;
+ int m_ncol;
+ int m_nrow;
+ double m_dx;
+ double m_dy;
+ gsl::vector m_bcpos;
+ gsl::vector m_bcatt;
+ std::vector<double> m_polynome;
+ };
+}
+#endif //_SENSORMODEL_
diff --git a/src/apps/pkascii2ogr.cc b/src/apps/pkascii2ogr.cc
new file mode 100644
index 0000000..b56a305
--- /dev/null
+++ b/src/apps/pkascii2ogr.cc
@@ -0,0 +1,122 @@
+/**********************************************************************
+pkascii2ogr.cc: program to create vector (ogr) points polygon) from text file
+Copyright (C) 2008-2012 Pieter Kempeneers
+
+This file is part of pktools
+
+pktools is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+pktools is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with pktools. If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+#include <string>
+#include <fstream>
+#include <assert.h>
+#include "base/Optionpk.h"
+#include "imageclasses/ImgWriterOgr.h"
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+int main(int argc, char *argv[])
+{
+ std::string versionString="version ";
+ versionString+=VERSION;
+ versionString+=", Copyright (C) 2008-2012 Pieter Kempeneers.\n\
+ This program comes with ABSOLUTELY NO WARRANTY; for details type use option
-h.\n\
+ This is free software, and you are welcome to redistribute it\n\
+ under certain conditions; use option --license for details.";
+ Optionpk<bool> version_opt("\0","version",versionString,false);
+ Optionpk<bool> license_opt("lic","license","show license information",false);
+ Optionpk<bool> todo_opt("\0","todo","",false);
+ Optionpk<bool> help_opt("h","help","shows this help info",false);
+ Optionpk<string> input_opt("i","input","Input ASCII file");
+ Optionpk<string> output_opt("o", "output", "Output file");
+ Optionpk<short> colX_opt("x", "x", "column number of x (0)", 0);
+ Optionpk<short> colY_opt("y", "y", "column number of y (1)", 1);
+ Optionpk<bool> polygon_opt("l", "line", "create OGRPolygon as geometry
instead of points. Fields are taken from first point and polygon is
automatically closed (no need to repeat first point at last line). (false: use
OGRPoint)", false);
+ Optionpk<string> fname_opt("n", "name", "Field names for the columns in the
input ascii file");
+ Optionpk<string> ftype_opt("ot", "ot", "Field type (Real, Integer, String)
for each of the fields as defined by name","Real");
+ Optionpk<string> itype_opt("of", "of", "image type string", "ESRI
Shapefile");
+ Optionpk<string> projection_opt("p", "projection", "use EPSG:<code> or Wkt
string", "EPSG:4326");
+ Optionpk<int> verbose_opt("v", "verbose", "verbose (0)", 0);
+
+ version_opt.retrieveOption(argc,argv);
+ license_opt.retrieveOption(argc,argv);
+ help_opt.retrieveOption(argc,argv);
+ todo_opt.retrieveOption(argc,argv);
+ input_opt.retrieveOption(argc,argv);
+ output_opt.retrieveOption(argc,argv);
+ colX_opt.retrieveOption(argc,argv);
+ colY_opt.retrieveOption(argc,argv);
+ polygon_opt.retrieveOption(argc,argv);
+ fname_opt.retrieveOption(argc,argv);
+ ftype_opt.retrieveOption(argc,argv);
+ itype_opt.retrieveOption(argc,argv);
+ projection_opt.retrieveOption(argc,argv);
+ verbose_opt.retrieveOption(argc,argv);
+
+ if(version_opt[0]){
+ cout << version_opt.getHelp() << endl;
+ exit(0);
+ }
+ if(license_opt[0]){
+ cout << Optionpk<bool>::getGPLv3License() << endl;
+ exit(0);
+ }
+
+ if(help_opt[0]){
+ cout << "usage: pksensormodel -i asciifile [OPTIONS]" << endl;
+ exit(0);
+ }
+
+ string theProjection;
+ theProjection=projection_opt[0];
+ int ogr_typecount=11;//hard coded for now!
+ while(ftype_opt.size()<fname_opt.size())
+ ftype_opt.push_back(ftype_opt[0]);
+ // vector<string> fname(fname_opt.size());
+ vector<OGRFieldType> ftype(ftype_opt.size());
+ if(verbose_opt[0])
+ cout << "field types can be: ";
+ for(int ifield=0;ifield<fname_opt.size();++ifield){
+ // fname[ifield]=fname_opt[ifield];
+ for(int iType = 0; iType < ogr_typecount; ++iType){
+ if(!ifield&&verbose_opt[0])
+ cout << " " << OGRFieldDefn::GetFieldTypeName((OGRFieldType)iType);
+ if( OGRFieldDefn::GetFieldTypeName((OGRFieldType)iType) != NULL
+ && EQUAL(OGRFieldDefn::GetFieldTypeName((OGRFieldType)iType),
+ ftype_opt[ifield].c_str()))
+ ftype[ifield]=(OGRFieldType) iType;
+ }
+ }
+ //todo: what if unknown?
+ if(verbose_opt[0]){
+ cout << endl << "field types are: ";
+ for(int ifield=0;ifield<ftype.size();++ifield)
+ cout << OGRFieldDefn::GetFieldTypeName(ftype[ifield]) << " ";
+ cout << endl;
+ }
+
+ ImgWriterOgr imgWriter;
+ imgWriter.open(output_opt[0]);
+ try{
+ if(polygon_opt[0])
+ imgWriter.ascii2shape(input_opt[0], "New Layer", fname_opt, ftype,
colX_opt[0], colY_opt[0], theProjection, wkbPolygon);
+ else
+ imgWriter.ascii2shape(input_opt[0], "New Layer", fname_opt, ftype,
colX_opt[0], colY_opt[0], theProjection, wkbPoint);
+ }
+ catch(string errorString){
+ cout << errorString << endl;
+ }
+ imgWriter.close();
+}
diff --git a/src/apps/pksensormodel.cc b/src/apps/pksensormodel.cc
new file mode 100644
index 0000000..54194ca
--- /dev/null
+++ b/src/apps/pksensormodel.cc
@@ -0,0 +1,676 @@
+/**********************************************************************
+pksensormodel.cc: program to calculate geometric position based on row
(sensor), col (sensor), roll, pitch, yaw and lens coordinates
+Copyright (C) 2008-2012 Pieter Kempeneers
+
+This file is part of pktools
+
+pktools is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+pktools is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with pktools. If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <vector>
+#include <math.h>
+#include <nlopt.hpp>
+#include "base/Optionpk.h"
+#include "algorithms/Histogram.h"
+#include "pksensormodel.h"
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+double objFunction(const std::vector<double> &x, std::vector<double> &grad,
void *my_func_data){
+ assert(grad.empty());
+ double error=0;
+ DataModel *dm=reinterpret_cast<DataModel*> (my_func_data);
+ gsl::vector bc_att(3);
+ bc_att[0]=x[0];
+ bc_att[1]=x[1];
+ bc_att[2]=x[2];
+ dm->setBoresightAtt(bc_att);
+ for(unsigned int ipoint=0;ipoint<dm->getSize();++ipoint){
+ double e=dm->getDistGeo(ipoint);
+ error+=e;
+ }
+ error/=dm->getSize();
+ return error;
+}
+
+int main(int argc, char *argv[])
+{
+ std::string versionString="version ";
+ versionString+=VERSION;
+ versionString+=", Copyright (C) 2008-2012 Pieter Kempeneers.\n\
+ This program comes with ABSOLUTELY NO WARRANTY; for details type use option
-h.\n\
+ This is free software, and you are welcome to redistribute it\n\
+ under certain conditions; use option --license for details.";
+ Optionpk<bool> version_opt("\0","version",versionString,false);
+ Optionpk<bool> license_opt("lic","license","show license information",false);
+ Optionpk<bool> todo_opt("\0","todo","",false);
+ Optionpk<bool> help_opt("h","help","shows this help info",false);
+ Optionpk<string> input_opt("i","input","name of the input text file");
+ Optionpk<string> datum_opt("datum","datum","GPS datum of the input
points","WGS84");
+ Optionpk<int> s_srs_opt("s_srs","s_srs","source EPSG (integer) code of GCP
input coordinates",4326);
+ Optionpk<int> t_srs_opt("t_srs","t_srs","target EPSG (integer) code to
project output coordinates",4326);
+ Optionpk<double> focal_opt("f","focal","focal length of the camera (in
m)",0.041517);
+ Optionpk<double> dx_opt("dx","dx","pixel size in CCD (in micro m)",20);
+ Optionpk<double> dy_opt("dy","dy","pixel size in CCD (in micro m)",20);
+ Optionpk<double> x_opt("x","x","gcp x coordinate)");
+ Optionpk<double> y_opt("y","y","gcp y coordinate)");
+ Optionpk<double> z_opt("z","z","gcp z coordinate in m)");
+ Optionpk<double> errorZ_opt("ez","ez","offset (error) in z coordinate of the
GCP",0);
+ Optionpk<int> col_opt("c","col","column of the pixel on CCD");
+ Optionpk<int> row_opt("r","row","row of the pixel on CCD");
+ Optionpk<double> roll_opt("roll","roll","platform attitude roll");
+ Optionpk<double> pitch_opt("pitch","pitch","platform attitude pitch");
+ Optionpk<double> yaw_opt("yaw","yaw","platform attitude yaw");
+ Optionpk<double> xl_opt("xl","xl","platform x position in epsg:4326");
+ Optionpk<double> yl_opt("yl","yl","platform y position in epsg:4326");
+ Optionpk<double> zl_opt("zl","zl","platform z position (in m)");
+ // Optionpk<double> bcpos_opt("bcp","bcp","bore sight offset position",0);
+ Optionpk<double> bcatt_opt("bca","bca","bore sight attitude calibration
offset for roll, pitch, yaw of the platform",0);
+ Optionpk<double> nx_opt("nx","nx","number of columns in CCD",1441);
+ Optionpk<double> ny_opt("ny","ny","number of rows in CCD",1);
+ Optionpk<double> fov_opt("fov","fov","field of view (in degrees)",39.74584);
+ Optionpk<double> ppx_opt("ppx","ppx","scan line principal point in x",711);
+ Optionpk<double> ppy_opt("ppy","ppy","scan line principal point in y",0);
+ Optionpk<char> fs_opt("fs","fs","field separator.",' ');
+ Optionpk<string> output_opt("o", "output", "Output ascii file (empty: use
stdout");
+ Optionpk<short> sensor_opt("s", "sensor", "Sensor type (0: whiskbroom, 1:
pushbroom, 2: frame, 3: apex (predefined settings))",0);
+ Optionpk<double> polynome_opt("pol","polynome","coefficients for polynome to
correct accross track");
+ Optionpk<bool> mean_opt("m","mean","calculate mean error",false);
+ Optionpk<bool> optimize_opt("opt","opt","optimize bore sight angles using
GCP points in input file",false);
+ Optionpk<double> lb_opt("lb","lb","lower bounds for offset bore sight
angles: use -lb offset_roll -lb offset_pitch -lb offset_yaw",0);
+ Optionpk<double> ub_opt("ub","ub","upper bounds for offset bore sight
angles: use -ub offset_roll -ub offset_pitch -ub offset_yaw",0);
+ Optionpk<unsigned int> maxit_opt("maxit","maxit","maximum number of
iterations",500);
+ Optionpk<double> tolerance_opt("tol","tolerance","relative tolerance for
stopping criterion",0.0001);
+ Optionpk<double> init_opt("is","init","initial state vector for bore sight
angles: -is init_roll -is init_pitch -is init_yaw",0);
+ Optionpk<double> threshold_opt("t","t","threshold for GCP error (in m)",0);
+ Optionpk<bool> gcprad_opt("gcprad","gcprad","gcp coordinates",false);
+ Optionpk<bool> pplrad_opt("prad","prad","platform pos coordinates",false);
+ Optionpk<bool> aplrad_opt("arad","arad","platform attitude angles",false);
+ Optionpk<bool> bcrad_opt("brad","brad","boresight attitude angles",false);
+ Optionpk<bool> getzenith_opt("gz","getzenith","get zenith angle from
platform",false);
+ Optionpk<short> verbose_opt("v", "verbose", "verbose mode when > 0", 0);
+
+ version_opt.retrieveOption(argc,argv);
+ license_opt.retrieveOption(argc,argv);
+ help_opt.retrieveOption(argc,argv);
+ todo_opt.retrieveOption(argc,argv);
+ input_opt.retrieveOption(argc,argv);
+ datum_opt.retrieveOption(argc,argv);
+ s_srs_opt.retrieveOption(argc,argv);
+ t_srs_opt.retrieveOption(argc,argv);
+ focal_opt.retrieveOption(argc,argv);
+ dx_opt.retrieveOption(argc,argv);
+ dy_opt.retrieveOption(argc,argv);
+ nx_opt.retrieveOption(argc,argv);
+ ny_opt.retrieveOption(argc,argv);
+ x_opt.retrieveOption(argc,argv);
+ y_opt.retrieveOption(argc,argv);
+ z_opt.retrieveOption(argc,argv);
+ errorZ_opt.retrieveOption(argc,argv);
+ xl_opt.retrieveOption(argc,argv);
+ yl_opt.retrieveOption(argc,argv);
+ zl_opt.retrieveOption(argc,argv);
+ roll_opt.retrieveOption(argc,argv);
+ pitch_opt.retrieveOption(argc,argv);
+ yaw_opt.retrieveOption(argc,argv);
+ col_opt.retrieveOption(argc,argv);
+ row_opt.retrieveOption(argc,argv);
+ // bcpos_opt.retrieveOption(argc,argv);
+ bcatt_opt.retrieveOption(argc,argv);
+ fov_opt.retrieveOption(argc,argv);
+ ppx_opt.retrieveOption(argc,argv);
+ ppy_opt.retrieveOption(argc,argv);
+ fs_opt.retrieveOption(argc,argv);
+ output_opt.retrieveOption(argc,argv);
+ sensor_opt.retrieveOption(argc,argv);
+ polynome_opt.retrieveOption(argc,argv);
+ mean_opt.retrieveOption(argc,argv);
+ optimize_opt.retrieveOption(argc,argv);
+ lb_opt.retrieveOption(argc,argv);
+ ub_opt.retrieveOption(argc,argv);
+ maxit_opt.retrieveOption(argc,argv);
+ tolerance_opt.retrieveOption(argc,argv);
+ init_opt.retrieveOption(argc,argv);
+ threshold_opt.retrieveOption(argc,argv);
+ gcprad_opt.retrieveOption(argc,argv);
+ pplrad_opt.retrieveOption(argc,argv);
+ aplrad_opt.retrieveOption(argc,argv);
+ bcrad_opt.retrieveOption(argc,argv);
+ getzenith_opt.retrieveOption(argc,argv);
+ verbose_opt.retrieveOption(argc,argv);
+
+ if(version_opt[0]){
+ cout << version_opt.getHelp() << endl;
+ exit(0);
+ }
+ if(license_opt[0]){
+ cout << Optionpk<bool>::getGPLv3License() << endl;
+ exit(0);
+ }
+
+ if(help_opt[0]){
+ cout << "usage: pksensormodel -i asciifile [OPTIONS]" << endl;
+ exit(0);
+ }
+
+ //ID, row of GCP, column of GCP, x_GCP, y_GCP, Z_GCP, x_platform,
y_platform, z_platform, platform roll, platform pitch, platform yaw of point.
+ vector<int> vID;
+ SensorModel::SensorModel theModel;
+
+ switch(sensor_opt[0]){
+ case(0):
+ theModel.setModel(SensorModel::WHISKBROOM);
+ break;
+ case(1):
+ theModel.setModel(SensorModel::PUSHBROOM);
+ break;
+ case(2):
+ theModel.setModel(SensorModel::FRAME);
+ break;
+ case(3)://APEX
+ theModel.setModel(SensorModel::WHISKBROOM);
+ fov_opt[0]=27.984271234;
+ nx_opt[0]=1000;
+ ny_opt[0]=1;
+ dx_opt[0]=40;
+ dy_opt[0]=0;
+ focal_opt[0]=0.08103;
+ ppx_opt[0]=498;
+ ppy_opt[0]=0;
+ break;
+ default:
+ std::cerr << "Error: sensor type " << sensor_opt[0] << " not supported" <<
std::endl;
+ exit(0);
+ break;
+ }
+ theModel.setFOV(fov_opt[0]);
+ theModel.setNcol(nx_opt[0]);
+ theModel.setNrow(ny_opt[0]);
+ theModel.setDx(dx_opt[0]);
+ theModel.setDy(dy_opt[0]);
+ theModel.setF(focal_opt[0]);
+ theModel.setPPx(ppx_opt[0]);
+ theModel.setPPy(ppy_opt[0]);
+ theModel.setPolynome(polynome_opt);
+ theModel.setDatum(datum_opt[0]);
+ // gsl::vector bc_pos(3);
+ gsl::vector bc_att(3);
+ // while(bcpos_opt.size()<3)
+ // bcpos_opt.push_back(bcpos_opt[0]);
+ while(bcatt_opt.size()<3)
+ bcatt_opt.push_back(bcatt_opt[0]);
+ if(bcrad_opt[0]){
+ // bc_pos[0]=theModel.rad2deg(bcpos_opt[0]);
+ // bc_pos[1]=theModel.rad2deg(bcpos_opt[1]);
+ bc_att[0]=theModel.rad2deg(bcatt_opt[0]);
+ bc_att[1]=theModel.rad2deg(bcatt_opt[1]);
+ bc_att[2]=theModel.rad2deg(bcatt_opt[2]);
+ }
+ else{
+ // bc_pos[0]=bcpos_opt[0];
+ // bc_pos[1]=bcpos_opt[1];
+ bc_att[0]=bcatt_opt[0];
+ bc_att[1]=bcatt_opt[1];
+ bc_att[2]=bcatt_opt[2];
+ }
+ // bc_pos[2]=bcpos_opt[2];
+ // theModel.setBoresightPos(bc_pos);
+ theModel.setBoresightAtt(bc_att);
+ if(verbose_opt[0]>1){
+ // std::cout << "bore sight position offset: " <<
theModel.getBoresightPos() << std::endl;
+ std::cout << "bore sight attitude offset: " << theModel.getBoresightAtt()
<< std::endl;
+ }
+ DataModel theDataModel(theModel);
+ theDataModel.setThreshold(threshold_opt[0]);
+
+ ifstream dataFile;
+
+ int nrow=0;
+ int ivalue=0;
+ double dvalue=0;
+
+ OGRSpatialReference oSourceSRS, oTargetSRS;
+ OGRCoordinateTransformation *poCT;
+ //input for sensor model should be universal lat lon (epsg:4326)
+ oSourceSRS.importFromEPSG(s_srs_opt[0]);
+ oTargetSRS.importFromEPSG(4326);
+ poCT = OGRCreateCoordinateTransformation( &oSourceSRS,
+ &oTargetSRS );
+
+ //frame ID, row, column, GCP X, GCP Y, GCP Z, platform X, platformY,
platform Z, platform roll, platform pitch, platform yaw.
+ for(int ifile=0;ifile<input_opt.size();++ifile){
+ if(verbose_opt[0]>1)
+ std::cout << "opening file " << input_opt[ifile] << std::endl;
+ dataFile.open(input_opt[ifile].c_str());
+ assert(dataFile);
+ if(fs_opt[0]>' '&&fs_opt[0]<='~'){//field separator is a regular character
(minimum ASCII code is space, maximum ASCII code is tilde)
+ string csvRecord;
+ while(getline(dataFile,csvRecord)){//read a line
+ istringstream csvstream(csvRecord);
+ string item;
+ int icol=0;//first column is id
+ gsl::vector thePosGCP(3);
+ gsl::vector thePosPlatform(3);
+ gsl::vector theAttPlatform(3);
+ while(getline(csvstream,item,fs_opt[0])){//read a column
+ if(verbose_opt[0]>1)
+ std::cout << item << " ";
+ switch(icol){
+ case(0)://id
+ ivalue=atoi(item.c_str());
+ vID.push_back(ivalue);
+ break;
+ case(1)://row
+ ivalue=atoi(item.c_str());
+ theDataModel.pushRow(ivalue);
+ break;
+ case(2)://col
+ ivalue=atoi(item.c_str());
+ theDataModel.pushCol(ivalue);
+ break;
+ case(3)://X
+ dvalue=atof(item.c_str());
+ if(gcprad_opt[0])
+ thePosGCP[0]=theModel.rad2deg(dvalue);
+ else
+ thePosGCP[0]=dvalue;
+ break;
+ case(4)://Y
+ dvalue=atof(item.c_str());
+ if(gcprad_opt[0])
+ thePosGCP[1]=theModel.rad2deg(dvalue);
+ else
+ thePosGCP[1]=dvalue;
+ break;
+ case(5)://Z
+ dvalue=atof(item.c_str());
+ thePosGCP[2]=dvalue;
+ break;
+ case(6)://XL
+ dvalue=atof(item.c_str());
+ if(pplrad_opt[0])
+ thePosPlatform[0]=theModel.rad2deg(dvalue);
+ else
+ thePosPlatform[0]=dvalue;
+ break;
+ case(7)://YL
+ dvalue=atof(item.c_str());
+ if(pplrad_opt[0])
+ thePosPlatform[1]=theModel.rad2deg(dvalue);
+ else
+ thePosPlatform[1]=dvalue;
+ break;
+ case(8)://ZL
+ dvalue=atof(item.c_str());
+ thePosPlatform[2]=dvalue;
+ break;
+ case(9)://Roll
+ dvalue=atof(item.c_str());
+ if(aplrad_opt[0])
+ theAttPlatform[0]=theModel.rad2deg(dvalue);
+ else
+ theAttPlatform[0]=dvalue;
+ break;
+ case(10)://Pitch
+ dvalue=atof(item.c_str());
+ if(aplrad_opt[0])
+ theAttPlatform[1]=theModel.rad2deg(dvalue);
+ else
+ theAttPlatform[1]=dvalue;
+ break;
+ case(11)://Yaw
+ dvalue=atof(item.c_str());
+ if(aplrad_opt[0])
+ theAttPlatform[2]=theModel.rad2deg(dvalue);
+ else
+ theAttPlatform[2]=dvalue;
+ break;
+ case(12)://track
+ break;
+ default:
+ std::cerr << "Error: too many columns in input file " <<
input_opt[ifile] << std::endl;
+ break;
+ }
+ ++icol;
+ }
+ if(s_srs_opt[0]!=4326){
+ if(verbose_opt[0]>1)
+ std::cout << "transforming: " << thePosGCP[0] << ", " <<
thePosGCP[1] << ", " << thePosGCP[2] << std::endl;
+ poCT->Transform(1,&(thePosGCP[0]),&(thePosGCP[1]),&(thePosGCP[2]));
+ if(verbose_opt[0]>1)
+ std::cout << "into: " << thePosGCP[0] << ", " << thePosGCP[1] <<
", " << thePosGCP[2] << std::endl;
+ //
poCT->Transform(1,&(thePosPlatform[0]),&(thePosPlatform[1]),&(thePosPlatform[2]));
+ }
+ theDataModel.pushPosGCP(thePosGCP);
+ theDataModel.pushPosPlatform(thePosPlatform);
+ theDataModel.pushAttPlatform(theAttPlatform);
+ if(verbose_opt[0]>1)
+ std::cout << endl;
+ ++nrow;
+ }
+ }//comma separated value (or ASCII character other than space or TAB)
+ else{//space or tab delimited fields
+ string spaceRecord;
+ while(!getline(dataFile, spaceRecord).eof()){
+ if(verbose_opt[0]>1)
+ std::cout << spaceRecord << std::endl;
+ istringstream lineStream(spaceRecord);
+ string item;
+ int icol=0;
+ gsl::vector thePosGCP(3);
+ gsl::vector thePosPlatform(3);
+ gsl::vector theAttPlatform(3);
+ while(lineStream >> item){
+ if(verbose_opt[0]>1)
+ std::cout << item << " ";
+ istringstream itemStream(item);
+ switch(icol){
+ case(0)://id
+ itemStream >> ivalue;
+ vID.push_back(ivalue);
+ break;
+ case(1)://row
+ itemStream >> ivalue;
+ theDataModel.pushRow(ivalue);
+ break;
+ case(2)://col
+ itemStream >> ivalue;
+ theDataModel.pushCol(ivalue);
+ break;
+ case(3)://X
+ itemStream >> dvalue;
+ if(gcprad_opt[0])
+ thePosGCP[0]=theModel.rad2deg(dvalue);
+ else
+ thePosGCP[0]=dvalue;
+ break;
+ case(4)://Y
+ itemStream >> dvalue;
+ if(gcprad_opt[0])
+ thePosGCP[1]=theModel.rad2deg(dvalue);
+ else
+ thePosGCP[1]=dvalue;
+ break;
+ case(5)://Z
+ itemStream >> dvalue;
+ thePosGCP[2]=dvalue;
+ break;
+ case(6)://XL
+ itemStream >> dvalue;
+ if(pplrad_opt[0])
+ thePosPlatform[0]=theModel.rad2deg(dvalue);
+ else
+ thePosPlatform[0]=dvalue;
+ break;
+ case(7)://YL
+ itemStream >> dvalue;
+ if(pplrad_opt[0])
+ thePosPlatform[1]=theModel.rad2deg(dvalue);
+ else
+ thePosPlatform[1]=dvalue;
+ break;
+ case(8)://ZL
+ itemStream >> dvalue;
+ thePosPlatform[2]=dvalue;
+ break;
+ case(9)://Roll
+ itemStream >> dvalue;
+ if(aplrad_opt[0])
+ theAttPlatform[0]=theModel.rad2deg(dvalue);
+ else
+ theAttPlatform[0]=dvalue;
+ break;
+ case(10)://Pitch
+ itemStream >> dvalue;
+ if(aplrad_opt[0])
+ theAttPlatform[1]=theModel.rad2deg(dvalue);
+ else
+ theAttPlatform[1]=dvalue;
+ break;
+ case(11)://Yaw
+ itemStream >> dvalue;
+ if(aplrad_opt[0])
+ theAttPlatform[2]=theModel.rad2deg(dvalue);
+ else
+ theAttPlatform[2]=dvalue;
+ break;
+ case(12)://track
+ break;
+ default:
+ std::cerr << "Error: too many columns in input file " <<
input_opt[ifile] << std::endl;
+ break;
+ }
+ ++icol;
+ }
+ if(s_srs_opt[0]!=4326){
+ if(verbose_opt[0]>1)
+ std::cout << "transforming: " << thePosGCP[0] << ", " <<
thePosGCP[1] << ", " << thePosGCP[2] << std::endl;
+ poCT->Transform(1,&(thePosGCP[0]),&(thePosGCP[1]),&(thePosGCP[2]));
+ if(verbose_opt[0]>1)
+ std::cout << "into: " << thePosGCP[0] << ", " << thePosGCP[1] <<
", " << thePosGCP[2] << std::endl;
+ //
poCT->Transform(1,&(thePosPlatform[0]),&(thePosPlatform[1]),&(thePosPlatform[2]));
+ }
+ theDataModel.pushPosGCP(thePosGCP);
+ theDataModel.pushPosPlatform(thePosPlatform);
+ theDataModel.pushAttPlatform(theAttPlatform);
+ if(verbose_opt[0]>1)
+ std::cout << std::endl;
+ if(verbose_opt[0]>1)
+ std::cout << "number of columns: " << icol << std::endl;
+ ++nrow;
+ }
+ }
+ //todo: assert sizes are all equal...
+ dataFile.close();
+ }//for ifile
+
+ for(int ipoint=0;ipoint<xl_opt.size();++ipoint){
+ if(verbose_opt[0]>1)
+ std::cout << "ipoint: " << ipoint << std::endl;
+ vID.push_back(ipoint);
+ gsl::vector thePosPlatform(3);
+ gsl::vector theAttPlatform(3);
+ assert(row_opt.size()>ipoint);
+ assert(col_opt.size()>ipoint);
+ theDataModel.pushRow(row_opt[ipoint]);
+ theDataModel.pushCol(col_opt[ipoint]);
+ if(x_opt.size()>ipoint){
+ assert(y_opt.size()>ipoint);
+ assert(z_opt.size()>ipoint);
+ gsl::vector thePosGCP(3);
+ if(gcprad_opt[0]){
+ thePosGCP[0]=theModel.rad2deg(x_opt[ipoint]);
+ thePosGCP[1]=theModel.rad2deg(y_opt[ipoint]);
+ }
+ else{
+ thePosGCP[0]=x_opt[ipoint];
+ thePosGCP[1]=y_opt[ipoint];
+ }
+ thePosGCP[2]=z_opt[ipoint];
+ if(s_srs_opt[0]!=4326)
+ poCT->Transform(1,&(thePosGCP[0]),&(thePosGCP[1]),&(thePosGCP[2]));
+ theDataModel.pushPosGCP(thePosGCP);
+ }
+ assert(yl_opt.size()>ipoint);
+ assert(zl_opt.size()>ipoint);
+ if(pplrad_opt[0]){
+ thePosPlatform[0]=theModel.rad2deg(xl_opt[ipoint]);
+ thePosPlatform[1]=theModel.rad2deg(yl_opt[ipoint]);
+ }
+ else{
+ thePosPlatform[0]=xl_opt[ipoint];
+ thePosPlatform[1]=yl_opt[ipoint];
+ }
+ thePosPlatform[2]=zl_opt[ipoint];
+ // if(s_srs_opt[0]!=4326)
+ //
poCT->Transform(1,&(thePosPlatform[0]),&(thePosPlatform[1]),&(thePosPlatform[2]));
+ theDataModel.pushPosPlatform(thePosPlatform);
+
+ assert(roll_opt.size()>ipoint);
+ assert(pitch_opt.size()>ipoint);
+ assert(yaw_opt.size()>ipoint);
+ if(aplrad_opt[0]){
+ theAttPlatform[0]=theModel.rad2deg(roll_opt[ipoint]);
+ theAttPlatform[1]=theModel.rad2deg(pitch_opt[ipoint]);
+ theAttPlatform[2]=theModel.rad2deg(yaw_opt[ipoint]);
+ }
+ else{
+ theAttPlatform[0]=roll_opt[ipoint];
+ theAttPlatform[1]=pitch_opt[ipoint];
+ theAttPlatform[2]=yaw_opt[ipoint];
+ }
+ theDataModel.pushAttPlatform(theAttPlatform);
+ }
+
+ //todo: remove GCP points with error above threshold
+ if(threshold_opt[0]>0){
+ for(int ipoint=0;ipoint<vID.size();++ipoint){
+ if(verbose_opt[0]>1)
+ std::cout << "point: " << ipoint << std::endl;
+ gsl::vector pos_platform=theDataModel.getPosPlatform(ipoint);
+ gsl::vector att_platform=theDataModel.getAttPlatform(ipoint);
+ gsl::vector pos_calc=theDataModel.getPos(ipoint);
+ gsl::vector pos_gcp=theDataModel.getPosGCP(ipoint);
+ ostringstream gcpss;
+ double e=theDataModel.getModel().getDistGeo(pos_gcp,pos_calc);
+ if(e>=theDataModel.getThreshold()){
+ vID.erase(vID.begin()+ipoint);
+ theDataModel.erase(ipoint);
+ }
+ }
+ if(!theDataModel.getSize())
+ std::cerr << "Error: no data after filtering, check if input is correcly
set (rad or degrees) or try to lower threshold" << std::endl;
+ }
+
+ if(optimize_opt[0]){
+ while(lb_opt.size()<3)
+ lb_opt.push_back(lb_opt[0]);
+ while(ub_opt.size()<3)
+ ub_opt.push_back(ub_opt[0]);
+ while(init_opt.size()<3)
+ init_opt.push_back(init_opt[0]);
+ //todo: make nlopt::LN_COBYLA as a command line option
+ //nlopt::opt opt(nlopt::LN_COBYLA,4*input_opt.size());//k,e,a,haze
+ nlopt::opt optimizer(nlopt::LN_SBPLX,3);//bore sight angles
+
+ optimizer.set_min_objective(objFunction, &theDataModel);
+
+ if(verbose_opt[0]>1)
+ std::cout << "set lower and upper bounds" << std::endl;
+ assert(lb_opt.size()==ub_opt.size());
+ for(int index=0;index<lb_opt.size();++index){
+ if(bcrad_opt[0]){
+ lb_opt[index]=theModel.rad2deg(lb_opt[index]);
+ ub_opt[index]=theModel.rad2deg(ub_opt[index]);
+ }
+ }
+ optimizer.set_lower_bounds(lb_opt);
+ optimizer.set_upper_bounds(ub_opt);
+
+ if(verbose_opt[0]>1)
+ std::cout << "set stopping criteria" << std::endl;
+ //set stopping criteria
+ if(maxit_opt[0])
+ optimizer.set_maxeval(maxit_opt[0]);
+ else
+ optimizer.set_xtol_rel(tolerance_opt[0]);
+ double minf=0;
+ std::vector<double> x=init_opt;
+
+ optimizer.optimize(x, minf);
+ if(verbose_opt[0]){
+ std::cout << "optimized with " << optimizer.get_algorithm_name() <<
"..." << std::endl;
+ for(int index=0;index<x.size();++index){
+ if(bcrad_opt[0])
+ std::cout << setprecision(12) << " -bca " <<
theModel.deg2rad(x[index]);
+ else
+ std::cout << setprecision(12) << " -bca " << x[index];
+ }
+ std::cout << std::endl;
+ }
+ }
+
+ double posX=0;
+ double posY=0;
+ ofstream outputStream;
+ if(output_opt.size()){
+ if(verbose_opt[0]>1)
+ std::cout << "opening output file: " << output_opt[0] << std::endl;
+ outputStream.open(output_opt[0].c_str());
+ }
+ Histogram hist;
+ vector<double> errorv;
+
+ oSourceSRS.importFromEPSG(4326);
+ oTargetSRS.importFromEPSG(t_srs_opt[0]);
+ poCT = OGRCreateCoordinateTransformation( &oSourceSRS,
+ &oTargetSRS );
+ for(int ipoint=0;ipoint<vID.size();++ipoint){
+ if(verbose_opt[0]>1)
+ std::cout << "point: " << ipoint << std::endl;
+ gsl::vector pos_platform=theDataModel.getPosPlatform(ipoint);
+ gsl::vector att_platform=theDataModel.getAttPlatform(ipoint);
+
+ gsl::vector pos_calc;
+ gsl::vector pos_gcp;
+ if(theDataModel.getSize())
+ pos_gcp=theDataModel.getPosGCP(ipoint);
+ else
+ pos_gcp=theDataModel.getPos(ipoint);
+ if(errorZ_opt[0]!=0)
+
pos_calc=theDataModel.getModel().getPos(pos_platform,att_platform,theDataModel.getRow(ipoint),theDataModel.getCol(ipoint),theDataModel.getHeight(ipoint)+errorZ_opt[0]);
+ else
+ pos_calc=theDataModel.getPos(ipoint);
+ double e=theDataModel.getModel().getDistGeo(pos_gcp,pos_calc);
+ if(theDataModel.getThreshold()&&e>theDataModel.getThreshold())
+ continue;
+ errorv.push_back(e);
+
+ ostringstream gcpss;
+ gcpss.precision(12);
+ poCT->Transform(1,&(pos_gcp[0]),&(pos_gcp[1]),&(pos_gcp[2]));
+ gcpss << pos_gcp[0] << " " << pos_gcp[1] << " " << pos_gcp[2] << " ";
+ gcpss << errorv.back() << " ";
+
+ poCT->Transform(1,&(pos_calc[0]),&(pos_calc[1]),&(pos_calc[2]));
+
+ if(output_opt.size()){
+ outputStream << setprecision(12) << vID[ipoint] << " " <<
theDataModel.getRow(ipoint) << " " << theDataModel.getCol(ipoint) << " " <<
pos_calc[0] << " " << pos_calc[1] << " " << pos_calc[2] << " " << gcpss.str();
+ if(getzenith_opt[0])
+ outputStream << " " << theDataModel.getModel().getZenith(att_platform);
+ outputStream << std::endl;
+ }
+ else{
+ std::cout << setprecision(12) << vID[ipoint] << " " <<
theDataModel.getRow(ipoint) << " " << theDataModel.getCol(ipoint) << " " <<
pos_calc[0] << " " << pos_calc[1] << " " << pos_calc[2] << " " << gcpss.str();
+ if(getzenith_opt[0])
+ std::cout << " " << theDataModel.getModel().getZenith(att_platform);
+ std::cout << std::endl;
+ }
+ }
+ if(mean_opt[0]){
+ double theMean=0;
+ double theVar=0;
+ hist.meanVar(errorv,theMean,theVar);
+ if(output_opt.size())
+ outputStream << setprecision(12) << "mean stddev: " << theMean << " " <<
sqrt(theVar) << std::endl;
+ else
+ std::cout << setprecision(12) << "mean stdev: " << theMean << " " <<
sqrt(theVar) << std::endl;
+ }
+ if(output_opt.size())
+ outputStream.close();
+}
diff --git a/src/apps/pksensormodel.h b/src/apps/pksensormodel.h
new file mode 100644
index 0000000..a9d7e8c
--- /dev/null
+++ b/src/apps/pksensormodel.h
@@ -0,0 +1,81 @@
+/**********************************************************************
+pksensormodel.h: program to calculate geometric position based on row
(sensor), col (sensor), roll, pitch, yaw and lens coordinates
+Copyright (C) 2008-2012 Pieter Kempeneers
+
+This file is part of pktools
+
+pktools is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+pktools is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with pktools. If not, see <http://www.gnu.org/licenses/>.
+***********************************************************************/
+#ifndef _PKSENSORMODEL_H_
+#define _PKSENSORMODEL_H_
+#include <vector>
+#include <gslwrap/matrix_double.h>
+#include "algorithms/SensorModel.h"
+
+double objFunction(const std::vector<double> &x, std::vector<double> &grad,
void *my_func_data);
+
+class DataModel{
+ public:
+ DataModel() : m_threshold(0){};
+ DataModel(const SensorModel::SensorModel& theModel) : m_model(theModel),
m_threshold(0){};
+ ~DataModel(){};
+ void setModel(const SensorModel::SensorModel& theModel){m_model=theModel;};
+ const SensorModel::SensorModel& getModel() const {return m_model;};
+ int getSize() const{return m_posGCP.size();};
+ void setThreshold(double theThreshold){m_threshold=theThreshold;};
+ double getThreshold(){return m_threshold;};
+ int erase(int index){
+ m_attPlatform.erase(m_attPlatform.begin()+index);
+ m_posPlatform.erase(m_posPlatform.begin()+index);
+ m_posGCP.erase(m_posGCP.begin()+index);
+ m_row.erase(m_row.begin()+index);
+ m_col.erase(m_col.begin()+index);
+ };
+ int pushAttPlatform(const gsl::vector& atp){m_attPlatform.push_back(atp);
return m_attPlatform.size();};
+ int pushPosPlatform(const gsl::vector& ppl){m_posPlatform.push_back(ppl);
return m_posPlatform.size();};
+ int pushPosGCP(const gsl::vector& pgcp){m_posGCP.push_back(pgcp); return
m_posGCP.size();};
+ int pushRow(int r){m_row.push_back(r); return m_row.size();};
+ int pushCol(int c){m_col.push_back(c); return m_col.size();};
+ gsl::vector getPosPlatform(int index)
const{assert(index>=0);assert(index<m_posPlatform.size());return(m_posPlatform[index]);};
+ gsl::vector getAttPlatform(int index)
const{assert(index>=0);assert(index<m_attPlatform.size());return(m_attPlatform[index]);};
+ gsl::vector getPosGCP(int index)
const{assert(index>=0);assert(index<m_posGCP.size());return(m_posGCP[index]);};
+ gsl::vector getPos(int index) const{
+ assert(index>=0);
+ assert(index<m_posPlatform.size());
+ assert(index<m_attPlatform.size());
+ assert(index<m_row.size());
+ assert(index<m_col.size());
+ assert(index<m_posGCP.size());
+
return(m_model.getPos(m_posPlatform[index],m_attPlatform[index],m_row[index],m_col[index],m_posGCP[index][2]));
+ };
+ double getDistGeo(int index)
const{assert(index>=0);assert(index<m_posGCP.size());return(m_model.getDistGeo(m_posGCP[index],getPos(index)));};
+ int getRow(int index)
const{assert(index>=0);assert(index<m_row.size());return(m_row[index]);};
+ int getCol(int index)
const{assert(index>=0);assert(index<m_col.size());return(m_col[index]);};
+ double getHeight(int index)
const{assert(index>=0);assert(index<m_posGCP.size());return(m_posGCP[index][2]);};
+ void setBoresightAtt(const gsl::vector& bc_att){
+ m_model.setBoresightAtt(bc_att);
+ // for(int index=0;index<m_attPlatform.size();++index)
+ // m_attPlatform[index]+=bc_att;
+ };
+ private:
+ SensorModel::SensorModel m_model;
+ vector<gsl::vector> m_posPlatform;
+ vector<gsl::vector> m_posGCP;
+ vector<gsl::vector> m_attPlatform;
+ vector<int> m_row;
+ vector<int> m_col;
+ double m_threshold;
+};
+
+#endif //_PKSENSORMODEL_H_
--
Alioth's /usr/local/bin/git-commit-notice on
/srv/git.debian.org/git/pkg-grass/pktools.git
_______________________________________________
Pkg-grass-devel mailing list
Pkg-grass-devel@lists.alioth.debian.org
http://lists.alioth.debian.org/cgi-bin/mailman/listinfo/pkg-grass-devel
