167 lines
3.7 KiB
C++
167 lines
3.7 KiB
C++
/**
|
|
* 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
|
|
|
|
|