"Fossies" - the Fresh Open Source Software Archive

Member "gama-2.05/lib/gnu_gama/g3/g3_model_linearization.cpp" (10 May 2019, 16773 Bytes) of package /linux/privat/gama-2.05.tar.gz:


As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style: standard) with prefixed line numbers and code folding option. Alternatively you can here view or download the uninterpreted source code file. For more information about "g3_model_linearization.cpp" see the Fossies "Dox" file reference documentation.

    1 /*
    2     GNU Gama -- adjustment of geodetic networks
    3     Copyright (C) 2005  Ales Cepek <cepek@gnu.org>
    4 
    5     This file is part of the GNU Gama C++ library.
    6 
    7     This library is free software; you can redistribute it and/or modify
    8     it under the terms of the GNU General Public License as published by
    9     the Free Software Foundation; either version 3 of the License, or
   10     (at your option) any later version.
   11 
   12     This library is distributed in the hope that it will be useful,
   13     but WITHOUT ANY WARRANTY; without even the implied warranty of
   14     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   15     GNU General Public License for more details.
   16 
   17     You should have received a copy of the GNU General Public License
   18     along with this library; if not, write to the Free Software
   19     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
   20 */
   21 
   22 #include <gnu_gama/g3/g3_model.h>
   23 #include <gnu_gama/g3/g3_cluster.h>
   24 #include <gnu_gama/outstream.h>
   25 #include <gnu_gama/adj/adj.h>
   26 #include <gnu_gama/sparse/smatrix_graph.h>
   27 #include <iomanip>
   28 
   29 
   30 using namespace std;
   31 using namespace GNU_gama::g3;
   32 
   33 
   34 namespace
   35 {
   36   class Linearization :
   37     public GNU_gama::BaseVisitor,
   38     public GNU_gama::Visitor<Angle>,
   39     public GNU_gama::Visitor<Azimuth>,
   40     public GNU_gama::Visitor<Distance>,
   41     public GNU_gama::Visitor<Height>,
   42     public GNU_gama::Visitor<HeightDiff>,
   43     public GNU_gama::Visitor<Vector>,
   44     public GNU_gama::Visitor<XYZ>,
   45     public GNU_gama::Visitor<ZenithAngle>
   46   {
   47   public:
   48 
   49     Linearization(GNU_gama::g3::Model* m) : model(m) {}
   50 
   51     void visit(Angle* p)
   52     {
   53       model->linearization(p);
   54     }
   55     void visit(Azimuth* p)
   56     {
   57       model->linearization(p);
   58     }
   59     void visit(Distance* p)
   60     {
   61       model->linearization(p);
   62     }
   63     void visit(Height* p)
   64     {
   65       model->linearization(p);
   66     }
   67     void visit(HeightDiff* p)
   68     {
   69       model->linearization(p);
   70     }
   71     void visit(Vector* p)
   72     {
   73       model->linearization(p);
   74     }
   75     void visit(XYZ* p)
   76     {
   77       model->linearization(p);
   78     }
   79     void visit(ZenithAngle* p)
   80     {
   81       model->linearization(p);
   82     }
   83 
   84   private:
   85 
   86     GNU_gama::g3::Model* model;
   87 
   88   };
   89 }
   90 
   91 
   92 
   93 void GNU_gama::g3::Model::update_linearization()
   94 {
   95   do
   96     {
   97       if (!check_observations()) update_observations();
   98 
   99       if (adj_input_data) delete adj_input_data;
  100       if (A             ) delete A;
  101 
  102       adj_input_data = new AdjInputData;
  103       A              = new SparseMatrix<>(dm_floats, dm_rows, dm_cols);
  104 
  105       rhs.reset(dm_rows);
  106       rhs_ind = 0;
  107 
  108       Linearization linearization(this);
  109       for (ObservationList::iterator
  110              i=active_obs->begin(), e=active_obs->end(); i!=e; ++i)
  111         {
  112           (*i)->accept(&linearization);
  113         }
  114 
  115     } while (!check_observations());
  116 
  117   if (dm_floats * dm_rows * dm_cols == 0)
  118     throw GNU_gama::Exception::string("No parameters and/or observations");
  119 
  120   adj_input_data->set_mat(A);
  121   adj_input_data->set_rhs(rhs);
  122 
  123   {
  124     GNU_gama::SparseMatrixGraph<> graph(A);
  125 
  126     dm_graph_is_connected = graph.connected();
  127   }
  128 
  129   {
  130     int minx=0;
  131     for (ParameterList::const_iterator
  132            i=par_list->begin(), e=par_list->end(); i!=e; ++i)
  133       {
  134         if ((*i)->constr()) minx++;
  135       }
  136 
  137     if (minx)
  138       {
  139         GNU_gama::IntegerList<>* minlist = new GNU_gama::IntegerList<>(minx);
  140         GNU_gama::IntegerList<>::iterator m = minlist->begin();
  141         for (ParameterList::const_iterator
  142                i=par_list->begin(), e=par_list->end(); i!=e; ++i)
  143           {
  144             const Parameter* p = *i;
  145             if (p->constr())
  146               {
  147                 *m = p->index();
  148                 ++m;
  149               }
  150           }
  151 
  152         adj_input_data->set_minx(minlist);
  153       }
  154   }
  155 
  156   {
  157     int nonzeroes=0, blocks=0;
  158     for (ClusterList::iterator ci = obsdata.clusters.begin(),
  159            ce = obsdata.clusters.end(); ci!=ce; ++ci)
  160       {
  161         if (int n = (*ci)->activeNonz())
  162           {
  163             nonzeroes += n;
  164             blocks++;
  165           }
  166       }
  167 
  168     GNU_gama::BlockDiagonal<>*
  169       bd = new GNU_gama::BlockDiagonal<>(blocks, nonzeroes);
  170 
  171     for (ClusterList::const_iterator ci = obsdata.clusters.begin(),
  172            ce = obsdata.clusters.end(); ci!=ce; ++ci)
  173       {
  174         CovMat<> C = (*ci)->activeCov();
  175         C /= (apriori_sd*apriori_sd);      // covariances ==> cofactors
  176         if (C.dim())
  177           {
  178             bd->add_block(C.dim(), C.bandWidth(), C.begin());
  179           }
  180       }
  181 
  182     adj_input_data->set_cov(bd);
  183   }
  184 
  185   adj->set(adj_input_data);
  186 
  187   return next_state_(linear_);
  188 }
  189 
  190 
  191 void Model::linearization(Angle* pangle)
  192 {
  193   using namespace std;
  194 
  195   Point* from  = points->find(pangle->from);
  196   Point* left  = points->find(pangle->left);
  197   Point* right = points->find(pangle->right);
  198 
  199   const double sca = Angular().scale();
  200   const double scl = Linear ().scale();
  201   const double sc  = sca / scl;
  202 
  203   E_3 FI = instrument(from,  pangle->from_dh);
  204   E_3 FL = instrument(left,  pangle->left_dh);
  205   E_3 FR = instrument(right, pangle->right_dh);
  206   E_3 FV = vertical(from);
  207   FL    -= FI;
  208   FR    -= FI;
  209   double ql  = sqrt(FL.e1*FL.e1 + FL.e2*FL.e2 + FL.e3*FL.e3);
  210   double qr  = sqrt(FR.e1*FR.e1 + FR.e2*FR.e2 + FR.e3*FR.e3);
  211   if (ql) ql = 1.0/ql;
  212   if (qr) qr = 1.0/qr;
  213   FL *= ql;
  214   FR *= qr;
  215   E_3 VL, VR;
  216   VL.cross(FV, FL);
  217   VR.cross(FV, FR);
  218 
  219 
  220   R_3 tran;
  221   tran.set_rotation(from->B(), from->L());
  222 
  223   E_3 Lxyz(left ->X.init_value() - from->X.init_value(),
  224            left ->Y.init_value() - from->Y.init_value(),
  225            left ->Z.init_value() - from->Z.init_value());
  226   E_3 Rxyz(right->X.init_value() - from->X.init_value(),
  227            right->Y.init_value() - from->Y.init_value(),
  228            right->Z.init_value() - from->Z.init_value());
  229 
  230   E_3 Lneu, Rneu;
  231   tran.inverse(Lxyz, Lneu);
  232   tran.inverse(Rxyz, Rneu);
  233 
  234   const double dl = sqrt (Lneu.e1*Lneu.e1 + Lneu.e2*Lneu.e2);
  235   const double dr = sqrt (Rneu.e1*Rneu.e1 + Rneu.e2*Rneu.e2);
  236   const double sl = atan2(Lneu.e2, Lneu.e1);
  237   const double sr = atan2(Rneu.e2, Rneu.e1);
  238 
  239   const double psl = sin(sl)/dl;
  240   const double pcl = cos(sl)/dl;
  241   const double psr = sin(sr)/dr;
  242   const double pcr = cos(sr)/dr;
  243 
  244   E_3 Lcoef(-psl,  pcl, 0.0);
  245   E_3 Rcoef(-psr,  pcr, 0.0);
  246 
  247   E_3 Fcoef( Rcoef );  Fcoef -= Lcoef;  Fcoef *= -1.0;
  248 
  249   tran.rotation(Lcoef, Lxyz);
  250   tran.rotation(Rcoef, Rxyz);
  251   tran.set_rotation(left->B(), left->L());
  252   tran.inverse(Lxyz, Lcoef);
  253   tran.set_rotation(right->B(), right->L());
  254   tran.inverse(Rxyz, Rcoef);
  255 
  256 
  257   // nonzero derivatives in project equations
  258   A->new_row();
  259 
  260   if (from->N.free()) A->add_element(Fcoef.e1*sc, from->N.index());
  261   if (from->E.free()) A->add_element(Fcoef.e2*sc, from->E.index());
  262   if (from->U.free()) A->add_element(Fcoef.e3*sc, from->U.index());
  263 
  264   if (left->N.free()) A->add_element(Lcoef.e1*sc, left->N.index());
  265   if (left->E.free()) A->add_element(Lcoef.e2*sc, left->E.index());
  266   if (left->U.free()) A->add_element(Lcoef.e3*sc, left->U.index());
  267 
  268   if (right->N.free()) A->add_element(Rcoef.e1*sc, right->N.index());
  269   if (right->E.free()) A->add_element(Rcoef.e2*sc, right->E.index());
  270   if (right->U.free()) A->add_element(Rcoef.e3*sc, right->U.index());
  271 
  272 
  273   // right hand site
  274 
  275   const double angle = GNU_gama::angle(VL, VR);
  276   rhs(++rhs_ind) = (pangle->obs() - angle)*sca;
  277 }
  278 
  279 
  280 
  281 void Model::linearization(Azimuth* a)
  282 {
  283   Point* from = points->find(a->from);
  284   Point* to   = points->find(a->to  );
  285 
  286   E_3 from_vertical, from_to, p1, v1, p2, v2;
  287 
  288   p1.set(from->X(), from->Y(), from->Z());
  289   v1 = from_vertical = vertical(from);
  290   v1 *= a->from_dh;
  291   p1 += v1;                               // instrument
  292 
  293   p2.set(to->X(), to->Y(), to->Z());
  294   v2  = vertical(to);
  295   v2 *= a->to_dh;
  296   p2 += v2;                               // target
  297 
  298   from_to  = p2;
  299   from_to -= p1;                          // instrument --> target
  300 
  301   R_3 R;
  302   R.set_rotation(from->B(), from->L());   // dif_NEU --> dif_XYZ
  303   E_3 local;
  304   R.inverse(from_to, local);
  305 
  306   // const double h = std::sqrt(local.e1*local.e1 + local.e2*local.e2);
  307 
  308   // pd - partial derivatives for the occupied station
  309   E_3 pd( std::cos(a->obs()), -std::sin(a->obs()), 0);
  310 
  311   // nonzero derivatives in project equations
  312   A->new_row();
  313   if (from->free_horizontal_position())
  314     {
  315       A->add_element(pd.e1, from->N.index());
  316       A->add_element(pd.e2, from->E.index());
  317     }
  318   // if (from->free_height())
  319   //   {
  320   //     A->add_element(pd.e3, from->U.index());
  321   //   }
  322 
  323   E_3 tmp;
  324   R.rotation(pd, tmp);
  325   tmp *= -1.0;
  326   R.set_rotation(to->B(),to->L());
  327   // pd - partial derivatives for the target station
  328   R.inverse (tmp, pd);
  329 
  330   if (to->free_horizontal_position())
  331     {
  332       A->add_element(pd.e1, to->N.index());
  333       A->add_element(pd.e2, to->E.index());
  334     }
  335   if (to->free_height())
  336     {
  337       A->add_element(pd.e3, to->U.index());
  338     }
  339 
  340 
  341   // right hand site
  342 
  343   double az = std::atan2(local.e2, local.e1);
  344   std::cout << "??? azimuth " << (a->obs()*GON_TO_RAD - az)*RAD_TO_GON
  345             << "\t" << a->obs() << "\t" << az*RAD_TO_GON
  346             << "\n";
  347   rhs(++rhs_ind) = a->obs()*GON_TO_RAD - az;
  348 }
  349 
  350 
  351 
  352 void Model::linearization(Distance* d)
  353 {
  354   Point* from = points->find(d->from);
  355   Point* to   = points->find(d->to  );
  356 
  357   {
  358     double dx = to->X() - from->X();
  359     double dy = to->Y() - from->Y();
  360     double dz = to->Z() - from->Z();
  361     if (double dd = std::sqrt(dx*dx + dy*dy + dz*dz))
  362       {
  363         dx /= dd;
  364         dy /= dd;
  365         dz /= dd;
  366       }
  367     from->set_diff_XYZ(-dx, -dy, -dz);
  368     to  ->set_diff_XYZ( dx,  dy,  dz);
  369   }
  370 
  371 
  372 
  373   // nonzero derivatives in project equations
  374   A->new_row();
  375   if (from->free_horizontal_position())
  376     {
  377       A->add_element(from->diff_N(), from->N.index());
  378       A->add_element(from->diff_E(), from->E.index());
  379     }
  380   if (from->free_height())
  381     {
  382       A->add_element(from->diff_U(), from->U.index());
  383     }
  384 
  385   if (to->free_horizontal_position())
  386     {
  387       A->add_element(to->diff_N(), to->N.index());
  388       A->add_element(to->diff_E(), to->E.index());
  389     }
  390   if (to->free_height())
  391     {
  392       A->add_element(to->diff_U(), to->U.index());
  393     }
  394 
  395 
  396 
  397   // right hand site
  398   {
  399     double dx = to->X_dh(d->to_dh) - from->X_dh(d->from_dh);
  400     double dy = to->Y_dh(d->to_dh) - from->Y_dh(d->from_dh);
  401     double dz = to->Z_dh(d->to_dh) - from->Z_dh(d->from_dh);
  402     double D  = std::sqrt(dx*dx + dy*dy + dz*dz);
  403 
  404     double rd = rhs(++rhs_ind) = (d->obs() - D)*Linear().scale();
  405 
  406     if (abs(rd) > tol_abs)
  407       {
  408         Model::Rejected robs;
  409 
  410         robs.criterion   = Model::Rejected::rhs;
  411         robs.observation = d;
  412         robs.data[0]     = rd;
  413 
  414         rejected_obs.push_back(robs);
  415         reset_parameters();
  416         d->set_active(false);
  417       }
  418   }
  419 }
  420 
  421 
  422 
  423 void Model::linearization(Height* height)
  424 {
  425   Point* point = points->find(height->id);
  426 
  427   // nonzero derivatives in project equations
  428   A->new_row();
  429 
  430   if (point->free_height())  A->add_element(1, point->U.index());
  431 
  432   // right hand site
  433 
  434   rhs(++rhs_ind) = (height->obs() - point->model_height())*Linear().scale();
  435 }
  436 
  437 
  438 
  439 void Model::linearization(HeightDiff* dh)
  440 {
  441   Point* from = points->find(dh->from);
  442   Point* to   = points->find(dh->to  );
  443 
  444   // nonzero derivatives in project equations
  445   A->new_row();
  446 
  447   if (from->free_height())  A->add_element(-1, from->U.index());
  448   if (to  ->free_height())  A->add_element(+1, to  ->U.index());
  449 
  450   // right hand site
  451 
  452   const double h = to->model_height() - from->model_height();
  453   rhs(++rhs_ind) = (dh->obs() - h)*Linear().scale();
  454 }
  455 
  456 
  457 
  458 void Model::linearization(Vector* v)
  459 {
  460   Point* from = points->find(v->from);
  461   Point* to   = points->find(v->to  );
  462 
  463   for (int i=1; i<=3; i++)
  464    {
  465      double tx=0, ty=0, tz=0;
  466      switch (i) {
  467      case 1: tx = 1.0; break;
  468      case 2: ty = 1.0; break;
  469      case 3: tz = 1.0; break;
  470      };
  471 
  472      from->set_diff_XYZ(-tx, -ty, -tz);
  473      to  ->set_diff_XYZ( tx,  ty,  tz);
  474 
  475      // nonzero derivatives in project equations
  476      A->new_row();
  477 
  478      if (from->free_horizontal_position())
  479        {
  480          A->add_element(from->diff_N(), from->N.index());
  481          A->add_element(from->diff_E(), from->E.index());
  482        }
  483      if (from->free_height())
  484        {
  485          A->add_element(from->diff_U(), from->U.index());
  486        }
  487 
  488      if (to->free_horizontal_position())
  489        {
  490          A->add_element(to->diff_N(), to->N.index());
  491          A->add_element(to->diff_E(), to->E.index());
  492        }
  493      if (to->free_height())
  494        {
  495          A->add_element(to->diff_U(), to->U.index());
  496        }
  497    }
  498 
  499    // right hand site
  500    {
  501      double rx, dx = to->X_dh(v->to_dh) - from->X_dh(v->from_dh);
  502      double ry, dy = to->Y_dh(v->to_dh) - from->Y_dh(v->from_dh);
  503      double rz, dz = to->Z_dh(v->to_dh) - from->Z_dh(v->from_dh);
  504 
  505      const double s = Linear().scale();
  506 
  507      rhs(++rhs_ind) = rx = (v->dx() - dx)*s;
  508      rhs(++rhs_ind) = ry = (v->dy() - dy)*s;
  509      rhs(++rhs_ind) = rz = (v->dz() - dz)*s;
  510 
  511      if (abs(rx) > tol_abs || abs(ry) > tol_abs || abs(rz) > tol_abs)
  512        {
  513          Model::Rejected robs;
  514 
  515          robs.criterion   = Model::Rejected::rhs;
  516          robs.observation = v;
  517          robs.data[0]     = rx;
  518          robs.data[1]     = ry;
  519          robs.data[2]     = rz;
  520 
  521          rejected_obs.push_back(robs);
  522          reset_parameters();
  523          v->set_active(false);
  524        }
  525    }
  526 }
  527 
  528 
  529 
  530 void Model::linearization(XYZ* xyz)
  531 {
  532   Point* point = points->find(xyz->id);
  533 
  534   for (int i=1; i<=3; i++)
  535    {
  536      double tx=0, ty=0, tz=0;
  537      switch (i) {
  538      case 1: tx = 1.0; break;
  539      case 2: ty = 1.0; break;
  540      case 3: tz = 1.0; break;
  541      };
  542 
  543      point->set_diff_XYZ(tx, ty, tz);
  544 
  545      // nonzero derivatives in project equations
  546      A->new_row();
  547 
  548      if (point->free_horizontal_position())
  549        {
  550          A->add_element(point->diff_N(), point->N.index());
  551          A->add_element(point->diff_E(), point->E.index());
  552        }
  553      if (point->free_height())
  554        {
  555          A->add_element(point->diff_U(), point->U.index());
  556        }
  557    }
  558 
  559    // right hand site
  560    {
  561      double rx, x = point->X();
  562      double ry, y = point->Y();
  563      double rz, z = point->Z();
  564 
  565      const double s = Linear().scale();
  566 
  567      rhs(++rhs_ind) = rx =(xyz->x() - x)*s;
  568      rhs(++rhs_ind) = ry = (xyz->y() - y)*s;
  569      rhs(++rhs_ind) = rz = (xyz->z() - z)*s;
  570 
  571      if (abs(rx) > tol_abs || abs(ry) > tol_abs || abs(rz) > tol_abs)
  572        {
  573          Model::Rejected robs;
  574 
  575          robs.criterion   = Model::Rejected::rhs;
  576          robs.observation = xyz;
  577          robs.data[0]     = rx;
  578          robs.data[1]     = ry;
  579          robs.data[2]     = rz;
  580 
  581          rejected_obs.push_back(robs);
  582          reset_parameters();
  583          xyz->set_active(false);
  584        }
  585    }
  586 }
  587 
  588 
  589 
  590 void Model::linearization(ZenithAngle* z)
  591 {
  592   Point* from = points->find(z->from);
  593   Point* to   = points->find(z->to  );
  594 
  595   E_3 from_vertical, from_to, p1, v1, p2, v2;
  596 
  597   p1.set(from->X(), from->Y(), from->Z());
  598   v1 = from_vertical = vertical(from);
  599   v1 *= z->from_dh;
  600   p1 += v1;                               // instrument
  601 
  602   p2.set(to->X(), to->Y(), to->Z());
  603   v2  = vertical(to);
  604   v2 *= z->to_dh;
  605   p2 += v2;                               // target
  606 
  607   from_to  = p2;
  608   from_to -= p1;                          // instrument --> target
  609 
  610   R_3 R;
  611   R.set_rotation(from->B(), from->L());   // dif_NEU --> dif_XYZ
  612   E_3 local;
  613   R.inverse(from_to, local);
  614 
  615   const double r = std::sqrt(local.e1*local.e1 + local.e2*local.e2);
  616   const double s = local.e1*local.e1 + local.e2*local.e2 + local.e3*local.e3;
  617   const double q = 1.0/(r*s);
  618 
  619   // pd - partial derivatives for the occupied station
  620   E_3 pd( -local.e1*q, -local.e2*q, r/s);
  621 
  622 
  623   const double sca = Angular().scale();
  624   const double scl = Linear ().scale();
  625   const double sc  = sca / scl;
  626 
  627 
  628   // nonzero derivatives in project equations
  629   A->new_row();
  630   if (from->free_horizontal_position())
  631     {
  632       A->add_element(pd.e1*sc, from->N.index());
  633       A->add_element(pd.e2*sc, from->E.index());
  634     }
  635   if (from->free_height())
  636     {
  637       A->add_element(pd.e3*sc, from->U.index());
  638     }
  639 
  640   E_3 tmp;
  641   R.rotation(pd, tmp);
  642   tmp *= -1.0;
  643   R.set_rotation(to->B(),to->L());
  644   // pd - partial derivatives for the target station
  645   R.inverse (tmp, pd);
  646 
  647   if (to->free_horizontal_position())
  648     {
  649       A->add_element(pd.e1*sc, to->N.index());
  650       A->add_element(pd.e2*sc, to->E.index());
  651     }
  652   if (to->free_height())
  653     {
  654       A->add_element(pd.e3*sc, to->U.index());
  655     }
  656 
  657 
  658   // right hand site
  659 
  660   double za = angle(from_vertical, from_to);
  661 
  662   /************************************************************/
  663   /*          !!! add refraction correction here !!!          */
  664   /************************************************************/
  665 
  666   rhs(++rhs_ind) = (z->obs() - za)*sca;
  667 }
  668