40 #ifndef PCL_REGISTRATION_DISTANCES_H 41 #define PCL_REGISTRATION_DISTANCES_H 43 #include <pcl/registration/eigen.h> 59 std::vector<double> data (m);
60 memcpy (&data[0], fvec,
sizeof (
double) * m);
62 std::nth_element(data.begin(), data.begin() + (data.size () >> 1), data.end());
63 return (data[data.size () >> 1]);
72 huber (
const Eigen::Vector4f &p_src,
const Eigen::Vector4f &p_tgt,
double sigma)
74 Eigen::Array4f diff = (p_tgt.array () - p_src.array ()).abs ();
76 for (
int i = 0; i < 3; ++i)
79 norm += diff[i] * diff[i];
81 norm += 2.0 * sigma * diff[i] - sigma * sigma;
91 huber (
double diff,
double sigma)
97 norm += 2.0 * sigma * diff - sigma * sigma;
108 gedikli (
double val,
double clipping,
double slope = 4)
110 return (1.0 / (1.0 + pow (fabs(val) / clipping, slope)));
118 l1 (
const Eigen::Vector4f &p_src,
const Eigen::Vector4f &p_tgt)
120 return ((p_src.array () - p_tgt.array ()).abs ().sum ());
128 l2 (
const Eigen::Vector4f &p_src,
const Eigen::Vector4f &p_tgt)
130 return ((p_src - p_tgt).norm ());
138 l2Sqr (
const Eigen::Vector4f &p_src,
const Eigen::Vector4f &p_tgt)
140 return ((p_src - p_tgt).squaredNorm ());
double computeMedian(double *fvec, int m)
Compute the median value from a set of doubles.
double gedikli(double val, double clipping, double slope=4)
Use a Gedikli kernel to estimate the distance between two vectors (for more information,...
This file defines compatibility wrappers for low level I/O functions.
double l2(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the Euclidean distance between two eigen vectors.
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 l1(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the Manhattan distance between two eigen vectors.
double l2Sqr(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the squared Euclidean distance between two eigen vectors.