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());
double l2Sqr(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the squared Euclidean distance between two eigen vectors.
double gedikli(double val, double clipping, double slope=4)
Use a Gedikli kernel to estimate the distance between two vectors (for more information,...
double huber(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt, double sigma)
Use a Huber kernel to estimate the distance between two vectors.
double computeMedian(double *fvec, int m)
Compute the median value from a set of doubles.
double l2(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the Euclidean distance between two eigen vectors.
double l1(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the Manhattan distance between two eigen vectors.