add banchmark image alignment 2

src/robust_cost.cpp

@@ -0,0 +1,166 @@
+/**
+* This file is part of dvo.
+*
+* Copyright 2012 Christian Kerl <[email protected]> (Technical University of Munich)
+* For more information see <http://vision.in.tum.de/data/software/dvo>.
+*
+* dvo 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.
+*
+* dvo 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 dvo. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <numeric>
+#include <algorithm>
+#include <iostream>
+#include <robust_cost.h>
+#include <math_utils.h>
+
+namespace vk {
+namespace robust_cost {
+
+const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f;
+const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f;
+
+TDistributionScaleEstimator::
+TDistributionScaleEstimator(const float dof) :
+  dof_(dof),
+  initial_sigma_(INITIAL_SIGMA)
+{}
+
+float TDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+    std::cerr<<"42\n";
+  float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_);
+  int num = 0;
+  float lambda = initial_lamda;
+  int iterations = 0;
+  do
+  {
+    ++iterations;
+    initial_lamda = lambda;
+    num = 0;
+    lambda = 0.0f;
+
+    for(std::vector<float>::iterator it=errors.begin(); it!=errors.end(); ++it)
+    {
+      if(std::isfinite(*it))
+      {
+        ++num;
+        const float error2 = (*it)*(*it);
+        lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) );
+      }
+    }
+    lambda = float(num) / lambda;
+  } while(std::abs(lambda - initial_lamda) > 1e-3);
+
+  return std::sqrt(1.0f / lambda);
+}
+
+const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745
+
+float MADScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+    std::cerr<<"74\n";
+  // error must be in absolute values!
+  return NORMALIZER * vk::getMedian(errors);
+}
+
+float NormalDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+    std::cerr<<"82\n";
+  const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size();
+  float var = 0.0;
+  std::for_each(errors.begin(), errors.end(), [&](const float d) {
+      var += (d - mean) * (d - mean);
+  });
+  return std::sqrt(var); // return standard deviation
+}
+
+const float TukeyWeightFunction::DEFAULT_B = 4.6851f;
+
+TukeyWeightFunction::TukeyWeightFunction(const float b)
+{
+  configure(b);
+}
+
+float TukeyWeightFunction::value(const float& x) const
+{
+  const float x_square = x * x;
+  if(x_square <= b_square)
+  {
+    const float tmp = 1.0f - x_square / b_square;
+    return tmp * tmp;
+  }
+  else
+  {
+    return 0.0f;
+  }
+}
+
+void TukeyWeightFunction::
+configure(const float& param)
+{
+  b_square = param * param;
+}
+
+const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f;
+
+TDistributionWeightFunction::
+TDistributionWeightFunction(const float dof)
+{
+  configure(dof);
+}
+
+float TDistributionWeightFunction::
+value(const float & x) const
+{
+  return ((dof_ + 1.0f) / (dof_ + (x * x)));
+}
+
+void TDistributionWeightFunction::
+configure(const float& param)
+{
+  dof_ = param;
+  normalizer_ = dof_ / (dof_ + 1.0f);
+}
+
+const float HuberWeightFunction::DEFAULT_K = 1.345f;
+
+HuberWeightFunction::
+HuberWeightFunction(const float k)
+{
+  configure(k);
+}
+
+void HuberWeightFunction::
+configure(const float& param)
+{
+  k = param;
+}
+
+float HuberWeightFunction::
+value(const float& t) const
+{
+  const float t_abs = std::abs(t);
+  if(t_abs < k)
+    return 1.0f;
+  else
+    return k / t_abs;
+}
+
+} // namespace robust_cost
+} // namespace vk
+
+