diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index ad6a41bde..3a07b8b98 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -101,6 +101,7 @@ Matrix Unit3::skew() const { /* ************************************************************************* */ Vector Unit3::error(const Unit3& q, boost::optional H) const { + // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix Bt = basis().transpose(); Vector xi = Bt * q.p_.vector(); if (H) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 64603bd99..8d301db72 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -46,7 +46,7 @@ public: /** * @brief Constructor * @param nZ measured direction in navigation frame - * @param bRef reference direction in body frame (default Z-axis) + * @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1]) */ AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) : nZ_(nZ), bRef_(bRef) { diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index abb72021a..1f5d0f2df 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -223,7 +223,7 @@ static PredecessorMap findOdometricPath( const NonlinearFactorGraph& pose2Graph) { PredecessorMap tree; - Key minKey; + Key minKey = keyAnchor; // this initialization does not matter bool minUnassigned = true; BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph) { diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 5e4b2faf2..0d586a051 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testPlanarSLAMExample_lago.cpp + * @file testLago.cpp * @brief Unit tests for planar SLAM example using the initialization technique * LAGO (Linear Approximation for Graph Optimization) *