Quaternion: Shortest rotation between two vectors #55

MaEtUgR · 2017-11-13T15:49:21Z

There are multiple application scenarios but I need this functionality for the quaternion controller: PX4/PX4-Autopilot#8003

And because of the reusability and the calculation itself suffering tedious corner cases I added it to the library with unit tests to ensure proper results.

A simple explanation for the corner cases: The input vectors are parallel but oposite and therefore require a 180 degree rotation but the cross product to get the perpendicular rotation axis is zero. For more discussion see: https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another .

…operation

…hat maps one vector to another including tedious corner case handling for parallel vectors with 180 degree rotations

coveralls · 2017-11-13T16:04:33Z

Coverage decreased (-0.1%) to 99.89% when pulling a2dc1b5 on quaternion-vec-to-vec into 3bd94fc on master.

coveralls · 2017-11-13T16:19:22Z

Coverage decreased (-0.1%) to 99.89% when pulling a2dc1b5 on quaternion-vec-to-vec into 3bd94fc on master.

including all the parallel vector corner cases

coveralls · 2017-11-13T16:50:17Z

Coverage remained the same at 100.0% when pulling 8f959d0 on quaternion-vec-to-vec into 3bd94fc on master.

MaEtUgR · 2017-11-13T16:52:36Z

Fixed coverage, there was a typo like error which resulted in not covering the case I designed the test for. I made the tests even simpler such that this cannot happen anymore.

Stifael · 2017-11-17T08:14:02Z

matrix/Quaternion.hpp

+     * @param src source vector (no need to normalize)
+     * @param eps epsilon threshold which decides if a value is considered zero
+     */
+    Quaternion(const Vector3<Type> &src, const Vector<Type, 3> dst = Vector3<Type>(0, 0, 1), const Type eps = 1e-5f) :


Vector3< Type > dst = Vector3< Type >(0,0,1)

const Type eps = (Type)1e-5;

There was Vector<Type, 3> which I switched to Vector3<Type> now, that's what you meant right?
const Type eps = (Type)1e-5 is enough, constants are casted auatomatically to the correct type on compile time.

You're right. Second statement is wrong... I put the explicit conversion now.

Stifael · 2017-11-17T08:52:55Z

matrix/Quaternion.hpp

+        Quaternion &q = *this;
+        Vector3<Type> cr = src.cross(dst);
+        float dt = src.dot(dst);
+        if (cr.norm() < eps && dt < 0) {


why do we not comment anything within matrix lib?
for someone who just reads the code the first time it would make it much easier to write that this is the case for parallel vectors pointing opposite

You're totally right, I added comments for that.

Stifael · 2017-11-17T09:05:02Z

matrix/Quaternion.hpp

+            q(0) = Type(0);
+            cr = src.cross(cr);
+        } else {
+            q(0) = src.dot(dst) + sqrt(src.norm_squared() * dst.norm_squared());


this function is definitely not obvious.
maybe write something like "Half-Way Quaternion Solution".

comment added

Stifael · 2017-11-17T09:32:33Z

matrix/Quaternion.hpp

+                }
+            }
+            q(0) = Type(0);
+            cr = src.cross(cr);


I think I get it what you do here with the cr manipulation, but I cannot really verify that it is correct or not?
Do you have a link to an example?

There are a bulk of (code) examples summarized here: https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another

Also if you look at the unit tests you see in the comments which one is going through which case. Plus I can share the MATLAB code I used for intial prototyping of the functionality.

MaEtUgR · 2017-11-29T15:23:01Z

@Stifael Please have another look.
Only thing I really chenged is: I took out the default destination vector because it's only confusing and not a big adavantage.

coveralls · 2017-11-29T17:54:10Z

Coverage remained the same at 100.0% when pulling dbb0e9b on quaternion-vec-to-vec into 41ad2bd on master.

jgoppert

Looks good to me.

Stifael · 2017-12-04T08:33:31Z

matrix/Vector.hpp

@@ -71,6 +71,11 @@ class Vector : public Matrix<Type, M, 1>
        return Type(sqrt(a.dot(a)));
    }

+    Type norm_squared() const {
+        const Vector &a(*this);
+        return Type(a.dot(a));


Is it actually necessary to cast it?

No, you're right, I forgot to get rid of it. It was only necessary for sqrt() which can return double...

…ched type conversions, took out default destination vector because confusing

MaEtUgR · 2017-12-04T12:29:35Z

I added the fix (took out the cast) to the latest commit.

coveralls · 2017-12-04T12:39:14Z

Coverage remained the same at 100.0% when pulling 21b98db on quaternion-vec-to-vec into 41ad2bd on master.

MaEtUgR added 3 commits November 13, 2017 16:23

Vector: added norm_squared() because sometimes you can safe the sqrt …

ba303ae

…operation

Test: added check for Vector.norm_squared()

e92acb0

Quaternion: added constructor which generates the shortest rotation t…

d8cdb1c

…hat maps one vector to another including tedious corner case handling for parallel vectors with 180 degree rotations

MaEtUgR added the enhancement label Nov 13, 2017

MaEtUgR self-assigned this Nov 13, 2017

MaEtUgR requested review from bkueng, jgoppert and Stifael November 13, 2017 15:49

MaEtUgR force-pushed the quaternion-vec-to-vec branch from 9380e80 to a2dc1b5 Compare November 13, 2017 15:49

Test: added check for quaternion vector to vector rotation constructor

8f959d0

including all the parallel vector corner cases

MaEtUgR force-pushed the quaternion-vec-to-vec branch from a2dc1b5 to 8f959d0 Compare November 13, 2017 16:30

Stifael suggested changes Nov 17, 2017

View reviewed changes

Stifael reviewed Nov 17, 2017

View reviewed changes

MaEtUgR force-pushed the quaternion-vec-to-vec branch 2 times, most recently from 0263638 to dbb0e9b Compare November 29, 2017 17:34

jgoppert approved these changes Nov 29, 2017

View reviewed changes

Stifael suggested changes Dec 4, 2017

View reviewed changes

Quaternion/Vector: Small refactor for review: put more comments, swit…

21b98db

…ched type conversions, took out default destination vector because confusing

MaEtUgR force-pushed the quaternion-vec-to-vec branch from dbb0e9b to 21b98db Compare December 4, 2017 12:28

Stifael approved these changes Dec 4, 2017

View reviewed changes

MaEtUgR merged commit f835d39 into master Dec 4, 2017

MaEtUgR deleted the quaternion-vec-to-vec branch December 4, 2017 14:34

MaEtUgR mentioned this pull request Dec 16, 2018

Small quaternion from vector improvements #78

Merged

MaEtUgR mentioned this pull request Nov 20, 2019

[WIP] Different Attitude Setpoint Generation Strategy PX4/PX4-Autopilot#13535

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Quaternion: Shortest rotation between two vectors #55

Quaternion: Shortest rotation between two vectors #55

MaEtUgR commented Nov 13, 2017

coveralls commented Nov 13, 2017

coveralls commented Nov 13, 2017

coveralls commented Nov 13, 2017

MaEtUgR commented Nov 13, 2017

Stifael Nov 17, 2017 •

edited

Loading

MaEtUgR Nov 29, 2017

MaEtUgR Nov 29, 2017

Stifael Nov 17, 2017 •

edited

Loading

MaEtUgR Nov 29, 2017

Stifael Nov 17, 2017

MaEtUgR Nov 29, 2017

Stifael Nov 17, 2017

MaEtUgR Nov 20, 2017 •

edited

Loading

MaEtUgR commented Nov 29, 2017 •

edited

Loading

coveralls commented Nov 29, 2017

jgoppert left a comment

Stifael Dec 4, 2017

MaEtUgR Dec 4, 2017

MaEtUgR commented Dec 4, 2017

coveralls commented Dec 4, 2017

Quaternion: Shortest rotation between two vectors #55

Quaternion: Shortest rotation between two vectors #55

Conversation

MaEtUgR commented Nov 13, 2017

coveralls commented Nov 13, 2017

coveralls commented Nov 13, 2017

coveralls commented Nov 13, 2017

MaEtUgR commented Nov 13, 2017

Stifael Nov 17, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Stifael Nov 17, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaEtUgR Nov 20, 2017 • edited Loading

Choose a reason for hiding this comment

MaEtUgR commented Nov 29, 2017 • edited Loading

coveralls commented Nov 29, 2017

jgoppert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaEtUgR commented Dec 4, 2017

coveralls commented Dec 4, 2017

Stifael Nov 17, 2017 •

edited

Loading

Stifael Nov 17, 2017 •

edited

Loading

MaEtUgR Nov 20, 2017 •

edited

Loading

MaEtUgR commented Nov 29, 2017 •

edited

Loading