ossimSarSensorModel.cpp 56.2 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
/*
 * Copyright (C) 2005-2017 by Centre National d'Etudes Spatiales (CNES)
 *
 * This file is licensed under MIT license:
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

25

26 27 28 29 30
#include "ossim/ossimSarSensorModel.h"
#include "ossim/ossimKeyWordListUtilities.h"
#include "ossim/ossimTraceHelpers.h"
#include "ossim/ossimRangeUtilities.h"
#include "ossim/ossimSarSensorModelPathsAndKeys.h"
31
#include <ossim/base/ossimRegExp.h>
32
#include <ossim/base/ossimLsrSpace.h>
33
#include <ossim/base/ossimKeywordNames.h>
34 35 36 37 38 39 40
#include <boost/static_assert.hpp>
#include <iostream>
#include <vector>
#include <algorithm>

#if defined(USE_BOOST_TIME)
using boost::posix_time::microseconds;
41
using boost::posix_time::seconds;
42 43
#else
using ossimplugins::time::microseconds;
44
using ossimplugins::time::seconds;
45
#endif
46 47

namespace {// Anonymous namespace
48
   const bool         k_verbose = false; // global verbose constant; TODO: use an option
49 50 51 52 53 54 55 56

   // Sometimes, we don't need to compare the actual distance, its square value is
   // more than enough.
   inline double squareDistance(ossimDpt const& lhs, ossimDpt const& rhs) {
      const double dx = lhs.x - rhs.x;
      const double dy = lhs.y - rhs.y;
      return dx*dx + dy*dy;
   }
57

58 59 60 61 62
   inline double squareDistance(ossimGpt const& lhs, ossimGpt const& rhs) {
      const ossimEcefPoint l(lhs);
      const ossimEcefPoint r(rhs);
      return (l-r).norm2();
   }
63

64
   template <typename Container>
65
      inline void unzip(Container const& in, Container& out_even, Container& out_odd)
66
      {
67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
         typedef typename Container::const_iterator const_iterator;
         typedef typename Container::size_type      size_type;

         const size_type size                         = in.size();
         const bool      has_a_odd_number_of_elements = size % 2 == 1;

         out_even.reserve(size/2+1);
         out_odd.reserve(size/2);

         const_iterator end = in.end();
         if (has_a_odd_number_of_elements)
         {
            std::advance(end, -1);
         }
         for (const_iterator it=in.begin(); it != end ; )
         {
            out_even.push_back(*it++);
            out_odd.push_back(*it++);
         }
         if (has_a_odd_number_of_elements)
         {
            assert(end != in.end());
            out_even.push_back(*end);
         }
         assert(out_even.size() >= out_odd.size());
         assert(out_odd.capacity() == size/2); // The correct number of element have been reserved
         assert(out_even.capacity() == size/2+1); // The correct number of element have been reserved
         assert(out_odd.size() + out_even.size() == size);
95 96 97 98
      }

   ossimTrace traceExec  ("ossimSarSensorModel:exec");
   ossimTrace traceDebug ("ossimSarSensorModel:debug");
99

100 101
   typedef char const* const* strings_iterator;
   static char const* const PRODUCTTYPE_STRINGS[] = { "SLC", "GRD", "MGD", "GEC", "EEC" };
102 103 104 105
}// Anonymous namespace

namespace ossimplugins
{
106 107
   RTTI_DEF1(ossimSarSensorModel, "ossimSarSensorModel", ossimSensorModel);

108
   const double ossimSarSensorModel::C = 299792458;
109

110 111
   const unsigned int ossimSarSensorModel::thePluginVersion = 2;

112 113 114 115 116 117 118 119 120 121
   ossimSarSensorModel::ProductType::ProductType(string_view const& s)
   {
      using ossimplugins::begin;
      using ossimplugins::end;
      strings_iterator const ProductType_it = std::find(begin(::PRODUCTTYPE_STRINGS),end(::PRODUCTTYPE_STRINGS), s);
      if (ProductType_it == end(::PRODUCTTYPE_STRINGS))  {
         throw std::runtime_error("Invalid Sar Sensor Product type: `"+s+"'");
      }
      m_value = Type(std::distance(begin(::PRODUCTTYPE_STRINGS), ProductType_it));
      assert(m_value < MAX__);
122 123
   }

124 125 126 127 128 129 130
   string_view ossimSarSensorModel::ProductType::ToString() const
   {
      BOOST_STATIC_ASSERT((MAX__ == array_size(::PRODUCTTYPE_STRINGS)));
      assert(m_value != UNDEFINED__); // Yes, I know UNDEFINED__ > MAX__
      assert(m_value < MAX__);
      return PRODUCTTYPE_STRINGS[m_value];
   }
131

132 133
   ossimSarSensorModel::ossimSarSensorModel()
      : theRadarFrequency(0.),
134
      theAzimuthTimeInterval(seconds(0)),
135 136 137 138
      theNearRangeTime(0.),
      theRangeSamplingRate(0.),
      theRangeResolution(0.),
      theBistaticCorrectionNeeded(false),
139 140
      theAzimuthTimeOffset(seconds(0)),
      theRangeTimeOffset(0.)
141 142 143 144 145 146
      {}

   ossimSarSensorModel::GCPRecordType const&
      ossimSarSensorModel::findClosestGCP(ossimDpt const& imPt) const
      {
         assert(!theGCPRecords.empty()&&"theGCPRecords is empty.");
147

148 149
         // Find the closest GCP
         double distance2 = squareDistance(imPt, theGCPRecords.front().imPt);
150

151 152 153 154 155 156
         std::vector<GCPRecordType>::const_iterator refGcp = theGCPRecords.begin();
         std::vector<GCPRecordType>::const_iterator gcpIt  = theGCPRecords.begin();
         std::vector<GCPRecordType>::const_iterator gcpEnd = theGCPRecords.end();
         for(++gcpIt ; gcpIt!=gcpEnd ; ++gcpIt)
         {
            const double currentDistance2 = squareDistance(imPt, gcpIt->imPt);
157

158 159 160 161 162 163 164 165 166
            if(currentDistance2 < distance2)
            {
               distance2 = currentDistance2;
               refGcp = gcpIt;
            }
         }

         assert(refGcp != theGCPRecords.end() && "A GCP record shall have been found!");
         return *refGcp;
167 168
      }

169 170
   void ossimSarSensorModel::lineSampleHeightToWorld(const ossimDpt& imPt, const double & heightAboveEllipsoid, ossimGpt& worldPt) const
   {
171
      // std::clog << "ossimSarSensorModel::lineSampleHeightToWorld()\n";
172
      assert(!theGCPRecords.empty()&&"theGCPRecords is empty.");
173

174
      GCPRecordType const& refGcp = findClosestGCP(imPt);
175

176 177 178 179 180 181 182 183 184 185 186
      // Set the height reference
      ossim_float64 hgtSet;
      if ( ossim::isnan(heightAboveEllipsoid) )
      {
         hgtSet = refGcp.worldPt.height();
      }
      else
      {
         hgtSet = heightAboveEllipsoid;
      }
      const ossimHgtRef hgtRef(AT_HGT, hgtSet);
187

188
      ossimEcefPoint ellPt;
189

190 191
      // Simple iterative inversion of inverse model starting at closest gcp
      projToSurface(refGcp,imPt,hgtRef,ellPt);
192

193 194
      worldPt = ossimGpt(ellPt);
   }
195

196 197
   void ossimSarSensorModel::lineSampleToWorld(ossimDpt const& imPt, ossimGpt& worldPt) const
   {
198
      // std::clog << "ossimSarSensorModel::lineSampleToWorld()\n";
199
      assert(!theGCPRecords.empty()&&"theGCPRecords is empty.");
200

201 202
      GCPRecordType const& refGcp = findClosestGCP(imPt);
      ossimGpt      const& refPt = refGcp.worldPt;
203

204
      const ossimHgtRef hgtRef(AT_DEM);
205

206
      ossimEcefPoint ellPt;
207

208 209
      // Simple iterative inversion of inverse model starting at closest gcp
      projToSurface(refGcp,imPt,hgtRef,ellPt);
210

211 212
      worldPt = ossimGpt(ellPt);
   }
213

214 215
   void ossimSarSensorModel::worldToLineSample(const ossimGpt& worldPt, ossimDpt & imPt) const
   {
216
      // std::clog << "ossimSarSensorModel::worldToLineSample()\n";
217
      assert(theRangeResolution>0&&"theRangeResolution is null.");
218

219 220 221
      // First compute azimuth and range time
      TimeType azimuthTime;
      double rangeTime;
222

223
      const bool success = worldToAzimuthRangeTime(worldPt, azimuthTime, rangeTime);
224

225 226 227 228 229
      if(!success)
      {
         imPt.makeNan();
         return;
      }
230 231
      // std::clog << "AzimuthTime: " << azimuthTime << "\n";
      // std::clog << "RangeTime: " << rangeTime << "\n";
232
      // std::clog << "GRD: " << isGRD() << "\n";
233

234 235
      // Convert azimuth time to line
      azimuthTimeToLine(azimuthTime,imPt.y);
236

237 238 239 240 241
      if(isGRD())
      {
         // GRD case
         double groundRange(0);
         slantRangeToGroundRange(rangeTime*C/2,azimuthTime,groundRange);
242 243
         // std::clog << "GroundRange: " << groundRange << "\n";
         // std::clog << "TheRangeResolution: " << theRangeResolution << "\n";
244 245 246 247 248 249 250 251

         // Eq 32 p. 31
         // TODO: possible micro-optimization: precompute 1/theRangeResolution, and
         // use *
         imPt.x = groundRange/theRangeResolution;
      }
      else
      {
252 253
         // std::clog << "TheNearRangeTime: " << theNearRangeTime << "\n";
         // std::clog << "TheRangeSamplingRate: " << theRangeSamplingRate << "\n";
254 255 256 257 258
         // SLC case
         // Eq 23 and 24 p. 28
         imPt.x = (rangeTime - theNearRangeTime)*theRangeSamplingRate;
      }
   }
259

260 261
   bool ossimSarSensorModel::worldToAzimuthRangeTime(const ossimGpt& worldPt, TimeType & azimuthTime, double & rangeTime) const
   {
262
      // std::clog << "ossimSarSensorModel::worldToAzimuthRangeTime()\n";
263 264
      // First convert lat/lon to ECEF
      ossimEcefPoint inputPt(worldPt);
265

266 267 268 269
      // Compute zero doppler time
      TimeType interpTime;
      ossimEcefPoint interpSensorPos;
      ossimEcefVector interpSensorVel;
270

271
      const bool success = zeroDopplerLookup(inputPt,azimuthTime,interpSensorPos,interpSensorVel);
272

273 274 275 276 277
      if(!success)
      {
         // TODO: check whether we could throw instead
         return false;
      }
278

279 280 281 282 283
      if(theBistaticCorrectionNeeded)
      {
         // Compute bistatic correction if needed
         DurationType bistaticCorrection;
         computeBistaticCorrection(inputPt,interpSensorPos,bistaticCorrection);
284

285 286
         // Update interpolated azimuth time
         azimuthTime += bistaticCorrection;
287

288 289 290
         // Update sensor position and velocity
         interpolateSensorPosVel(interpTime,interpSensorPos,interpSensorVel);
      }
291

292 293 294
      // rangeTime is the round-tripping time to target
      const double rangeDistance = (interpSensorPos-inputPt).magnitude();
      rangeTime = theRangeTimeOffset + 2*rangeDistance/C;
295

296 297
      return true;
   }
298

299 300
   void ossimSarSensorModel::lineSampleToAzimuthRangeTime(const ossimDpt & imPt, TimeType & azimuthTime, double & rangeTime) const
   {
301
      // std::clog << "ossimSarSensorModel::lineSampleToAzimuthRangeTime()\n";
302 303
      // First compute azimuth time here
      lineToAzimuthTime(imPt.y,azimuthTime);
304

305 306 307 308 309 310 311 312 313 314 315 316 317
      // Then compute range time
      if(isGRD())
      {
         // Handle grd case here
         double slantRange;
         groundRangeToSlantRange(imPt.x*theRangeResolution,azimuthTime, slantRange);
         rangeTime = theRangeTimeOffset + 2*slantRange/C;
      }
      else
      {
         rangeTime = theRangeTimeOffset + theNearRangeTime + imPt.x*(1/theRangeSamplingRate);
      }
   }
318

319 320
   void ossimSarSensorModel::computeRangeDoppler(const ossimEcefPoint & inputPt, const ossimEcefPoint & sensorPos, const ossimEcefVector sensorVel, double & range, double & doppler) const
   {
321
      // std::clog << "ossimSarSensorModel::computeRangeDoppler()\n";
322
      assert(theRadarFrequency>0&&"theRadarFrequency is null");
323

324 325
      // eq. 19, p. 25
      const ossimEcefVector s2gVec = inputPt - sensorPos;
326

327
      range = s2gVec.magnitude();
328

329
      const double coef = -2*C/(theRadarFrequency*range);
330

331 332
      doppler = coef * sensorVel.dot(s2gVec);
   }
333

334 335 336
   void ossimSarSensorModel::interpolateSensorPosVel(const TimeType & azimuthTime, ossimEcefPoint& sensorPos, ossimEcefVector& sensorVel, unsigned int deg) const
   {
      assert(!theOrbitRecords.empty()&&"The orbit records vector is empty");
337

338
      // Lagrangian interpolation of sensor position and velocity
339

340
      unsigned int nBegin(0), nEnd(0);
341

342 343 344
      sensorPos[0] = 0;
      sensorPos[1] = 0;
      sensorPos[2] = 0;
345

346 347 348
      sensorVel[0] = 0;
      sensorVel[1] = 0;
      sensorVel[2] = 0;
349

350 351
      // First, we search for the correct set of record to use during
      // interpolation
352

353 354
      // If there are less records than degrees, use them all
      if(theOrbitRecords.size()<deg)
355
      {
356
         nEnd = theOrbitRecords.size()-1;
357
      }
358
      else
359
      {
360 361
         // Search for the deg number of records around the azimuth time
         unsigned int t_min_idx = 0;
362
         DurationType t_min = abs(azimuthTime - theOrbitRecords.front().azimuthTime);
363

364
         unsigned int count = 0;
365

366 367
         for(std::vector<OrbitRecordType>::const_iterator it = theOrbitRecords.begin();it!=theOrbitRecords.end();++it,++count)
         {
368
            const DurationType current_time = abs(azimuthTime-it->azimuthTime);
369

370 371 372 373 374 375
            if(t_min > current_time)
            {
               t_min_idx = count;
               t_min = current_time;
            }
         }
376
         // TODO: see if these expressions can be simplified
377 378 379 380
         nBegin = std::max((int)t_min_idx-(int)deg/2+1,(int)0);
         nEnd = std::min(nBegin+deg-1,(unsigned int)theOrbitRecords.size());
         nBegin = nEnd<theOrbitRecords.size()-1 ? nBegin : nEnd-deg+1;
      }
381

382 383 384 385 386 387 388 389
      // Compute lagrangian interpolation using records from nBegin to nEnd
      for(unsigned int i = nBegin; i < nEnd; ++i)
      {
         double w = 1.;

         unsigned int j = nBegin;
         for( ; j != i ; ++j)
         {
390 391 392 393
            const DurationType td1 = azimuthTime                    - theOrbitRecords[j].azimuthTime;
            const DurationType td2 = theOrbitRecords[i].azimuthTime - theOrbitRecords[j].azimuthTime;
            const double f = td1 / td2;
            w *= f;
394 395 396 397
         }
         ++j;
         for( ; j < nEnd; ++j)
         {
398 399 400 401
            const DurationType td1 = azimuthTime                    - theOrbitRecords[j].azimuthTime;
            const DurationType td2 = theOrbitRecords[i].azimuthTime - theOrbitRecords[j].azimuthTime;
            const double f = td1 / td2;
            w *= f;
402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426
         }

         sensorPos[0]+=w*theOrbitRecords[i].position[0];
         sensorPos[1]+=w*theOrbitRecords[i].position[1];
         sensorPos[2]+=w*theOrbitRecords[i].position[2];

         sensorVel[0]+=w*theOrbitRecords[i].velocity[0];
         sensorVel[1]+=w*theOrbitRecords[i].velocity[1];
         sensorVel[2]+=w*theOrbitRecords[i].velocity[2];
      }
   }

   void ossimSarSensorModel::slantRangeToGroundRange(const double & slantRange, const TimeType & azimuthTime, double & groundRange) const
   {
      applyCoordinateConversion(slantRange,azimuthTime,theSlantRangeToGroundRangeRecords,groundRange);
   }

   void ossimSarSensorModel::groundRangeToSlantRange(const double & groundRange, const TimeType & azimuthTime, double & slantRange) const
   {
      applyCoordinateConversion(groundRange,azimuthTime,theGroundRangeToSlantRangeRecords,slantRange);
   }

   void ossimSarSensorModel::applyCoordinateConversion(const double & in, const TimeType& azimuthTime, const std::vector<CoordinateConversionRecordType> & records, double & out) const
   {
      assert(!records.empty()&&"The records vector is empty.");
427
      // std::clog << "conv coord(" << in << ", az="<<azimuthTime<<")\n";
428

429 430
      // First, we need to find the correct pair of records for interpolation
      std::vector<CoordinateConversionRecordType>::const_iterator it = records.begin();
431

432
      CoordinateConversionRecordType srgrRecord;
433

434 435
      std::vector<CoordinateConversionRecordType>::const_iterator  previousRecord = it;
      ++it;
436

437
      std::vector<CoordinateConversionRecordType>::const_iterator nextRecord = it;
438

439
      // Look for the correct record
440
      // std::clog << "Looking for " << azimuthTime << " within records:\n";
441 442
      while(it!=records.end())
      {
443
         // std::clog << "- record: " << it->azimuthTime << "...";
444 445 446 447 448
         // nextRecord = it;

         if(azimuthTime >= previousRecord->azimuthTime
               && azimuthTime < nextRecord->azimuthTime)
         {
449
            // std::clog << " found!\n";
450
            break;
451 452 453
         }
         else
         {
454 455 456
            previousRecord = nextRecord;
            ++it;
            nextRecord = it;
457
            // std::clog << " NOT found => next!\n";
458 459 460 461 462 463 464
         }
      }
      assert(nextRecord == it);
      if(it == records.end())
      {
         if(azimuthTime < records.front().azimuthTime)
         {
465
            srgrRecord = records.front();
466
            // std::clog << "Not found, but before first => srgrRecord: " << srgrRecord.azimuthTime << "\n";
467 468 469
         }
         else if(azimuthTime >= records.back().azimuthTime)
         {
470
            srgrRecord = records.back();
471
            // std::clog << "Not found, but after last => srgrRecord: " << srgrRecord.azimuthTime << "\n";
472 473 474 475 476 477 478 479 480 481
         }
      }
      else
      {
         assert(nextRecord != records.end());
         assert(!previousRecord->coefs.empty()&&"previousRecord coefficients vector is empty.");
         assert(!nextRecord->coefs.empty()&&"nextRecord coefficients vector is empty.");

         // If azimuth time is between 2 records, interpolate
         const double interp
482
            = DurationType(azimuthTime             - previousRecord->azimuthTime)
483 484
            / (nextRecord->azimuthTime - previousRecord->azimuthTime)
            ;
485
         // std::clog << "interp: " << interp << " ="
486 487 488
         // << " (" << azimuthTime             << " - " << previousRecord->azimuthTime << " (="<< (azimuthTime             - previousRecord->azimuthTime)<< ") )"
         // << "/(" << nextRecord->azimuthTime << " - " << previousRecord->azimuthTime << " (="<< (nextRecord->azimuthTime - previousRecord->azimuthTime)<< ") )"
         // << "\n";
489

490
         srgrRecord.rg0 = (1-interp) * previousRecord->rg0 + interp*nextRecord->rg0;
491

492 493 494
         srgrRecord.coefs.clear();
         std::vector<double>::const_iterator pIt = previousRecord->coefs.begin();
         std::vector<double>::const_iterator nIt = nextRecord->coefs.begin();
495

496 497
         for(;pIt != previousRecord->coefs.end() && nIt != nextRecord->coefs.end();++pIt,++nIt)
         {
498
            srgrRecord.coefs.push_back(interp*(*nIt)+(1-interp)*(*pIt));
499
         }
500

501 502
         assert(!srgrRecord.coefs.empty()&&"Slant range to ground range interpolated coefficients vector is empty.");
      }
503

504 505 506
      // Now that we have the interpolated coefs, compute ground range
      // from slant range
      const double sr_minus_sr0 =  in-srgrRecord.rg0;
507

508
      assert(!srgrRecord.coefs.empty()&&"Slant range to ground range coefficients vector is empty.");
509

510
      out = 0;
511

512 513 514 515 516
      for(std::vector<double>::const_reverse_iterator cIt = srgrRecord.coefs.rbegin();cIt!=srgrRecord.coefs.rend();++cIt)
      {
         out = *cIt + sr_minus_sr0*out;
      }
   }
517 518


519 520
   bool ossimSarSensorModel::zeroDopplerLookup(const ossimEcefPoint & inputPt, TimeType & interpAzimuthTime, ossimEcefPoint & interpSensorPos, ossimEcefVector & interpSensorVel) const
   {
521
      assert((theOrbitRecords.size()>=2) && "Orbit records vector contains less than 2 elements");
522

523
      std::vector<OrbitRecordType>::const_iterator it = theOrbitRecords.begin();
524

525
      double doppler2(0.);
526

527 528 529
      // Compute range and doppler of first record
      // NOTE: here we only use the scalar product with vel and discard
      // the constant coef as it has no impact on doppler sign
530

531
      double doppler1 = (inputPt-it->position).dot(it->velocity);
532 533


534
      bool dopplerSign1 = doppler1 < 0;
535

536
      ++it; // -> it != begin
537

538 539 540 541 542 543
      // Look for the consecutive records where doppler freq changes sign
      // Note: implementing a bisection algorithm here might be faster
      for ( ; it!=theOrbitRecords.end() ; ++it)
      {
         // compute range and doppler of current record
         doppler2 = (inputPt-it->position).dot(it->velocity);
544

545
         const bool dopplerSign2 = doppler2 <0;
546

547 548 549 550 551 552 553 554 555
         // If a change of sign is detected
         if(dopplerSign1 != dopplerSign2)
         {
            break;
         }
         else
         {
            doppler1 = doppler2;
         }
556
      }
557

558
      // In this case, we need to extrapolate
559
      if(it == theOrbitRecords.end())
560
      {
561 562
         std::vector<OrbitRecordType>::const_iterator record1 = theOrbitRecords.begin();
         std::vector<OrbitRecordType>::const_iterator record2 = record1 + theOrbitRecords.size()-1;
563 564 565 566
         doppler1 = (inputPt-record1->position).dot(record1->velocity);
         doppler2 = (inputPt-record2->position).dot(record2->velocity);
         const DurationType delta_td = record2->azimuthTime - record1->azimuthTime;
         interpAzimuthTime = record1->azimuthTime - doppler1 / (doppler2 - doppler1) * delta_td;
567
      }
568
      else
569
      {
570 571 572 573
         assert(it != theOrbitRecords.begin());
         assert(it != theOrbitRecords.end());
         std::vector<OrbitRecordType>::const_iterator record2 = it;
         std::vector<OrbitRecordType>::const_iterator record1 = --it;
574 575 576
         // now interpolate time and sensor position
         const double abs_doppler1 = std::abs(doppler1);
         const double interpDenom = abs_doppler1+std::abs(doppler2);
577

578
         assert(interpDenom>0&&"Both doppler frequency are null in interpolation weight computation");
579

580
         const double interp = abs_doppler1/interpDenom;
581
         // std::clog << "interp: " << interp << "\n";
582

583
         const DurationType delta_td = record2->azimuthTime - record1->azimuthTime;
584
         // std::clog << "delta_td: " << delta_td << " = " << record2->azimuthTime <<" - " <<record1->azimuthTime<< "\n";
585

586
         // Compute interpolated time offset wrt record1
587
         // (No need for that many computations (day-frac -> ms -> day frac))
588
         const DurationType td     = delta_td * interp;
589
         // std::clog << "td: " << td  << "(" << td.total_microseconds() << "us)\n";
590
         // Compute interpolated azimuth time
591
         interpAzimuthTime = record1->azimuthTime + td + theAzimuthTimeOffset;
592
      }
593

594
      // std::clog << "interpAzimuthTime: " << interpAzimuthTime << "\n";
595

596 597
      // Interpolate sensor position and velocity
      interpolateSensorPosVel(interpAzimuthTime,interpSensorPos, interpSensorVel);
598

599 600
      return true;
   }
601

602 603 604
   void ossimSarSensorModel::computeBistaticCorrection(const ossimEcefPoint & inputPt, const ossimEcefPoint & sensorPos, DurationType & bistaticCorrection) const
   {
      // Bistatic correction (eq 25, p 28)
605
      double halftrange = 1000000. * (sensorPos-inputPt).magnitude()/C;
606 607
      bistaticCorrection= microseconds(static_cast<unsigned long>(floor(halftrange+0.5)));
   }
608 609


610 611 612
   void ossimSarSensorModel::azimuthTimeToLine(const TimeType & azimuthTime, double & line) const
   {
      assert(!theBurstRecords.empty()&&"Burst records are empty (at least one burst should be available)");
613

614
      std::vector<BurstRecordType>::const_iterator currentBurst = theBurstRecords.begin();
615

616 617 618 619 620 621 622 623 624 625 626 627 628
      // Look for the correct burst. In most cases the number of burst
      // records will be 1 (except for TOPSAR Sentinel1 products)
      std::vector<BurstRecordType>::const_iterator it = theBurstRecords.begin();
      std::vector<BurstRecordType>::const_iterator itend = theBurstRecords.end();
      for(; it!= itend ; ++it)
      {
         if(azimuthTime >= it->azimuthStartTime
               && azimuthTime < it->azimuthStopTime)
         {
            currentBurst = it;
            break;
         }
      }
629

630 631 632
      // If no burst is found, we will use the first (resp. last burst to
      // extrapolate line
      if(it == itend)
633
      {
634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649
         if(! theBurstRecords.empty())
         {
            if(azimuthTime < theBurstRecords.front().azimuthStartTime)
            {
               currentBurst = theBurstRecords.begin();
            }
            else if (azimuthTime > theBurstRecords.back().azimuthStopTime)
            {
               currentBurst = theBurstRecords.end()-1;
            }
         }
         else
         {
            // Fall back to the only record
            currentBurst = theBurstRecords.begin();
         }
650 651
      }

652
      const DurationType timeSinceStart = azimuthTime - currentBurst->azimuthStartTime;
653

654
      // Eq 22 p 27
655
      line = (timeSinceStart/theAzimuthTimeInterval) + currentBurst->startLine;
656
      // std::clog << "line = " << line << " <- " << timeSinceStart << "/" << theAzimuthTimeInterval << "+" << currentBurst->startLine << "\n";
657
   }
658

659 660 661
   void ossimSarSensorModel::lineToAzimuthTime(const double & line, TimeType & azimuthTime) const
   {
      assert(!theBurstRecords.empty()&&"Burst records are empty (at least one burst should be available)");
662

663
      std::vector<BurstRecordType>::const_iterator currentBurst = theBurstRecords.begin();
664

665 666 667 668 669 670 671 672
      if(theBurstRecords.size() != 1)
      {
         // Look for the correct burst. In most cases the number of burst
         // records will be 1 (except for TOPSAR Sentinel1 products)
         std::vector<BurstRecordType>::const_iterator it = theBurstRecords.begin();
         std::vector<BurstRecordType>::const_iterator itend = theBurstRecords.end();
         for( ; it!= itend; ++it)
         {
673 674
            if(line >= it->startLine && line < it->endLine)
            {
675 676
               currentBurst = it;
               break;
677
            }
678
         }
679

680 681
         if(it == itend)
         {
682 683
            if(line < theBurstRecords.front().startLine)
            {
684
               currentBurst = theBurstRecords.begin();
685 686 687
            }
            else if (line >= theBurstRecords.back().endLine)
            {
688
               currentBurst = theBurstRecords.end()-1;
689
            }
690
         }
691

692
      }
693

694 695
      const DurationType timeSinceStart = (line - currentBurst->startLine)*theAzimuthTimeInterval;
      // std::clog << "timeSinceStart: " << timeSinceStart.total_microseconds() << "us\n";
696

697
      // Eq 22 p 27
698 699
      azimuthTime = currentBurst->azimuthStartTime + timeSinceStart + theAzimuthTimeOffset;
      // std::clog << "offset: "         << theAzimuthTimeOffset << "\n";
700
      // std::clog << "->azimuthTime: "  << azimuthTime << "\n";
701
   }
702 703 704



705 706
   bool ossimSarSensorModel::projToSurface(const GCPRecordType & initGcp, const ossimDpt & target, const ossimHgtRef & hgtRef, ossimEcefPoint & ellPt) const
   {
707
     
708 709
      // Initialize current estimation
      ossimEcefPoint currentEstimation(initGcp.worldPt);
710

711
      // Compute corresponding image position
712
      // std::clog << "initGCP: " << initGcp.imPt << "\n";
713
      ossimDpt currentImPoint(initGcp.imPt);
714

715 716
      ossim_float64 currentImSquareResidual = squareDistance(target,currentImPoint);
      double currentHeightResidual = initGcp.worldPt.height() - hgtRef.getRefHeight(initGcp.worldPt);
717

718
      bool init = true;
719

720
      unsigned int iter = 0;
721

722 723 724 725 726 727 728
      // TODO: Every time the function is called, an allocation (+a free) is done.
      // This is not efficient. => Find a static matrix of 3x3 elements
      // Moreover, NEWMAT implies a lot of objet creations, hence allocations
      NEWMAT::SymmetricMatrix BtB(3);
      NEWMAT::ColumnVector BtF(3);
      NEWMAT::ColumnVector F(3);
      NEWMAT::ColumnVector dR(3);
729

730 731 732 733 734 735
      // Stop condition: img residual < 1e-2 pixels, height residual² <
      // 0.01² m, nb iter < 50. init ensure that loop runs at least once.
      while((init || (currentImSquareResidual > (0.01*0.01) || std::abs(currentHeightResidual) > 0.01))  && iter < 50)
      {
         if(init)
            init =false;
736

737
         // std::clog<<"Iter: "<<iter<<", Res: im="<<currentImSquareResidual<<", hgt="<<currentHeightResidual<<'\n';
738

739 740 741 742
         // compute residuals
         F(1) = target.x - currentImPoint.x;
         F(2) = target.y - currentImPoint.y;
         F(3) = currentHeightResidual;
743

744
         // std::clog<<"F("<<iter<<")="<<F<<'\n';
745

746 747
         // Delta use for partial derivatives estimation (in meters)
         const double d = 10.;
748

749 750 751
         // Compute partial derivatives
         ossimEcefVector p_fx, p_fy, p_fh,dx(d,0,0),dy(0,d,0),dz(0,0,d);
         ossimDpt tmpImPt;
752

753
         ossim_float64 rdx(0.0),rdy(0.0),rdz(0.0), fdx(0.0),fdy(0.0),fdz(0.0);
754

755 756 757
         ossimGpt currentEstimationWorld(currentEstimation);
         ossimGpt tmpGpt = ossimGpt(currentEstimation+dx);
         worldToLineSample(tmpGpt,tmpImPt);
758 759 760 761
         // std::clog << "currentEstimationWorld: " << currentEstimationWorld << "\n";
         // std::clog << "currentEstimation: " << currentEstimation << "\n";
         // std::clog << "tmpGpt: " << tmpGpt << "\n";
         // std::clog << "tmpImPt: " << tmpImPt << "\n";
762 763 764
         p_fx[0] = (currentImPoint.x-tmpImPt.x)/d;
         p_fy[0] = (currentImPoint.y-tmpImPt.y)/d;
         p_fh[0] = (currentEstimationWorld.height()-tmpGpt.height())/d;
765

766 767 768 769 770
         tmpGpt = ossimGpt(currentEstimation+dy);
         worldToLineSample(tmpGpt,tmpImPt);
         p_fx[1] = (currentImPoint.x-tmpImPt.x)/d;
         p_fy[1] = (currentImPoint.y-tmpImPt.y)/d;
         p_fh[1] = (currentEstimationWorld.height()-tmpGpt.height())/d;
771

772 773 774 775 776
         tmpGpt = ossimGpt(currentEstimation+dz);
         worldToLineSample(tmpGpt,tmpImPt);
         p_fx[2] = (currentImPoint.x-tmpImPt.x)/d;
         p_fy[2] = (currentImPoint.y-tmpImPt.y)/d;
         p_fh[2] = (currentEstimationWorld.height()-tmpGpt.height())/d;
777

778 779 780 781
         // Form B-matrix
         NEWMAT::Matrix B = ossimMatrix3x3::create(p_fx[0], p_fx[1], p_fx[2],
               p_fy[0], p_fy[1], p_fy[2],
               p_fh[0], p_fh[1], p_fh[2]);
782

783
         // std::clog<<"B: "<<B<<'\n';
784

785 786 787
         // Invert system
         try {
            dR = B.i() * F;
Guillaume Pasero's avatar
Guillaume Pasero committed
788
         } catch (NEWMAT::SingularException const& e) {
789 790 791 792 793 794
         ellPt = currentEstimation;
         
         ossimNotify(ossimNotifyLevel_WARN) <<"ossim::SarSensorModel::projToSurface(): singular matrix can not be inverted. Returning best estimation so far ("<<ellPt<<") for output point ("<<target<<")\n";
         std::clog << "initGCP: " << initGcp.imPt <<", "<<initGcp.worldPt<< "\n";
         
         return true;
795
         }
796

797 798 799 800 801
         // Update estimate
         for (ossim_int32 k=0; k<3; k++)
         {
            currentEstimation[k] -= dR(k+1);
         }
802

803
         // std::clog<<"dR: "<<dR<<'\n';
804

805
         currentEstimationWorld=ossimGpt(currentEstimation);
806

807 808 809
         // Update residuals
         const ossim_float64 atHgt = hgtRef.getRefHeight(currentEstimationWorld);
         currentHeightResidual = atHgt - currentEstimationWorld.height();
810

811
         worldToLineSample(currentEstimationWorld,currentImPoint);
812

813
         // std::clog<<currentImPoint<<'\n';
814

815
         currentImSquareResidual = squareDistance(currentImPoint,target);
816

817 818
         ++iter;
      }
819

820
      // std::clog<<"Iter: "<<iter<<", Res: im="<<currentImSquareResidual<<", hgt="<<currentHeightResidual<<'\n';
821

822 823 824
      ellPt = currentEstimation;
      return true;
   }
825 826 827



828 829 830 831 832 833 834 835 836 837 838
   //*************************************************************************************************
   // Infamous DUP
   //*************************************************************************************************
   ossimObject* ossimSarSensorModel::dup() const
   {
      return new ossimSarSensorModel(*this);
   }
   bool ossimSarSensorModel::useForward() const
   {
      return false;
   }
839

840 841
   bool ossimSarSensorModel::autovalidateInverseModelFromGCPs(const double & xtol, const double & ytol, const double azTimeTol, const double & rangeTimeTol) const
   {
842
      std::clog << "ossimSarSensorModel::autovalidateInverseModelFromGCPs()\n";
843 844 845 846
      if(theGCPRecords.empty())
      {
         return false;
      }
847

848
      bool success = true;
849

850
      unsigned int gcpId = 1;
851

852
      std::clog << theGCPRecords.size() << " GCPS\n";
853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875