Added DepthNav

Author: sytelus

AirLib/include/common/VectorMath.hpp

@@ -188,17 +188,25 @@ class VectorMathT {
         return rotateVectorReverse(v_world, q, assume_unit_quat);
     }
 
+    static Vector3T transformToBodyFrame(const Vector3T& v_world, const Pose& body_pose, bool assume_unit_quat = true)
+    {
+        //translate
+        Vector3T translated = v_world - body_pose.position;
+        //rotate
+        return transformToBodyFrame(translated, body_pose.orientation, assume_unit_quat);
+    }
+
     static Vector3T transformToWorldFrame(const Vector3T& v_body, const QuaternionT& q, bool assume_unit_quat = true)
     {
         return rotateVector(v_body, q, assume_unit_quat);
     }
 
-    static Vector3T transformToWorldFrame(const Vector3T& v_body, const Pose& pose, bool assume_unit_quat = true)
+    static Vector3T transformToWorldFrame(const Vector3T& v_body, const Pose& body_pose, bool assume_unit_quat = true)
     {
         //translate
-        Vector3T translated = v_body + pose.position;
+        Vector3T translated = v_body + body_pose.position;
         //rotate
-        return transformToWorldFrame(translated, pose.orientation, assume_unit_quat);
+        return transformToWorldFrame(translated, body_pose.orientation, assume_unit_quat);
     }
 
     static QuaternionT negate(const QuaternionT& q)
@@ -226,6 +234,9 @@ class VectorMathT {
     static void toEulerianAngle(const QuaternionT& q
         , RealT& pitch, RealT& roll, RealT& yaw)
     {
+        //z-y-x rotation convention (Tait-Bryan angles) 
+        //http://www.sedris.org/wg8home/Documents/WG80485.pdf
+
         RealT ysqr = q.y() * q.y();
 
         // roll (x-axis rotation)
@@ -300,6 +311,9 @@ class VectorMathT {
     //all angles in radians
     static QuaternionT toQuaternion(RealT pitch, RealT roll, RealT yaw)
     {
+        //z-y-x rotation convention (Tait-Bryan angles) 
+        //http://www.sedris.org/wg8home/Documents/WG80485.pdf
+
         QuaternionT q;
         RealT t0 = std::cos(yaw * 0.5f);
         RealT t1 = std::sin(yaw * 0.5f);
@@ -413,32 +427,122 @@ class VectorMathT {
     * RPY rotates about the fixed axes in the order x-y-z,
     * which is the same as euler angles in the order z-y'-x''.
     */
-    static RealT yawFromQuaternion(const QuaternionT& q) {
+    static RealT yawFromQuaternion(const QuaternionT& q) 
+    {
         return atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
             1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
     }
 
-    static QuaternionT quaternionFromYaw(RealT yaw) {
-        return QuaternionT(Eigen::AngleAxisd(yaw, Vector3T::UnitZ()));
+    static QuaternionT quaternionFromYaw(RealT yaw) 
+    {
+        return QuaternionT(Eigen::AngleAxis<RealT>(yaw, Vector3T::UnitZ()));
+    }
+
+    static QuaternionT toQuaternion(const Vector3T& axis, RealT angle) 
+    {
+        //Alternative:
+        //auto s = std::sinf(angle / 2);
+        //auto u = axis.normalized();
+        //return Quaternionr(std::cosf(angle / 2), u.x() * s, u.y() * s, u.z() * s);
+
+        return QuaternionT(Eigen::AngleAxis<RealT>(angle, axis));
+    }
+
+    //linear interpolate
+    static QuaternionT lerp(const QuaternionT& from, const QuaternionT& to, RealT alpha)
+    {
+        QuaternionT r;
+        RealT n_alpha = 1 - alpha;
+        r.x = n_alpha * from.x + alpha * to.x;
+        r.y = n_alpha * from.y + alpha * to.y;
+        r.z = n_alpha * from.z + alpha * to.z;
+        r.w = n_alpha * from.w + alpha * to.w;
+        return r.normalized();
+    }
+
+    //spherical lerp
+    static QuaternionT slerp(const QuaternionT& from, const QuaternionT& to, RealT alpha)
+    {
+        QuaternionT r;
+        RealT n_alpha = 1 - alpha;
+        RealT Wa, Wb;
+        RealT theta = acos(from.x*to.x + from.y*to.y + from.z*to.z + from.w*to.w);
+        RealT sn = sin(theta);
+        Wa = sin(n_alpha*theta) / sn;
+        Wb = sin(alpha*theta) / sn;
+        r.x = Wa * from.x + Wb * to.x;
+        r.y = Wa * from.y + Wb * to.y;
+        r.z = Wa * from.z + Wb * to.z;
+        r.w = Wa * from.w + Wb * to.w;
+        return r.normalized();
+    }
+
+    Vector3T lerp(const Vector3T& from, const Vector3T& to, RealT alpha)
+    {
+        return (from + alpha * (to - from));
+    }
+
+    Vector3T slerp(const Vector3T& from, const Vector3T& to, RealT alpha)
+    {
+        RealT dot = from.dot(to);
+        dot = Utils::clip<RealT>(dot, -1, 1);
+        RealT theta = std::acos(dot)*alpha;
+        Vector3T relative = (to - from * dot).normalized();
+        return from * std::cos(theta) + relative * std::sin(theta);
+    }
+
+    Vector3T nlerp(const Vector3T& from, const Vector3T& to, float alpha)
+    {
+        return lerp(from, to, alpha).normalized();
+    }
+
+    static QuaternionT lookAt(Vector3T sourcePoint, Vector3T destPoint)
+    {
+        Vector3T toVector = (destPoint - sourcePoint);
+
+        RealT dot = VectorMath::front().dot(toVector);
+        dot = Utils::clip<RealT>(dot, -1, 1);
+        RealT ang = std::acosf(dot);
+
+        Vector3T axis = VectorMath::front().cross(toVector);
+        if (axis == Vector3T::Zero())
+            axis = VectorMath::up();
+        else
+            axis = axis.normalized();
+
+        return VectorMath::toQuaternion(axis, ang);
     }
 
     static const Vector3T front() 
     {
         static Vector3T v(1, 0, 0);
         return v;
     }
-
+    static const Vector3T back()
+    {
+        static Vector3T v(-1, 0, 0);
+        return v;
+    }
     static const Vector3T down() 
     {
         static Vector3T v(0, 0, 1);
         return v;
     }
-
+    static const Vector3T up()
+    {
+        static Vector3T v(0, 0, -1);
+        return v;
+    }
     static const Vector3T right() 
     {
         static Vector3T v(0, 1, 0);
         return v;
     }
+    static const Vector3T left()
+    {
+        static Vector3T v(0, -1, 0);
+        return v;
+    }
 };
 typedef VectorMathT<Eigen::Vector3d, Eigen::Quaternion<double,Eigen::DontAlign>, double> VectorMathd;
 typedef VectorMathT<Eigen::Vector3f, Eigen::Quaternion<float,Eigen::DontAlign>, float> VectorMathf;

AirLibUnitTests/QuaternionTest.hpp

@@ -10,6 +10,47 @@ class QuaternionTest : public TestBase
 {
 public:
     virtual void run() override
+    {
+        //eulerAngleTest();
+        lookAtTest();
+    }
+
+private:
+    Quaternionr lookAt(Vector3r sourcePoint, Vector3r destPoint)
+    {
+        Vector3r toVector = (destPoint - sourcePoint).normalized();
+
+        Vector3r rotAxis = VectorMath::front().cross(toVector).normalized();
+        if (rotAxis.squaredNorm() == 0)
+            rotAxis = VectorMath::up();
+        float dot = VectorMath::front().dot(toVector);
+        float ang = std::acosf(dot);
+
+        return VectorMath::toQuaternion(rotAxis, ang);
+    }
+
+    Quaternionr toQuaternion(const Vector3r& axis, float angle) {
+        auto s = std::sinf(angle / 2);
+        auto u = axis.normalized();
+        return Quaternionr(std::cosf(angle / 2), u.x() * s, u.y() * s, u.z() * s);
+    }
+
+    void lookAtTest()
+    {
+        auto q = lookAt(Vector3r::Zero(), Vector3r(-1.f, 0, 0));
+        std::cout << VectorMath::toString(q) << std::endl;
+
+        float pitch, roll, yaw;
+        VectorMath::toEulerianAngle(q, pitch, roll, yaw);
+        std::cout << pitch << "\t" << roll << "\t" << yaw << "\t" << std::endl;
+
+        //q = VectorMath::toQuaternion(0, 0, Utils::degreesToRadians(180.0f));
+        //std::cout << VectorMath::toString(q) << std::endl;
+        //VectorMath::toEulerianAngle(q, pitch, roll, yaw);
+        //std::cout << pitch << "\t" << roll << "\t" << yaw << "\t" << std::endl;
+    }
+
+    void eulerAngleTest()
     {
         RandomGeneratorR r(-1000.0f, 1000.0f);
         RandomGeneratorR rw(-1000.0f, 1000.0f);
@@ -22,7 +63,7 @@ class QuaternionTest : public TestBase
 
             Quaternionr qd = VectorMath::toQuaternion(pitch, roll, yaw);
             if (std::signbit(qd.w()) != std::signbit(q.w())) {
-                qd.coeffs() = - qd.coeffs();
+                qd.coeffs() = -qd.coeffs();
             }
 
             auto dw = std::abs(qd.w() - q.w());

Examples/DepthNav.hpp

@@ -0,0 +1,60 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#pragma once
+
+// includes needed to call RPC APIs
+#include "common/common_utils/StrictMode.hpp"
+STRICT_MODE_OFF
+#ifndef RPCLIB_MSGPACK
+#define RPCLIB_MSGPACK clmdep_msgpack
+#endif // !RPCLIB_MSGPACK
+#include "rpc/rpc_error.h"
+STRICT_MODE_ON
+
+//includes for vector math and other common types
+#include "common/Common.hpp"
+
+namespace msr { namespace airlib {
+
+class DepthNav {
+public: //types
+    struct Params {
+        //Camera FOV
+        float fov = Utils::degreesToRadians(90.0f);
+        //min dot product required for goal vector to be
+        //considered so goal is inside frustum
+        float min_frustrum_alighment = (1.0f + 0.75f) / 2;
+    };
+
+public:
+    DepthNav(const Params& params = Params())
+        : params_(params)
+    {
+    }
+
+    //goal is specified in world frame and typically provided by the global planner
+    //current_pose is current pose of the vehicle in world frame
+    //return next pose that vehicle should be in
+    Pose getNextPose(const Vector3r& goal, const Pose& current_pose)
+    {
+        //get goal in body frame
+        auto goal_body = VectorMath::transformToBodyFrame(goal, current_pose);
+
+        //is goal inside frustum?
+        float frustrum_alighment = std::fabsf(goal_body.dot(VectorMath::front())); 
+        if (frustrum_alighment >= params_.min_frustrum_alighment) {
+
+        }
+        else {
+            //goal is not in the frustum. Let's rotate ourselves until we get 
+            //goal in the frustum
+
+        }
+    }
+
+private:
+    Params params_;
+};
+
+}}

Examples/Examples.vcxproj

@@ -185,6 +185,7 @@
     <ClCompile Include="main.cpp" />
   </ItemGroup>
   <ItemGroup>
+    <ClInclude Include="DepthNav.hpp" />
     <ClInclude Include="GaussianMarkovTest.hpp" />
     <ClInclude Include="RandomPointPoseGenerator.hpp" />
     <ClInclude Include="StandAlonePhysics.hpp" />

Examples/Examples.vcxproj.filters

@@ -35,5 +35,8 @@
     <ClInclude Include="GaussianMarkovTest.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="DepthNav.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>