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) 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); 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 { 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