12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added comments and fixed warning

release/4.3a0
Luca 2014-07-25 14:04:41 -04:00
parent b5b1bbfba0
commit 1548c8e34e
4 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/geometry/Unit3.cpp
View File

@ -101,6 +101,7 @@ Matrix Unit3::skew() const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const { Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix Bt = basis().transpose(); Matrix Bt = basis().transpose();
Vector xi = Bt * q.p_.vector(); Vector xi = Bt * q.p_.vector();
if (H) if (H)

2
gtsam/navigation/AttitudeFactor.h
View File

@ -46,7 +46,7 @@ public:
/** /**
* @brief Constructor * @brief Constructor
* @param nZ measured direction in navigation frame * @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)) : AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
nZ_(nZ), bRef_(bRef) { nZ_(nZ), bRef_(bRef) {

2
gtsam/slam/lago.cpp
View File

@ -223,7 +223,7 @@ static PredecessorMap<Key> findOdometricPath(
const NonlinearFactorGraph& pose2Graph) { const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree; PredecessorMap<Key> tree;
Key minKey; Key minKey = keyAnchor; // this initialization does not matter
bool minUnassigned = true; bool minUnassigned = true;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph) { BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph) {

2
gtsam/slam/tests/testLago.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testPlanarSLAMExample_lago.cpp * @file testLago.cpp
* @brief Unit tests for planar SLAM example using the initialization technique * @brief Unit tests for planar SLAM example using the initialization technique
* LAGO (Linear Approximation for Graph Optimization) * LAGO (Linear Approximation for Graph Optimization)
* *