diff --git a/.travis.sh b/.travis.sh index 9fc09a3f8..535a72f4b 100755 --- a/.travis.sh +++ b/.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 } diff --git a/.travis.yml b/.travis.yml index ca6a426ea..d8094ef4d 100644 --- a/.travis.yml +++ b/.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: diff --git a/cython/requirements.txt b/cython/requirements.txt index cd77b097d..8d3c7aeb4 100644 --- a/cython/requirements.txt +++ b/cython/requirements.txt @@ -1,3 +1,3 @@ Cython>=0.25.2 backports_abc>=0.5 -numpy>=1.12.0 +numpy>=1.11.0 diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 1ec9a5ad3..4e030606d 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/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() = matrix.triangularView(); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); if(matrix_.rows() != matrix_.cols()) @@ -416,4 +416,3 @@ namespace gtsam { class CholeskyFailed; } - diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 052bb3343..66e8fc4c0 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -541,10 +542,16 @@ std::map 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; + if (corruptingNoise) { + sampler = Sampler(corruptingNoise); + } + std::vector::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( - id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); + id1, id2, Pose3(R12, {x, y, z}), model)); } } return factors; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 3ab199bab..032799429 100644 --- a/gtsam/slam/dataset.h +++ b/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::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 parse3DPoses(const std::string& filename);