Added function to convert from a Rot3 to a vector quaternion that works in matlab. Fixed plot2DTrajectory to actually plot poses when there are no marginals

gtsam.h

@@ -418,6 +418,7 @@ virtual class Rot3 : gtsam::Value {
   double pitch() const;
   double yaw() const;
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
+  Vector quaternion() const;
 };
 
 virtual class Pose2 : gtsam::Value {

gtsam/geometry/Rot3.h

@@ -345,6 +345,12 @@ namespace gtsam {
      */
     Quaternion toQuaternion() const;
 
+    /**
+     * Converts to a generic matrix to allow for use with matlab
+     * In format: w x y z
+     */
+    Vector quaternion() const;
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/geometry/Rot3M.cpp

@@ -343,6 +343,17 @@ Quaternion Rot3::toQuaternion() const {
   return Quaternion(rot_);
 }
 
+/* ************************************************************************* */
+Vector Rot3::quaternion() const {
+  Quaternion q = toQuaternion();
+  Vector v(4);
+  v(0) = q.w();
+  v(1) = q.x();
+  v(2) = q.y();
+  v(3) = q.z();
+  return v;
+}
+
 /* ************************************************************************* */
 pair<Matrix3, Vector3> RQ(const Matrix3& A) {
 

matlab/+gtsam/plot2DTrajectory.m

@@ -42,11 +42,15 @@ for i = 0:keys.size-1
 end
 
 % Draw final covariance ellipse
-if ~isempty(lastIndex) && haveMarginals
+if ~isempty(lastIndex)
     lastKey = keys.at(lastIndex);
     lastPose = values.at(lastKey);
-    P = marginals.marginalCovariance(lastKey);
+    if haveMarginals
-    gtsam.plotPose2(lastPose, 'g', P);
+        P = marginals.marginalCovariance(lastKey);
+        gtsam.plotPose2(lastPose, 'g', P);
+    else
+        gtsam.plotPose2(lastPose, 'g', []);
+    end
 end
 
 if ~holdstate