Commit f6b41eb0 authored by Julien Osman's avatar Julien Osman
Browse files

Merge branch '2160-SarSensorModel' into 2159

parents 3367d6e5 4a3140d3
......@@ -19,4 +19,7 @@
#
project(OTBBoostAdapters)
set(OTBBoostAdapters_LIBRARIES OTBBoostAdapters)
otb_module_impl()
/*
* Copyright (C) 2005-2020 Centre National d'Etudes Spatiales (CNES)
*
* This file is part of Orfeo Toolbox
*
* https://www.orfeo-toolbox.org/
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef ossimDateTime_h
#define ossimDateTime_h
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/config.hpp>
#include "otbOperatorUtilities.h"
// boost::posix_time::time_duration doesn't have a sufficient precision to
// store things such an azimuth time interval, and yet, this IS a duration.
// Hence this new class injected into boost namespace to emulate a duration
// with precision behind the microsecond.
namespace boost { namespace posix_time {
class precise_duration;
double ratio_(precise_duration const& lhs, precise_duration const& rhs);
class precise_duration
: private boostAdapter::addable<precise_duration>
, private boostAdapter::substractable<precise_duration>
, private boostAdapter::streamable<precise_duration>
, private boostAdapter::multipliable2<precise_duration, double>
, private boostAdapter::dividable<precise_duration, double>
, private boostAdapter::equality_comparable<precise_duration>
, private boostAdapter::less_than_comparable<precise_duration>
, private boostAdapter::addable<ptime, precise_duration>
, private boostAdapter::substractable<ptime, precise_duration>
{
public:
typedef double scalar_type;
/**@name Construction/destruction
*/
//@{
/** Initialization constructor.
*/
precise_duration() {} // = default;
explicit precise_duration(double usec_frac)
: m_usec_frac(usec_frac) {}
precise_duration(time_duration const& d)
: m_usec_frac(d.total_microseconds()) {}
//@}
double total_seconds() const {
return total_microseconds() / 1000000.;
}
double total_microseconds() const {
return m_usec_frac;
}
bool is_negative() const { return total_microseconds() < 0.0; }
precise_duration invert_sign() { return precise_duration(- total_seconds()); }
std::ostream & display(std::ostream & os) const { return os << m_usec_frac; }
std::istream & read (std::istream & is) { return is >> m_usec_frac; }
protected:
/**@name Operations
*/
//@{
void add(precise_duration const& rhs) { m_usec_frac += rhs.total_microseconds(); }
void sub(precise_duration const& rhs) { m_usec_frac -= rhs.total_microseconds(); }
void mult(scalar_type coeff) { m_usec_frac *= coeff; }
void div(scalar_type coeff) { assert(coeff && "Cannot divide by 0"); m_usec_frac /= coeff; }
friend precise_duration& operator+=(precise_duration & u, precise_duration const& v) {
u.add(v);
return u;
}
friend ptime& operator+=(ptime & u, precise_duration const& v) {
const time_duration d = microseconds(static_cast<int> (floor(v.total_microseconds()+0.5)));
u += d;
return u;
}
friend precise_duration& operator-=(precise_duration & u, precise_duration const& v) {
u.sub(v);
return u;
}
friend ptime& operator-=(ptime & u, precise_duration const& v) {
const time_duration d = microseconds(static_cast<int>( floor(v.total_microseconds()+0.5)));
u -= d;
return u;
}
template <typename U, typename V> static U diff(V const& lhs, V const& rhs) {
U const res(lhs.total_microseconds() - rhs.total_microseconds());
return res;
}
friend precise_duration& operator*=(precise_duration & u, scalar_type const& v) {
u.mult(v);
return u;
}
friend precise_duration& operator/=(precise_duration & u, scalar_type const& v) {
u.div(v);
return u;
}
friend bool operator<(precise_duration const& lhs, precise_duration const& rhs) {
return lhs.total_microseconds() < rhs.total_microseconds();
}
friend bool operator==(precise_duration const& lhs, precise_duration const& rhs) {
return lhs.total_microseconds() == rhs.total_microseconds();
}
public:
friend scalar_type ratio_(precise_duration const& lhs, precise_duration const& rhs)
{ return lhs.total_microseconds() / rhs.total_microseconds(); }
//@}
private:
double m_usec_frac;
};
time_duration abs(time_duration d);
}
} // boost::time namespaces
namespace otb
{
namespace MetaData
{
using TimeType = boost::posix_time::ptime;
using DurationType = boost::posix_time::precise_duration;
}
}
#endif
/*
* Copyright (C) 2005-2019 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.
*/
#ifndef otbOperatorUtilities_h
#define otbOperatorUtilities_h
namespace boostAdapter {
// Uses Barton-Nackman trick to help implement operator on classes
// Strongly inspired by boost.operators interface
//TODO: imported from ossim plugins, but can we use boost instead ?
#define DEFINE_OPERATORS(name, compound, op) \
template <typename T> struct name ## 1 \
{ \
friend T operator op(T lhs, T const& rhs) { \
lhs compound rhs; \
return lhs; \
} \
}; \
template <typename T, typename U> struct name ## 2 \
{ \
friend T operator op(T lhs, U const& rhs) { \
lhs compound rhs; \
return lhs; \
} \
friend T operator op(U const& lhs, T rhs) { \
rhs compound lhs; \
return rhs; \
} \
}; \
template <typename T, typename U=void> struct name : name ## 2<T,U> {}; \
template <typename T> struct name<T,void> : name ## 1<T> {};
template <typename U, typename V> struct substractable_asym
{
friend U operator-(V const& lhs, V const& rhs) {
return V::template diff<U,V>(lhs, rhs);
}
};
DEFINE_OPERATORS(addable, +=, +);
DEFINE_OPERATORS(substractable, -=, -);
DEFINE_OPERATORS(multipliable, *=, *);
#undef DEFINE_OPERATORS
template <typename T, typename R> struct dividable {
typedef R scalar_type;
friend T operator/(T lhs, scalar_type const& rhs) {
lhs /= rhs;
return lhs;
}
friend scalar_type operator/(T const& lhs, T const& rhs) {
return ratio_(lhs, rhs);
}
};
template <typename T> struct streamable {
friend std::ostream & operator<<(std::ostream & os, const T & v)
{ return v.display(os); }
friend std::istream & operator>>(std::istream & is, T & v)
{ return v.read(is); }
};
template <typename T> struct less_than_comparable {
friend bool operator>(T const& lhs, T const& rhs) {
return rhs < lhs;
}
friend bool operator>=(T const& lhs, T const& rhs) {
return !(lhs < rhs);
}
friend bool operator<=(T const& lhs, T const& rhs) {
return !(rhs < lhs);
}
};
template <typename T> struct equality_comparable {
friend bool operator!=(T const& lhs, T const& rhs) {
return !(rhs == lhs);
}
};
}// namespace boostAdapter
#endif // otbOperatorUtilities_h
......@@ -21,10 +21,9 @@
set(DOCUMENTATION "Adapters for the Boost library.")
otb_module(OTBBoostAdapters
ENABLE_SHARED
DEPENDS
OTBBoost
DESCRIPTION
"${DOCUMENTATION}"
)
#
# Copyright (C) 2005-2020 Centre National d'Etudes Spatiales (CNES)
#
# This file is part of Orfeo Toolbox
#
# https://www.orfeo-toolbox.org/
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
set(OTBBoostAdapters_SRC
otbDateTime.cxx
)
add_library(OTBBoostAdapters ${OTBBoostAdapters_SRC})
target_link_libraries(OTBBoostAdapters
${OTBBoost_LIBRARIES}
)
otb_module_target(OTBBoostAdapters)
/*
* Copyright (C) 2005-2020 Centre National d'Etudes Spatiales (CNES)
*
* This file is part of Orfeo Toolbox
*
* https://www.orfeo-toolbox.org/
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "otbDateTime.h"
namespace boost
{
namespace posix_time
{
time_duration abs(time_duration d)
{
if(d.is_negative())
d = d.invert_sign();
return d;
}
}
}
\ No newline at end of file
......@@ -97,7 +97,13 @@ public:
void ParseGeom(ImageMetadata &) override;
std::vector<std::map<std::string, std::string> > saveMetadataBands(std::string file) ;
std::vector<Orbit> getOrbits(const std::string & referenceTime) const;
std::vector<BurstRecord> CreateBurstRecord(const std::string & firstLineTimeStr,
const std::string & lastLineTimeStr,
const unsigned long endLine,
const unsigned long endSample) const;
protected:
/* class ctor */
......
......@@ -277,6 +277,7 @@ struct OTBMetadata_EXPORT Time : tm
int GetMinute() const;
double GetSecond() const;
///@}
};
......
......@@ -32,6 +32,9 @@
#include "itkPoint.h"
#include "itkPointSet.h"
#include "otbDateTime.h"
namespace otb
{
......@@ -68,7 +71,7 @@ struct OTBMetadata_EXPORT DopplerCentroid
/** \struct SARNoise
*
* \breif This structure is used to handle Noise look up tables
* \brief This structure is used to handle Noise look up tables
*/
struct OTBMetadata_EXPORT SARNoise
{
......@@ -82,20 +85,50 @@ struct OTBMetadata_EXPORT SARNoise
/** \struct Orbit
*
* \breif This structure is used to handle orbit information
* \brief This structure is used to handle orbit information
*/
struct OTBMetadata_EXPORT Orbit
{
using PointType = itk::Point<double, 3>;
/** Timestamp at which orbit state vectors apply */
MetaData::Time time;
MetaData::TimeType time;
/** Position vector */
PointType position;
/** Velocity vector */
PointType velocity;
};
/** \struct BurstRecord
*
* \brief This structure is used to handle burst records
*/
struct OTBMetadata_EXPORT BurstRecord
{
MetaData::TimeType azimuthStartTime;
unsigned long startLine;
MetaData::TimeType azimuthStopTime;
unsigned long endLine;
unsigned long startSample;
unsigned long endSample;
double azimuthAnxTime;
};
/** \struct GCPTime
*
* \brief This structure contains the azimuth and range times associated with a gcp
*/
struct OTBMetadata_EXPORT GCPTime
{
/** Azimuth time of the gcp */
MetaData::TimeType azimuthTime;
/** Slant range time of the gcp */
double slantRangeTime;
};
/** \struct SARParam
*
* \brief SAR sensors parameters
......@@ -109,6 +142,10 @@ struct OTBMetadata_EXPORT SARParam
*/
std::vector<AzimuthFmRate> azimuthFmRates;
MetaData::DurationType azimuthTimeInterval;
double nearRangeTime;
double rangeSamplingRate;
/** Doppler centroid estimates */
std::vector<DopplerCentroid> dopplerCentroids;
......@@ -117,6 +154,12 @@ struct OTBMetadata_EXPORT SARParam
/** List of orbit information */
std::vector<Orbit> orbits;
/** List of burst records */
std::vector<BurstRecord> burstRecords;
/** map between GCP ids and corresponding azimuth and range times */
std::unordered_map<std::string, GCPTime> gcpTimes;
};
/** \struct SARCalib
......@@ -151,4 +194,3 @@ struct OTBMetadata_EXPORT SARCalib
} // end namespace otb
#endif
......@@ -125,6 +125,13 @@ protected:
/* Fetch the noise LUTs */
std::vector<SARNoise> GetNoiseVector(const XMLMetadataSupplier&) const;
/* Fetch the burst records */
std::vector<BurstRecord> GetBurstRecords(const XMLMetadataSupplier&) const;
/* Fetch azimuth and range times corresponding to GCPs */
std::unordered_map<std::string, GCPTime> GetGCPTimes(const XMLMetadataSupplier & xmlMS,
const Projection::GCPParam & gcps) const;
/* Compute the mean terrain elevation */
double getBandTerrainHeight(const XMLMetadataSupplier&) const;
......
......@@ -110,7 +110,7 @@ public:
*
* @return A std::string
*/
std::string PrintSelf();
std::string PrintSelf() const;
protected:
/**
......
......@@ -328,11 +328,9 @@ std::vector<Orbit> CosmoImageMetadataInterface::getOrbits(const std::string & re
int hour = static_cast<int> (total_seconds/3600.0);
int minutes = static_cast<int> ((total_seconds-hour*3600)/60.0);
double seconds = total_seconds - hour*3600 - minutes*60;
std::string timestr = reference_UTC + "T" + std::to_string(hour) + ":" + std::to_string(minutes) + ":" + std::to_string(seconds);
std::string timestr = reference_UTC + " " + std::to_string(hour) + ":" + std::to_string(minutes) + ":" + std::to_string(seconds);
MetaData::Time time = Utils::LexicalCast<MetaData::Time,std::string>(timestr, std::string("T"));
orbit.time = time ;
orbit.time = boost::posix_time::time_from_string(timestr);
orbit.position[0] = std::stod(vECEF_sat_pos[i*3 + 0]) ;
orbit.position[1] = std::stod(vECEF_sat_pos[i*3 + 1]) ;
......@@ -344,7 +342,34 @@ std::vector<Orbit> CosmoImageMetadataInterface::getOrbits(const std::string & re
orbitVector.push_back(orbit);
}
return orbitVector ;
return orbitVector;
}
std::vector<BurstRecord> CosmoImageMetadataInterface::CreateBurstRecord(const std::string & firstLineTimeStr, const std::string & lastLineTimeStr, const unsigned long endLine, const unsigned long endSample) const
{
BurstRecord record;
record.startLine = 0;
record.startSample = 0;
record.endLine = endLine;
record.endSample = endSample;
std::stringstream ss;
auto facet = new boost::posix_time::time_input_facet("%Y-%m-%dT%H:%M:%S%F");
ss.imbue(std::locale(std::locale(), facet));
ss << firstLineTimeStr;
ss >> record.azimuthStartTime;
ss << lastLineTimeStr;
ss >> record.azimuthStopTime;
record.azimuthAnxTime = 0.;
return {record};
}
void CosmoImageMetadataInterface::ParseGdal(ImageMetadata & imd)
......@@ -380,7 +405,6 @@ void CosmoImageMetadataInterface::ParseGdal(ImageMetadata & imd)
Fetch(MDNum::PRF, imd, "S01_PRF");
//getTime
auto subDsName = "HDF5:" + m_MetadataSupplierInterface->GetResourceFile() + "://S01/SBI";
auto metadataBands = this->saveMetadataBands(subDsName) ;
......@@ -392,22 +416,54 @@ void CosmoImageMetadataInterface::ParseGdal(ImageMetadata & imd)
int hour = static_cast<int> (total_seconds/3600.0);
int minutes = static_cast<int> ((total_seconds-hour*3600)/60.0);
double seconds = total_seconds - hour*3600 - minutes*60;
std::string first_line_time = reference_UTC + "T" + std::to_string(hour) + ":" + std::to_string(minutes) + ":" + std::to_string(seconds);
// Helper function to convert to a two digit string.
auto formatTimeInt = [](int i) { return (i < 10 ? "0" + std::to_string(i)
: std::to_string(i) );};
auto formatTimeDouble = [](double d) { return (d < 10 ? "0" + std::to_string(d)
: std::to_string(d) );};
std::string first_line_time = reference_UTC + "T" + formatTimeInt(hour) + ":" + formatTimeInt(minutes) + ":" + formatTimeDouble(seconds);
total_seconds = std::stod(metadataBands[0]["S01_SBI_Zero_Doppler_Azimuth_Last_Time"]);
hour = static_cast<int> (total_seconds/3600.0);
minutes = static_cast<int> ((total_seconds-hour*3600)/60.0);
seconds = total_seconds - hour*3600 - minutes*60;
std::string last_line_time = reference_UTC + "T" + std::to_string(hour) + ":" + std::to_string(minutes) + ":" + std::to_string(seconds);
std::string last_line_time = reference_UTC + "T" + formatTimeInt(hour) + ":" + formatTimeInt(minutes) + ":" + formatTimeDouble(seconds);
MetaData::Time startTime = Utils::LexicalCast<MetaData::Time,std::string>(first_line_time, std::string("T"));
MetaData::Time stoptTime = Utils::LexicalCast<MetaData::Time,std::string>(last_line_time, std::string("T"));
imd.Add(MDTime::AcquisitionStartTime, startTime);
imd.Add(MDTime::AcquisitionStopTime, stoptTime);
// Retrieve the product dimension, as it is not stored in the metadata
auto dataset = static_cast<GDALDataset*>(GDALOpen(subDsName.c_str(), GA_ReadOnly));
int sizex = dataset->GetRasterXSize();
int sizey = dataset->GetRasterYSize();
GDALClose(dataset);
//SAR Parameters
SARParam sarParam;
sarParam.orbits = this->getOrbits(reference_UTC);
sarParam.burstRecords = CreateBurstRecord(first_line_time, last_line_time, sizex-1, sizey-1);
auto rangeTimeInterval = std::stod(metadataBands[0]["S01_SBI_Column_Time_Interval"]);