[static, private]
Checks if two Eigen::Vector3d are equal.
{ static const float threshold = 0.0001; return abs(a[0] - b[0]) <= threshold && abs(a[1] - b[1]) <= threshold && abs(a[2] - b[2]) <= threshold; }