Line data    Source code

       1             : #include "PseudoHuber.h"
       2             : #include "Scaling.h"
       3             : #include "TypeCasts.hpp"
       4             : 
       5             : #include <cmath>
       6             : #include <stdexcept>
       7             : 
       8             : namespace elsa
       9             : {
      10             :     template <typename data_t>
      11             :     PseudoHuber<data_t>::PseudoHuber(const DataDescriptor& domainDescriptor, real_t delta)
      12             :         : Functional<data_t>(domainDescriptor), _delta{delta}
      13           6 :     {
      14             :         // sanity check delta
      15           6 :         if (delta <= static_cast<real_t>(0.0))
      16           0 :             throw InvalidArgumentError("PseudoHuber: delta has to be positive.");
      17           6 :     }
      18             : 
      19             :     template <typename data_t>
      20             :     PseudoHuber<data_t>::PseudoHuber(const Residual<data_t>& residual, real_t delta)
      21             :         : Functional<data_t>(residual), _delta{delta}
      22          10 :     {
      23             :         // sanity check delta
      24          10 :         if (delta <= static_cast<real_t>(0.0))
      25           0 :             throw InvalidArgumentError("PseudoHuber: delta has to be positive.");
      26          10 :     }
      27             : 
      28             :     template <typename data_t>
      29             :     data_t PseudoHuber<data_t>::evaluateImpl(const DataContainer<data_t>& Rx)
      30           4 :     {
      31             :         // note: this is currently not a reduction in DataContainer, but implemented here "manually"
      32             : 
      33           4 :         auto result = static_cast<data_t>(0.0);
      34             : 
      35        1350 :         for (index_t i = 0; i < Rx.getSize(); ++i) {
      36        1346 :             data_t temp = Rx[i] / _delta;
      37        1346 :             result += _delta * _delta
      38        1346 :                       * (sqrt(static_cast<data_t>(1.0) + temp * temp) - static_cast<data_t>(1.0));
      39        1346 :         }
      40             : 
      41           4 :         return result;
      42           4 :     }
      43             : 
      44             :     template <typename data_t>
      45             :     void PseudoHuber<data_t>::getGradientInPlaceImpl(DataContainer<data_t>& Rx)
      46           4 :     {
      47        1350 :         for (index_t i = 0; i < Rx.getSize(); ++i) {
      48        1346 :             data_t temp = Rx[i] / _delta;
      49        1346 :             Rx[i] = Rx[i] / sqrt(static_cast<data_t>(1.0) + temp * temp);
      50        1346 :         }
      51           4 :     }
      52             : 
      53             :     template <typename data_t>
      54             :     LinearOperator<data_t> PseudoHuber<data_t>::getHessianImpl(const DataContainer<data_t>& Rx)
      55           4 :     {
      56           4 :         DataContainer<data_t> scaleFactors(Rx.getDataDescriptor());
      57        1350 :         for (index_t i = 0; i < Rx.getSize(); ++i) {
      58        1346 :             data_t temp = Rx[i] / _delta;
      59        1346 :             data_t tempSq = temp * temp;
      60        1346 :             data_t sqrtOnePTempSq = sqrt(static_cast<data_t>(1.0) + tempSq);
      61        1346 :             scaleFactors[i] =
      62        1346 :                 (sqrtOnePTempSq - tempSq / sqrtOnePTempSq) / (static_cast<data_t>(1.0) + tempSq);
      63        1346 :         }
      64             : 
      65           4 :         return leaf(Scaling<data_t>(Rx.getDataDescriptor(), scaleFactors));
      66           4 :     }
      67             : 
      68             :     template <typename data_t>
      69             :     PseudoHuber<data_t>* PseudoHuber<data_t>::cloneImpl() const
      70           4 :     {
      71           4 :         return new PseudoHuber(this->getResidual(), _delta);
      72           4 :     }
      73             : 
      74             :     template <typename data_t>
      75             :     bool PseudoHuber<data_t>::isEqual(const Functional<data_t>& other) const
      76           4 :     {
      77           4 :         if (!Functional<data_t>::isEqual(other))
      78           0 :             return false;
      79             : 
      80           4 :         auto otherPHuber = downcast_safe<PseudoHuber>(&other);
      81           4 :         if (!otherPHuber)
      82           0 :             return false;
      83             : 
      84           4 :         if (_delta != otherPHuber->_delta)
      85           0 :             return false;
      86             : 
      87           4 :         return true;
      88           4 :     }
      89             : 
      90             :     // ------------------------------------------
      91             :     // explicit template instantiation
      92             :     template class PseudoHuber<float>;
      93             :     template class PseudoHuber<double>;
      94             :     // no complex-number instantiations for PseudoHuber! (they would not really be useful)
      95             : 
      96             : } // namespace elsa