Inverse rotation using EulerAngles

[neunertm]

When calling inverseRotate on EulerAngles, the procedure is as follows:

1. convert EulerAngles to RotationQuaternion
2. call inverted on RotationQuaternion
3. call rotate on RotationQuaternion

The question is whether rotating with the inverse (transpose) of a RotationMatrix is not the better way. Therefore, I wrote a little test attached below. The output on my machine (compiled in release mode) is:

running 1000000 rotation tests
timing angle.inverseRotate(vector)
Total time: 498 ms. Average: 0.000498 ms
timing angle.toRotationMatrix().inverted().rotate(vector)
Total time: 150 ms. Average: 0.00015 ms
average error: 3.24446e-16
max error 2.67665e-15 for pair:
euler angles: 3.03138 1.18812 -0.602041
vector to rotate: -0.678126 0.990644 -0.910583
vector inverse rotated (direct): -0.203027 -1.45571 -0.331804
vector inverse rotated (rotation matrix transpose): -0.203027 -1.45571 -0.331804

So it seems numerical precision is pretty similar but runtime is faster. So maybe we should specialize inverseRotate for EulerAngles (both Xyz and Zyx)?

@gehrinch As discussed by phone, here the test.

#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;

	}

[gehrinch]

@neunertm that looks good to me. We should specialize it then.

[neunertm]

I can take a look if you want. But if you prefer to do it yourself, I am happy to back off ;).