| inline void welsch_weight(Eigen::VectorXd& r, double p) | |
| { | |
| for (int i = 0; i < r.size(); ++i) | |
| { | |
| r[i] = (r[i] >= 0) ? std::exp(-r[i] / (2 * p * p)) : 0.0; | |
| } | |
| } | |
| inline double welsch_energy(const Eigen::VectorXd& r, double p) | |
| { | |
| double energy = 0.0; | |
| for (int i = 0; i < r.size(); ++i) | |
| { | |
| energy += 1.0 - std::exp(-r[i] / (2 * p * p)); | |
| } | |
| return energy; | |
| } | |
| inline void robust_weight(Eigen::VectorXd& r, double p) | |
| { | |
| welsch_weight(r, p); | |
| } | |
| inline double get_energy(const Eigen::VectorXd& r, double p) | |
| { | |
| return welsch_energy(r, p); | |
| } | |