61 std::vector<double> data(m);
62 memcpy(&data[0], fvec,
sizeof(
double) * m);
64 std::nth_element(data.begin(), data.begin() + (data.size() >> 1), data.end());
65 return (data[data.size() >> 1]);
74 huber(
const Eigen::Vector4f& p_src,
const Eigen::Vector4f& p_tgt,
double sigma)
76 Eigen::Array4f diff = (p_tgt.array() - p_src.array()).abs();
78 for (
int i = 0; i < 3; ++i) {
80 norm += diff[i] * diff[i];
82 norm += 2.0 * sigma * diff[i] - sigma * sigma;
92 huber(
double diff,
double sigma)
98 norm += 2.0 * sigma * diff - sigma * sigma;
109 gedikli(
double val,
double clipping,
double slope = 4)
111 return (1.0 / (1.0 + pow(std::abs(val) / clipping, slope)));
119 l1(
const Eigen::Vector4f& p_src,
const Eigen::Vector4f& p_tgt)
121 return ((p_src.array() - p_tgt.array()).abs().sum());
129 l2(
const Eigen::Vector4f& p_src,
const Eigen::Vector4f& p_tgt)
131 return ((p_src - p_tgt).norm());
139 l2Sqr(
const Eigen::Vector4f& p_src,
const Eigen::Vector4f& p_tgt)
141 return ((p_src - p_tgt).squaredNorm());