double HiFunctionActual(value* x, value* x1, value* x2) {
double dX1X2 = DistanceFunction(x1, x2);
double res = square(DistanceFunction(x, x1))
+ square(DistanceFunction(x, x2)) - square(dX1X2);
res /= 2 * dX1X2;
return res;
}
bool StereoFilter::FilterSinglePoint(float x, float y, float z) {
// check to see if this is near any of the points in the last message
// NOTE: could implement an octree search if this linear search turns out to be too slow
for (int i = 0; i < last_stereo_msg_->number_of_points; i++) {
// compute distance to this point
float dist = DistanceFunction(x, last_stereo_msg_->x[i], y, last_stereo_msg_->y[i], z, last_stereo_msg_->z[i]);
// std::cout << "dist = " << dist << std::endl;
if (dist < distance_threshold_) {
// std::cout << "(" << last_stereo_msg_->x[i] << ", " << x << ") , (" << last_stereo_msg_->y[i] << ", " << y << ") , (" << last_stereo_msg_->z[i] << ", " << z << ")" << std::endl;
return true;
}
}
return false;
}
int InRangeFunction(value* x1, value* x2, double r) {
return DistanceFunction(x1, x2) <= r;
}
int EqualFunction(value* x1, value* x2) {
return DistanceFunction(x1, x2) == 0;
}
