| 67 | } |
| 68 | |
| 69 | bool testIsometry3d(const std::string& id, const Eigen::Isometry3d& expect, |
| 70 | const Eigen::Isometry3d& actual) |
| 71 | { |
| 72 | static const double EPSILON = 0.000001; |
| 73 | EXPECT_GT(EPSILON, fabs(expect.translation().x() - actual.translation().x())) |
| 74 | << id << " Translation x - expect: " << expect.translation().x() |
| 75 | << " actual: " << actual.translation().x(); |
| 76 | EXPECT_GT(EPSILON, fabs(expect.translation().y() - actual.translation().y())) |
| 77 | << id << " Translation y - expect: " << expect.translation().y() |
| 78 | << " actual: " << actual.translation().y(); |
| 79 | EXPECT_GT(EPSILON, fabs(expect.translation().z() - actual.translation().z())) |
| 80 | << id << " Translation z - expect: " << expect.translation().z() |
| 81 | << " actual: " << actual.translation().z(); |
| 82 | |
| 83 | Eigen::Quaterniond q1(expect.rotation()); |
| 84 | Eigen::Quaterniond q2(actual.rotation()); |
| 85 | EXPECT_GT(EPSILON, fabs(q1.x() - q2.x())) |
| 86 | << id << " Quaternion x - expect: " << q1.x() << " actual: " << q2.x(); |
| 87 | EXPECT_GT(EPSILON, fabs(q1.y() - q2.y())) |
| 88 | << id << " Quaternion y - expect: " << q1.y() << " actual: " << q2.y(); |
| 89 | EXPECT_GT(EPSILON, fabs(q1.z() - q2.z())) |
| 90 | << id << " Quaternion z - expect: " << q1.z() << " actual: " << q2.z(); |
| 91 | EXPECT_GT(EPSILON, fabs(q1.w() - q2.w())) |
| 92 | << id << " Quaternion w - expect: " << q1.w() << " actual: " << q2.w(); |
| 93 | |
| 94 | return true; |
| 95 | } |
| 96 | |
| 97 | bool testVector(const std::string& id, const std::vector<double>& expect, |
| 98 | const std::vector<double>& actual) |
nothing calls this directly
no outgoing calls
no test coverage detected