Squashed 'third_party/ct/' content from commit 0048d02
Change-Id: Ia7e5360cbb414f92ce4f118bd9613ea23597db52
git-subtree-dir: third_party/ct
git-subtree-split: 0048d027531b6cf1ea730da17b68a0b7ef9070b1
diff --git a/ct_rbd/src/kindrEulerInverseRotateTest.cpp b/ct_rbd/src/kindrEulerInverseRotateTest.cpp
new file mode 100644
index 0000000..1ae971c
--- /dev/null
+++ b/ct_rbd/src/kindrEulerInverseRotateTest.cpp
@@ -0,0 +1,71 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#include <chrono>
+#include <kindr/Core>
+
+int main()
+{
+ static const size_t nTests = 1000000;
+ std::vector<kindr::EulerAnglesXyzD, Eigen::aligned_allocator<kindr::EulerAnglesXyzD>> angles(nTests);
+
+ typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> test_vector_t;
+ test_vector_t vectors(nTests);
+ test_vector_t vectorsRotatedNormal(nTests);
+ test_vector_t vectorsRotatedMatrix(nTests);
+
+ std::cout << "running " << nTests << " rotation tests" << std::endl;
+
+ for (size_t i = 0; i < nTests; i++)
+ {
+ angles[i].setRandom();
+ vectors[i].setRandom();
+ }
+
+ std::cout << "timing angle.inverseRotate(vector)" << std::endl;
+ auto start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ vectorsRotatedNormal[i] = angles[i].inverseRotate(vectors[i]);
+ }
+ auto end = std::chrono::high_resolution_clock::now();
+ auto diff = end - start;
+ size_t msTotal = std::chrono::duration<double, std::milli>(diff).count();
+ std::cout << "Total time: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ std::cout << "timing angle.toRotationMatrix().inverted().rotate(vector)" << std::endl;
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ vectorsRotatedMatrix[i] = kindr::RotationMatrixD(angles[i]).inverted().rotate(vectors[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::milli>(diff).count();
+ std::cout << "Total time: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ double error = 0.0;
+ double maxError = 0.0;
+ size_t maxErrorIndex = 0;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ double err = (vectorsRotatedNormal[i] - vectorsRotatedMatrix[i]).norm();
+ if (err > maxError)
+ {
+ maxError = err;
+ maxErrorIndex = i;
+ }
+ error += err;
+ }
+ std::cout << "average error: " << error / double(nTests) << std::endl;
+ std::cout << "max error " << maxError << " for pair: " << std::endl;
+ std::cout << "euler angles: " << angles[maxErrorIndex].toImplementation().transpose() << std::endl;
+ std::cout << "vector to rotate: " << vectors[maxErrorIndex].transpose() << std::endl;
+ std::cout << "vector inverse rotated (direct): " << vectorsRotatedNormal[maxErrorIndex].transpose() << std::endl;
+ std::cout << "vector inverse rotated (rotation matrix transpose): "
+ << vectorsRotatedMatrix[maxErrorIndex].transpose() << std::endl;
+}