From f4564f444e75bf7405ec666733200379aaff6b61 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Oct 2018 10:47:03 -0400 Subject: [PATCH 1/6] 2D pose plotting --- cython/gtsam/utils/plot.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 3b1897548..19402a080 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -4,6 +4,31 @@ import numpy as np import matplotlib.pyplot as plt +def plot_pose2_on_axes(axes, pose, axis_length=0.1): + """Plot a 2D pose on given axis 'axes' with given 'axis_length'.""" + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + t = pose.translation() + origin = np.array([t.x(), t.y()]) + + # draw the camera axes + x_axis = origin + gRp[:, 0] * axis_length + line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], 'r-') + + y_axis = origin + gRp[:, 1] * axis_length + line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], 'g-') + + +def plot_pose2(fignum, pose, axis_length=0.1): + """Plot a 2D pose on given figure with given 'axis_length'.""" + # get figure object + fig = plt.figure(fignum) + axes = fig.gca() + plot_pose2_on_axes(axes, pose, axis_length) + + def plot_point3_on_axes(axes, point, linespec): """Plot a 3D point on given axis 'axes' with given 'linespec'.""" axes.plot([point.x()], [point.y()], [point.z()], linespec) From 984028697979e37c7ed9b05c09f37334f442a827 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 22 Oct 2018 11:31:12 +0000 Subject: [PATCH 2/6] Fixed noise model, removed extraneous variable --- examples/ImuFactorExample2.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 1137ca214..b4ad5d574 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -57,9 +57,9 @@ int main(int argc, char* argv[]) { Values initialEstimate, totalEstimate, result; // Add a prior on pose x0. This indirectly specifies where the origin is. - // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z. auto noise = noiseModel::Diagonal::Sigmas( - (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); newgraph.push_back(PriorFactor(X(0), pose_0, noise)); // Add imu priors @@ -102,8 +102,6 @@ int main(int argc, char* argv[]) { Vector6 covvec; covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; auto cov = noiseModel::Diagonal::Variances(covvec); - Vector6 zerovec; - zerovec << 0, 0, 0, 0, 0, 0; auto f = boost::make_shared >( b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); From 2a0cee830d7278bb79aafc330f838f6c934e3878 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 23 Oct 2018 19:55:41 +0000 Subject: [PATCH 3/6] Updated outdated comment --- gtsam/inference/Symbol.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 2d7ede8e7..6b5bfcc03 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -26,10 +26,9 @@ namespace gtsam { /** - * Character and index key used in VectorValues, GaussianFactorGraph, - * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol - * keys when linearizing a nonlinear factor graph. This key is not type - * safe, so cannot be used with any Nonlinear* classes. + * Character and index key used to refer to variables. Will simply cast to a Key, + * i.e., a large integer. Keys are used to retrieve values from Values, + * specify what variables factors depend on, etc. */ class GTSAM_EXPORT Symbol { protected: From a1065a6424be380a977e8354947c532fd3fa51b8 Mon Sep 17 00:00:00 2001 From: AltNav NUC Date: Tue, 30 Oct 2018 15:32:37 -0700 Subject: [PATCH 4/6] Added gtsam:: qualifier --- gtsam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index ca37d2d62..24a717c3c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2693,7 +2693,7 @@ virtual class Scenario { virtual class ConstantTwistScenario : gtsam::Scenario { ConstantTwistScenario(Vector w, Vector v); ConstantTwistScenario(Vector w, Vector v, - const Pose3& nTb0); + const gtsam::Pose3& nTb0); }; virtual class AcceleratingScenario : gtsam::Scenario { From 70a9bbc40498b9eb44b2c7cee158597fa3f140a1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 31 Oct 2018 12:51:30 +0000 Subject: [PATCH 5/6] Fix testWrap failure after Duy change --- wrap/tests/expected-cython/geometry.pyx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx index ec69ad081..4bd14b130 100644 --- a/wrap/tests/expected-cython/geometry.pyx +++ b/wrap/tests/expected-cython/geometry.pyx @@ -155,7 +155,7 @@ cdef class Test: def create_ptrs(self): cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().create_ptrs() return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second)) - def __str__(self): + def __repr__(self): strBuf = RedirectCout() self.print_('') return strBuf.str() From 72f71b2ff53a0d325fcc4b7d54a09aa3986c0a26 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 31 Oct 2018 14:01:13 +0000 Subject: [PATCH 6/6] Patched warning, see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1402 --- gtsam/3rdparty/Eigen/Eigen/src/Core/AssignEvaluator.h | 4 ++-- .../Eigen/Eigen/src/Core/products/GeneralMatrixVector.h | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/AssignEvaluator.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/AssignEvaluator.h index b0ec7b7ca..dbe435d86 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/AssignEvaluator.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/AssignEvaluator.h @@ -39,7 +39,7 @@ public: enum { DstAlignment = DstEvaluator::Alignment, SrcAlignment = SrcEvaluator::Alignment, - DstHasDirectAccess = DstFlags & DirectAccessBit, + DstHasDirectAccess = (DstFlags & DirectAccessBit) == DirectAccessBit, JointAlignment = EIGEN_PLAIN_ENUM_MIN(DstAlignment,SrcAlignment) }; @@ -83,7 +83,7 @@ private: && int(OuterStride)!=Dynamic && int(OuterStride)%int(InnerPacketSize)==0 && (EIGEN_UNALIGNED_VECTORIZE || int(JointAlignment)>=int(InnerRequiredAlignment)), MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit), - MayLinearVectorize = bool(MightVectorize) && MayLinearize && DstHasDirectAccess + MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize) && bool(DstHasDirectAccess) && (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)) || MaxSizeAtCompileTime == Dynamic), /* If the destination isn't aligned, we have to do runtime checks and we don't unroll, so it's only good for large enough sizes. */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h index 3c1a7fc40..a597c1f4e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h @@ -183,8 +183,8 @@ EIGEN_DONT_INLINE void general_matrix_vector_product