Merge branch 'develop' into feature/cmake-cython-path

.travis.sh

@@ -63,7 +63,7 @@ function configure()
       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
       -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-      -DCMAKE_VERBOSE_MAKEFILE=ON
+      -DCMAKE_VERBOSE_MAKEFILE=OFF
 }
 
 

.travis.yml

@@ -14,7 +14,8 @@ addons:
     - clang-9
     - build-essential pkg-config
     - cmake
-    - libpython-dev python-numpy
+    - python3-dev libpython-dev
+    - python3-numpy
     - libboost-all-dev
 
 # before_install:

cython/requirements.txt

@@ -1,3 +1,3 @@
 Cython>=0.25.2
 backports_abc>=0.5
-numpy>=1.12.0
+numpy>=1.11.0

gtsam/base/SymmetricBlockMatrix.h

@@ -76,7 +76,7 @@ namespace gtsam {
       blockStart_(0)
     {
       fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
-      matrix_.setZero(variableColOffsets_.back(), variableColOffsets_.back());
+      matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
       assertInvariants();
     }
 
@@ -86,7 +86,7 @@ namespace gtsam {
       blockStart_(0)
     {
       fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
-      matrix_.setZero(variableColOffsets_.back(), variableColOffsets_.back());
+      matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
       assertInvariants();
     }
 
@@ -95,7 +95,7 @@ namespace gtsam {
     SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
       blockStart_(0)
     {
-      matrix_.setZero(matrix.rows(), matrix.cols());
+      matrix_.resize(matrix.rows(), matrix.cols());
       matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
       fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
       if(matrix_.rows() != matrix_.cols())
@@ -416,4 +416,3 @@ namespace gtsam {
   class CholeskyFailed;
 
 }
-

gtsam/slam/dataset.cpp

@@ -37,6 +37,7 @@
 #include <boost/assign/list_inserter.hpp>
 #include <boost/filesystem/operations.hpp>
 #include <boost/filesystem/path.hpp>
+#include <boost/optional.hpp>
 
 #include <cmath>
 #include <fstream>
@@ -541,10 +542,16 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
 }
 
 /* ************************************************************************* */
-BetweenFactorPose3s parse3DFactors(const string& filename) {
+BetweenFactorPose3s parse3DFactors(const string& filename, 
+    const noiseModel::Diagonal::shared_ptr& corruptingNoise) {
   ifstream is(filename.c_str());
   if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
 
+  boost::optional<Sampler> sampler;
+  if (corruptingNoise) {
+    sampler = Sampler(corruptingNoise);
+  }
+
   std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
@@ -585,8 +592,13 @@ BetweenFactorPose3s parse3DFactors(const string& filename) {
       mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
+      auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
+      if (sampler) {
+        R12 = R12.retract(sampler->sample());
+      }
+
       factors.emplace_back(new BetweenFactor<Pose3>(
-          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
+          id1, id2, Pose3(R12, {x, y, z}), model));
     }
   }
   return factors;

gtsam/slam/dataset.h

@@ -159,7 +159,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
 
 /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
 using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
-GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename);
+GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, 
+    const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
 
 /// Parse vertices in 3D TORO graph file into a map of Pose3s.
 GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);