From 51df17ffdf559453f69ed758e23872612dacf102 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:17:09 -0500 Subject: [PATCH 01/84] Remove virtual functions using raw memory access --- gtsam/linear/GaussianFactor.h | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 8c8e2cb2b..58eaf4460 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -99,9 +99,6 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; - /// Return the diagonal of the Hessian for this factor (raw memory version) - virtual void hessianDiagonal(double* d) const = 0; - /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const = 0; @@ -121,18 +118,9 @@ namespace gtsam { /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; - /// y += alpha * A'*A*x - virtual void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const = 0; - - /// y += alpha * A'*A*x - virtual void multiplyHessianAdd(double alpha, const double* x, double* y) const = 0; - /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; - /// A'*b for Jacobian, eta for Hessian (raw memory version) - virtual void gradientAtZero(double* d) const = 0; - private: /** Serialization function */ friend class boost::serialization::access; From a143815e79e9cbdbbb0298048450fdca3601f96f Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:18:49 -0500 Subject: [PATCH 02/84] Comment out raw memory access parts --- gtsam/linear/GaussianFactorGraph.cpp | 15 +++++++-------- gtsam/linear/GaussianFactorGraph.h | 4 ++-- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 54e721cd7..ad43b675b 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -355,14 +355,13 @@ namespace gtsam { f->multiplyHessianAdd(alpha, x, y); } - /* ************************************************************************* */ - void GaussianFactorGraph::multiplyHessianAdd(double alpha, - const double* x, double* y) const { - vector FactorKeys = getkeydim(); - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) - f->multiplyHessianAdd(alpha, x, y, FactorKeys); - - } + ///* ************************************************************************* */ + //void GaussianFactorGraph::multiplyHessianAdd(double alpha, + // const double* x, double* y) const { + //vector FactorKeys = getkeydim(); + //BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + // f->multiplyHessianAdd(alpha, x, y, FactorKeys); + //} /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 910b25d1e..1861e2607 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -311,8 +311,8 @@ namespace gtsam { VectorValues& y) const; ///** y += alpha*A'A*x */ - void multiplyHessianAdd(double alpha, const double* x, - double* y) const; + //void multiplyHessianAdd(double alpha, const double* x, + // double* y) const; ///** In-place version e <- A*x that overwrites e. */ void multiplyInPlace(const VectorValues& x, Errors& e) const; From 102974588a4baf9c249b78b365b3f100e98bb4e7 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:19:44 -0500 Subject: [PATCH 03/84] Comment out the test for raw memory access --- .../linear/tests/testGaussianFactorGraph.cpp | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index d789c42fd..2fc1e359b 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -316,27 +316,28 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) } /* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplyHessianAdd3 ) -{ - GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); +// Raw memory access +//TEST( GaussianFactorGraph, multiplyHessianAdd3 ) +//{ +// GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); - // brute force - Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); - Vector X(6); X<<1,2,3,4,5,6; - Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; - EXPECT(assert_equal(Y,AtA*X)); +// // brute force +// Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); +// Vector X(6); X<<1,2,3,4,5,6; +// Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; +// EXPECT(assert_equal(Y,AtA*X)); - double* x = &X[0]; +// double* x = &X[0]; - Vector fast_y = gtsam::zero(6); - gfg.multiplyHessianAdd(1.0, x, fast_y.data()); - EXPECT(assert_equal(Y, fast_y)); +// Vector fast_y = gtsam::zero(6); +// gfg.multiplyHessianAdd(1.0, x, fast_y.data()); +// EXPECT(assert_equal(Y, fast_y)); - // now, do it with non-zero y - gfg.multiplyHessianAdd(1.0, x, fast_y.data()); - EXPECT(assert_equal(2*Y, fast_y)); +// // now, do it with non-zero y +// gfg.multiplyHessianAdd(1.0, x, fast_y.data()); +// EXPECT(assert_equal(2*Y, fast_y)); -} +//} /* ************************************************************************* */ From 84f60184812dd54bccd661b4963f7d85bc0f13dc Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:21:06 -0500 Subject: [PATCH 04/84] Remove raw memory access codes --- gtsam/linear/HessianFactor.cpp | 76 ---------------------------------- gtsam/linear/HessianFactor.h | 9 ---- 2 files changed, 85 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f282682b3..b1f8dc6a6 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -358,23 +358,6 @@ VectorValues HessianFactor::hessianDiagonal() const { return d; } -/* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n -void HessianFactor::hessianDiagonal(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - const Matrix& B = info_(pos, pos).selfadjointView(); - DMap(d + 9 * j) += B.diagonal(); - } -} - /* ************************************************************************* */ map HessianFactor::hessianBlockDiagonal() const { map blocks; @@ -548,48 +531,6 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, } } -/* ************************************************************************* */ -void HessianFactor::multiplyHessianAdd(double alpha, const double* x, - double* yvalues, vector offsets) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - // Create a vector of temporary y values, corresponding to rows i - vector y; - y.reserve(size()); - for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); - - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - } - - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; -} - - /* ************************************************************************* */ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; @@ -599,23 +540,6 @@ VectorValues HessianFactor::gradientAtZero() const { return g; } -/* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n -void HessianFactor::gradientAtZero(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - DVector dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + 9 * j) += dj; - } -} - /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 62d84925c..44118cece 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -340,9 +340,6 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; - /* ************************************************************************* */ - virtual void hessianDiagonal(double* d) const; - /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -380,15 +377,9 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /// eta for Hessian VectorValues gradientAtZero() const; - virtual void gradientAtZero(double* d) const; - /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor From fe7fc8a6ef532dc646baf82cb25558301f8446a0 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:21:46 -0500 Subject: [PATCH 05/84] Remove raw memory access codes --- gtsam/linear/JacobianFactor.cpp | 58 --------------------------------- gtsam/linear/JacobianFactor.h | 10 ------ 2 files changed, 68 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a63bbf473..723d84d57 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -460,25 +460,6 @@ VectorValues JacobianFactor::hessianDiagonal() const { return d; } -/* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n -void JacobianFactor::hessianDiagonal(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal - DVector dj; - for (size_t k = 0; k < 9; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - DMap(d + 9 * j) += dj; - } -} - /* ************************************************************************* */ map JacobianFactor::hessianBlockDiagonal() const { map blocks; @@ -528,40 +509,6 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, transposeMultiplyAdd(alpha, Ax, y); } -void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, - double* y, std::vector keys) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) - * ConstDMap(x + keys[keys_[pos]], - keys[keys_[pos] + 1] - keys[keys_[pos]]); - - // Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for (size_t pos = 0; pos < size(); ++pos) - DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - -} - /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; @@ -573,11 +520,6 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } -/* ************************************************************************* */ -void JacobianFactor::gradientAtZero(double* d) const { - //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); -} - /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 99ea91cd9..6057b7819 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -185,9 +185,6 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; - /// Raw memory access version of hessianDiagonal - virtual void hessianDiagonal(double* d) const; - /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -279,16 +276,9 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /// A'*b for Jacobian VectorValues gradientAtZero() const; - /* ************************************************************************* */ - virtual void gradientAtZero(double* d) const; - /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; From fe774981162c04882b183f2bf41992ec6e365025 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:23:33 -0500 Subject: [PATCH 06/84] Modify and add raw memory access for HessianFactor --- gtsam/slam/RegularHessianFactor.h | 90 ++++++++++++++++++++++++++----- 1 file changed, 77 insertions(+), 13 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 0e20bb709..995fd1f04 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -31,6 +31,9 @@ private: typedef Eigen::Matrix MatrixDD; // camera hessian block typedef Eigen::Matrix VectorD; + // Use eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; public: @@ -51,30 +54,75 @@ public: HessianFactor(keys, augmentedInformation) { } + /** Return the diagonal of the Hessian for this factor */ + VectorValues hessianDiagonal() const { + return HessianFactor::hessianDiagonal(); + } + + /** Return the diagonal of the Hessian for this factor (raw memory version) */ + void hessianDiagonal(double* d) const { + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + DMap(d + D * j) += B.diagonal(); + } + } + /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { HessianFactor::multiplyHessianAdd(alpha, x, y); } - // Scratch space for multiplyHessianAdd - typedef Eigen::Matrix DVector; - mutable std::vector y; - - void multiplyHessianAdd(double alpha, const double* x, - double* yvalues) const { + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, + std::vector offsets) const { // Create a vector of temporary y values, corresponding to rows i - y.resize(size()); - BOOST_FOREACH(DVector & yi, y) - yi.setZero(); - - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; + std::vector y; + y.reserve(size()); + for (const_iterator it = begin(); it != end(); it++) + y.push_back(zero(getDim(it))); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after - DVector xj(D); + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) + DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += + alpha * y[i]; + } + + // Scratch space for multiplyHessianAdd + mutable std::vector y; + + void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { + // Create a vector of temporary y values, corresponding to rows i + y.resize(size()); + BOOST_FOREACH(VectorD & yi, y) + yi.setZero(); + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + VectorD xj(D); for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { Key key = keys_[j]; const double* xj = x + key * D; @@ -95,6 +143,22 @@ public: } } + /** eta for Hessian */ + VectorValues gradientAtZero() const { + return HessianFactor::gradientAtZero(); + } + + /** eta for Hessian (raw memory version) */ + void gradientAtZero(double* d) const { + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + VectorD dj = -info_(pos,size()).knownOffDiagonal(); + DMap(d + D * j) += dj; + } + } + }; } From 2acb5a261149de788ba88bd4ce664b129e86ec33 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:25:05 -0500 Subject: [PATCH 07/84] Create JacobianFactor derived class for fixed size and add raw memory access --- gtsam/slam/RegularJacobianFactor.h | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 gtsam/slam/RegularJacobianFactor.h diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h new file mode 100644 index 000000000..0223c66ab --- /dev/null +++ b/gtsam/slam/RegularJacobianFactor.h @@ -0,0 +1,4 @@ +#ifndef REGULARJACOBIANFACTOR_H +#define REGULARJACOBIANFACTOR_H + +#endif // REGULARJACOBIANFACTOR_H From 7a504f3babdc05684cc97b6605788e6524353f8a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:25:28 -0500 Subject: [PATCH 08/84] Create JacobianFactor derived class for fixed size and add raw memory access --- gtsam/slam/RegularJacobianFactor.h | 143 ++++++++++++++++++++++++++++- 1 file changed, 140 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 0223c66ab..a518505ca 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -1,4 +1,141 @@ -#ifndef REGULARJACOBIANFACTOR_H -#define REGULARJACOBIANFACTOR_H +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file RegularJacobianFactor.h + * @brief JacobianFactor class with fixed sized blcoks + * @author Sungtae An + * @date Nov 11, 2014 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +template +class RegularJacobianFactor: public JacobianFactor { + +private: + + typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix VectorD; + // Use eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + +public: + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + RegularJacobianFactor(const TERMS& terms, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(terms, b, model) { + } + + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. */ + template + RegularJacobianFactor(const KEYS& keys, + const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal()) : + JacobianFactor(keys, augmentedMatrix, sigmas) { + } + + /// Return the diagonal of the Hessian for this factor + VectorValues hessianDiagonal() const { + return JacobianFactor::hessianDiagonal(); + } + + /// Raw memory access version of hessianDiagonal + void hessianDiagonal(double* d) const { + // Loop over all variables in the factor + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + // Get the diagonal block, and insert its diagonal + DVector dj; + for (size_t k = 0; k < D; ++k) + dj(k) = Ab_(j).col(k).squaredNorm(); + + DMap(d + D * j) += dj; + } + } + + /// y += alpha * A'*A*x + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + + void multiplyHessianAdd(double alpha, const double* x, double* y, + std::vector keys) const { + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) + * ConstDMap(x + keys[keys_[pos]], + keys[keys_[pos] + 1] - keys[keys_[pos]]); + + // Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( + pos).transpose() * Ax; + } + + void multiplyHessianAdd(double alpha, const double* x, double* y) const { + if (empty()) return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; pos Date: Fri, 14 Nov 2014 14:07:37 -0500 Subject: [PATCH 09/84] Unit test for Regular Hessian Factor --- gtsam/slam/tests/testRegularHessianFactor.cpp | 109 ++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 gtsam/slam/tests/testRegularHessianFactor.cpp diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp new file mode 100644 index 000000000..14de545a6 --- /dev/null +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testRegularHessianFactor.cpp + * @author Frank Dellaert + * @date March 4, 2014 + */ + +#include +#include + +//#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +const double tol = 1e-5; + +/* ************************************************************************* */ +TEST(RegularHessianFactor, ConstructorNWay) +{ + Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114); + Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124); + Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134); + + Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224); + Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234); + + Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334); + + Vector g1 = (Vector(2) << -7, -9); + Vector g2 = (Vector(2) << -9, 1); + Vector g3 = (Vector(2) << 2, 3); + + double f = 10; + + std::vector js; + js.push_back(0); js.push_back(1); js.push_back(3); + std::vector Gs; + Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); + std::vector gs; + gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); + RegularHessianFactor<2> factor(js, Gs, gs, f); + + // multiplyHessianAdd: + { + // brute force + Matrix AtA = factor.information(); + HessianFactor::const_iterator i1 = factor.begin(); + HessianFactor::const_iterator i2 = i1 + 1; + Vector X(6); X << 1,2,3,4,5,6; + Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696; + EXPECT(assert_equal(Y,AtA*X)); + + VectorValues x = map_list_of + (0, (Vector(2) << 1,2)) + (1, (Vector(2) << 3,4)) + (3, (Vector(2) << 5,6)); + + VectorValues expected; + expected.insert(0, Y.segment<2>(0)); + expected.insert(1, Y.segment<2>(2)); + expected.insert(3, Y.segment<2>(4)); + + // VectorValues way + VectorValues actual; + factor.multiplyHessianAdd(1, x, actual); + EXPECT(assert_equal(expected, actual)); + + // now, do it with non-zero y + factor.multiplyHessianAdd(1, x, actual); + EXPECT(assert_equal(2*expected, actual)); + + // RAW ACCESS + Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; + Vector fast_y = gtsam::zero(8); + double xvalues[8] = {1,2,3,4,0,0,5,6}; + factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + EXPECT(assert_equal(expected_y, fast_y)); + + // now, do it with non-zero y + factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + EXPECT(assert_equal(2*expected_y, fast_y)); + + // check some expressions + EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal())); + EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView())); + EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal())); + } +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From 95adc49ac392699b6e8629b569c51d202ab9d97d Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:07:53 -0500 Subject: [PATCH 10/84] Unit test for Regular Jacobian Factor --- .../slam/tests/testRegularJacobianFactor.cpp | 126 ++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 gtsam/slam/tests/testRegularJacobianFactor.cpp diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp new file mode 100644 index 000000000..0377fd3ee --- /dev/null +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -0,0 +1,126 @@ +/** + * @file testRegularJacobianFactor.cpp + * @brief unit test regular jacobian factors + * @author Sungtae An + * @date Nov 12, 2014 + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(5, Matrix3::Identity())) + (make_pair(10, 2*Matrix3::Identity())) + (make_pair(15, 3*Matrix3::Identity())); + + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5)); + } +} + +/* ************************************************************************* */ +TEST(RegularJacobianFactor, constructorNway) +{ + using namespace simple; + JacobianFactor expected(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor<3> actual(terms, b, noise); + + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(noise == expected.get_model()); + EXPECT(noise == actual.get_model()); +} + +TEST(RegularJacobianFactor, hessianDiagonal) +{ + using namespace simple; + JacobianFactor expected(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor<3> actual(terms, b, noise); + + EXPECT(assert_equal(expected.hessianDiagonal(),actual.hessianDiagonal())); + expected.hessianDiagonal().print(); + actual.hessianDiagonal().print(); + double actualValue[9]; + actual.hessianDiagonal(actualValue); + for(int i=0; i<9; ++i) + std::cout << actualValue[i] << std::endl; + + // Why unwhitened? +} + +TEST(RegularJacobian, gradientAtZero) +{ + using namespace simple; + JacobianFactor expected(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor<3> actual(terms, b, noise); + EXPECT(assert_equal(expected.gradientAtZero(),actual.gradientAtZero())); + + // raw memory access is not available now +} + +TEST(RegularJacobian, multiplyHessianAdd) +{ + using namespace simple; + RegularJacobianFactor<3> factor(terms, b, noise); + + VectorValues X; + X.insert(5, (Vector(3) << 10.,20.,30.)); + X.insert(10, (Vector(3) << 10.,20.,30.)); + X.insert(15, (Vector(3) << 10.,20.,30.)); + + VectorValues Y; + Y.insert(5, (Vector(3) << 10.,10.,10.)); + Y.insert(10, (Vector(3) << 20.,20.,20.)); + Y.insert(15, (Vector(3) << 30.,30.,30.)); + + // muultiplyHessianAdd Y += alpha*A'A*X + factor.multiplyHessianAdd(2.0, X, Y); + + VectorValues expectedValues; + expectedValues.insert(5, (Vector(3) << 490.,970.,1450.)); + expectedValues.insert(10, (Vector(3) << 980.,1940.,2900.)); + expectedValues.insert(15, (Vector(3) << 1470.,2910.,4350.)); + + EXPECT(assert_equal(expectedValues,Y)); + + //double dataX[9] = {10., 20., 30., 10., 20., 30., 10., 20., 30.}; + //double dataY[9] = {10., 10., 10., 20., 20., 20., 30., 30., 30.}; + + std::cout << "size: " << factor.size() << std::endl; + double dataX[9] = {0.,}; + double dataY[9] = {0.,}; + + std::vector ks; + ks.push_back(5);ks.push_back(10);ks.push_back(15); + + // Raw memory access version + factor.multiplyHessianAdd(2.0, dataX, dataY, ks); + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From 2733b66a2324ca238ba5b837b790255d54c00c57 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:08:17 -0500 Subject: [PATCH 11/84] Add a few comments --- gtsam/linear/Preconditioner.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 9af362fba..c7f4a5b68 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -125,7 +125,9 @@ void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const void BlockJacobiPreconditioner::build( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { + // n is the number of keys const size_t n = keyInfo.size(); + // dims_ is a vector that contains the dimension of keys dims_ = keyInfo.colSpec(); /* prepare the buffer of block diagonals */ From ee3c7ce1826412ff1773a4c1d494658cac61e4a1 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:09:42 -0500 Subject: [PATCH 12/84] Add a different test for building blocks --- tests/testPreconditioner.cpp | 41 ++++++++++++++++++++++++++++++++---- 1 file changed, 37 insertions(+), 4 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 40d49a934..46b243913 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -42,6 +42,21 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { return fg; } +/* ************************************************************************* */ +static GaussianFactorGraph createSimpleGaussianFactorGraphUnordered() { + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_ + fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg += JacobianFactor(2, -10*eye(2), 1, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2); + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2); + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2); + return fg; +} + /* ************************************************************************* */ // Copy of BlockJacobiPreconditioner::build std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) @@ -89,10 +104,6 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & TEST( Preconditioner, buildBlocks ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - Values initial; - initial.insert(0,Point2(4, 5)); - initial.insert(1,Point2(0, 1)); - initial.insert(2,Point2(-5, 7)); // Expected Hessian block diagonal matrices std::map expectedHessian =gfg.hessianBlockDiagonal(); @@ -110,6 +121,28 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } +/* ************************************************************************* */ +TEST( Preconditioner, buildBlocks2 ) { + // Create simple Gaussian factor graph and initial values + GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =gfg.hessianBlockDiagonal(); + + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(gfg, KeyInfo(gfg)); + + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), 3); + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + for(; it1!=expectedHessian.end(); it1++, it2++) + EXPECT(assert_equal(it1->second, *it2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 03ebcb618545afb61e7a8e0462322673c03ecd31 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:10:08 -0500 Subject: [PATCH 13/84] Add some changes --- gtsam/slam/RegularHessianFactor.h | 117 +++++++++++++++++------------ gtsam/slam/RegularJacobianFactor.h | 70 ++++++++++------- 2 files changed, 110 insertions(+), 77 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 995fd1f04..92e7d92bc 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -31,9 +31,6 @@ private: typedef Eigen::Matrix MatrixDD; // camera hessian block typedef Eigen::Matrix VectorD; - // Use eigen magic to access raw memory - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; public: @@ -54,30 +51,57 @@ public: HessianFactor(keys, augmentedInformation) { } - /** Return the diagonal of the Hessian for this factor */ - VectorValues hessianDiagonal() const { - return HessianFactor::hessianDiagonal(); - } - - /** Return the diagonal of the Hessian for this factor (raw memory version) */ - void hessianDiagonal(double* d) const { - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - const Matrix& B = info_(pos, pos).selfadjointView(); - DMap(d + D * j) += B.diagonal(); - } - } - /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { HessianFactor::multiplyHessianAdd(alpha, x, y); } + // Scratch space for multiplyHessianAdd + typedef Eigen::Matrix DVector; + mutable std::vector y; + + void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { + // Create a vector of temporary y values, corresponding to rows i + y.resize(size()); + BOOST_FOREACH(DVector & yi, y) + yi.setZero(); + + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + DVector xj(D); + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + Key key = keys_[j]; + const double* xj = x + key * D; + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + Key key = keys_[i]; + DMap(yvalues + key * D) += alpha * y[i]; + } + } + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + // Create a vector of temporary y values, corresponding to rows i std::vector y; y.reserve(size()); @@ -110,52 +134,45 @@ public: alpha * y[i]; } - // Scratch space for multiplyHessianAdd - mutable std::vector y; + /** Return the diagonal of the Hessian for this factor */ + VectorValues hessianDiagonal() const { + return HessianFactor::hessianDiagonal(); + } - void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { - // Create a vector of temporary y values, corresponding to rows i - y.resize(size()); - BOOST_FOREACH(VectorD & yi, y) - yi.setZero(); + /** Return the diagonal of the Hessian for this factor (raw memory version) */ + void hessianDiagonal(double* d) const { - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - VectorD xj(D); - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - Key key = keys_[j]; - const double* xj = x + key * D; - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); - } + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { - Key key = keys_[i]; - DMap(yvalues + key * D) += alpha * y[i]; + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + DMap(d + 9 * j) += B.diagonal(); } } - /** eta for Hessian */ VectorValues gradientAtZero() const { return HessianFactor::gradientAtZero(); } - /** eta for Hessian (raw memory version) */ + /* ************************************************************************* */ + // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n void gradientAtZero(double* d) const { - // Loop over all variables in the factor + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal VectorD dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + D * j) += dj; + DMap(d + 9 * j) += dj; } } diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index a518505ca..8b85b74fd 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include @@ -27,14 +28,6 @@ namespace gtsam { template class RegularJacobianFactor: public JacobianFactor { -private: - - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; - // Use eigen magic to access raw memory - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - public: /** Construct an n-ary factor @@ -57,24 +50,6 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /// Return the diagonal of the Hessian for this factor - VectorValues hessianDiagonal() const { - return JacobianFactor::hessianDiagonal(); - } - - /// Raw memory access version of hessianDiagonal - void hessianDiagonal(double* d) const { - // Loop over all variables in the factor - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal - DVector dj; - for (size_t k = 0; k < D; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - DMap(d + D * j) += dj; - } - } - /// y += alpha * A'*A*x void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { @@ -83,16 +58,24 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + if (empty()) return; Vector Ax = zero(Ab_.rows()); // Just iterate over all A matrices and multiply in correct config part for (size_t pos = 0; pos < size(); ++pos) + { + std::cout << "pos: " << pos << " keys_[pos]: " << keys_[pos] << " keys[keys_[pos]]: " << keys[keys_[pos]] << std::endl; Ax += Ab_(pos) * ConstDMap(x + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]); - + } // Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { model_->whitenInPlace(Ax); @@ -109,6 +92,12 @@ public: } void multiplyHessianAdd(double alpha, const double* x, double* y) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -127,6 +116,33 @@ public: DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } + /// Return the diagonal of the Hessian for this factor + VectorValues hessianDiagonal() const { + return JacobianFactor::hessianDiagonal(); + } + + /// Raw memory access version of hessianDiagonal + /* ************************************************************************* */ + // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + void hessianDiagonal(double* d) const { + // Use eigen magic to access raw memory + //typedef Eigen::Matrix DVector; + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + // Get the diagonal block, and insert its diagonal + DVector dj; + //for (size_t k = 0; k < 9; ++k) + for (size_t k = 0; k < D; ++k) + dj(k) = Ab_(j).col(k).squaredNorm(); + + //DMap(d + 9 * j) += dj; + DMap(d + D * j) += dj; + } + } + VectorValues gradientAtZero() const { return JacobianFactor::gradientAtZero(); } From c6faa784e2cc9a65a7ccfc606445638b850b6a3a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 17:41:15 -0500 Subject: [PATCH 14/84] fixed unit test with Luca --- gtsam/slam/RegularJacobianFactor.h | 39 +++-- .../slam/tests/testRegularJacobianFactor.cpp | 158 ++++++++++++------ 2 files changed, 132 insertions(+), 65 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 8b85b74fd..23fa219ab 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -56,8 +56,13 @@ public: JacobianFactor::multiplyHessianAdd(alpha, x, y); } + // Note: this is not assuming a fixed dimension for the variables, + // but requires the vector accumulatedDims to tell the dimension of + // each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + // then accumulatedDims is [0 3 9 11 13] + // NOTE: size of accumulatedDims is size of keys + 1!! void multiplyHessianAdd(double alpha, const double* x, double* y, - std::vector keys) const { + const std::vector& accumulatedDims) const { // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -68,13 +73,13 @@ public: return; Vector Ax = zero(Ab_.rows()); - // Just iterate over all A matrices and multiply in correct config part + // Just iterate over all A matrices and multiply in correct config part (looping over keys) + // E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + // Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) for (size_t pos = 0; pos < size(); ++pos) { - std::cout << "pos: " << pos << " keys_[pos]: " << keys_[pos] << " keys[keys_[pos]]: " << keys[keys_[pos]] << std::endl; Ax += Ab_(pos) - * ConstDMap(x + keys[keys_[pos]], - keys[keys_[pos] + 1] - keys[keys_[pos]]); + * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); } // Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { @@ -85,10 +90,11 @@ public: // multiply with alpha Ax *= alpha; - // Again iterate over all A matrices and insert Ai^e into y - for (size_t pos = 0; pos < size(); ++pos) - DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( + // Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos){ + DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; + } } void multiplyHessianAdd(double alpha, const double* x, double* y) const { @@ -123,10 +129,9 @@ public: /// Raw memory access version of hessianDiagonal /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + // TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n void hessianDiagonal(double* d) const { // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; @@ -134,11 +139,15 @@ public: for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal DVector dj; - //for (size_t k = 0; k < 9; ++k) - for (size_t k = 0; k < D; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - //DMap(d + 9 * j) += dj; + for (size_t k = 0; k < D; ++k){ + if (model_){ + Vector column_k = Ab_(j).col(k); + column_k = model_->whiten(column_k); + dj(k) = dot(column_k, column_k); + }else{ + dj(k) = Ab_(j).col(k).squaredNorm(); + } + } DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index 0377fd3ee..397e5949c 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -22,103 +22,161 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +static const size_t fixedDim = 3; +static const size_t nrKeys = 3; + +// Keys are assumed to be from 0 to n namespace { namespace simple { // Terms we'll use const vector > terms = list_of > - (make_pair(5, Matrix3::Identity())) - (make_pair(10, 2*Matrix3::Identity())) - (make_pair(15, 3*Matrix3::Identity())); + (make_pair(0, Matrix3::Identity())) + (make_pair(1, 2*Matrix3::Identity())) + (make_pair(2, 3*Matrix3::Identity())); // RHS and sigmas const Vector b = (Vector(3) << 1., 2., 3.); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5)); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5)); } } +/* ************************************************************************* */ +// Convert from double* to VectorValues +VectorValues double2vv(const double* x, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + size_t n = nrKeys*dim; + Vector xVec(n); + for (size_t i = 0; i < n; i++){ + xVec(i) = x[i]; + } + return VectorValues(xVec, dims); +} + +/* ************************************************************************* */ +void vv2double(const VectorValues& vv, double* y, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + Vector yvector = vv.vector(dims); + size_t n = nrKeys*dim; + for (size_t j = 0; j < n; j++) + y[j] = yvector[j]; +} + + /* ************************************************************************* */ TEST(RegularJacobianFactor, constructorNway) { using namespace simple; - JacobianFactor expected(terms[0].first, terms[0].second, + JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); - RegularJacobianFactor<3> actual(terms, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); - LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); - EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); - EXPECT(assert_equal(b, expected.getb())); - EXPECT(assert_equal(b, actual.getb())); - EXPECT(noise == expected.get_model()); - EXPECT(noise == actual.get_model()); + LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back()); + EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1))); + EXPECT(assert_equal(b, factor.getb())); + EXPECT(assert_equal(b, regularFactor.getb())); + EXPECT(noise == factor.get_model()); + EXPECT(noise == regularFactor.get_model()); } TEST(RegularJacobianFactor, hessianDiagonal) { using namespace simple; - JacobianFactor expected(terms[0].first, terms[0].second, + JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); - RegularJacobianFactor<3> actual(terms, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); - EXPECT(assert_equal(expected.hessianDiagonal(),actual.hessianDiagonal())); - expected.hessianDiagonal().print(); - actual.hessianDiagonal().print(); + // we compute hessian diagonal from the standard Jacobian + VectorValues expectedHessianDiagonal = factor.hessianDiagonal(); + + // we compute hessian diagonal from the regular Jacobian, but still using the + // implementation of hessianDiagonal in the base class + VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); + + EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); + + // we compare against the Raw memory access implementation of hessianDiagonal double actualValue[9]; - actual.hessianDiagonal(actualValue); - for(int i=0; i<9; ++i) - std::cout << actualValue[i] << std::endl; - - // Why unwhitened? + regularFactor.hessianDiagonal(actualValue); + VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonalRaw)); } +/* ************************************************************************* */ TEST(RegularJacobian, gradientAtZero) { using namespace simple; - JacobianFactor expected(terms[0].first, terms[0].second, + JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); - RegularJacobianFactor<3> actual(terms, b, noise); - EXPECT(assert_equal(expected.gradientAtZero(),actual.gradientAtZero())); + RegularJacobianFactor regularFactor(terms, b, noise); + EXPECT(assert_equal(factor.gradientAtZero(),regularFactor.gradientAtZero())); + // create and test raw memory access version // raw memory access is not available now } +/* ************************************************************************* */ TEST(RegularJacobian, multiplyHessianAdd) { using namespace simple; - RegularJacobianFactor<3> factor(terms, b, noise); + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + // arbitrary vector X VectorValues X; - X.insert(5, (Vector(3) << 10.,20.,30.)); - X.insert(10, (Vector(3) << 10.,20.,30.)); - X.insert(15, (Vector(3) << 10.,20.,30.)); + X.insert(0, (Vector(3) << 10.,20.,30.)); + X.insert(1, (Vector(3) << 10.,20.,30.)); + X.insert(2, (Vector(3) << 10.,20.,30.)); + // arbitrary vector Y VectorValues Y; - Y.insert(5, (Vector(3) << 10.,10.,10.)); - Y.insert(10, (Vector(3) << 20.,20.,20.)); - Y.insert(15, (Vector(3) << 30.,30.,30.)); + Y.insert(0, (Vector(3) << 10.,10.,10.)); + Y.insert(1, (Vector(3) << 20.,20.,20.)); + Y.insert(2, (Vector(3) << 30.,30.,30.)); - // muultiplyHessianAdd Y += alpha*A'A*X - factor.multiplyHessianAdd(2.0, X, Y); + // multiplyHessianAdd Y += alpha*A'A*X + double alpha = 2.0; + VectorValues expectedMHA = Y; + factor.multiplyHessianAdd(alpha, X, expectedMHA); - VectorValues expectedValues; - expectedValues.insert(5, (Vector(3) << 490.,970.,1450.)); - expectedValues.insert(10, (Vector(3) << 980.,1940.,2900.)); - expectedValues.insert(15, (Vector(3) << 1470.,2910.,4350.)); + VectorValues actualMHA = Y; + regularFactor.multiplyHessianAdd(alpha, X, actualMHA); - EXPECT(assert_equal(expectedValues,Y)); + EXPECT(assert_equal(expectedMHA, actualMHA)); - //double dataX[9] = {10., 20., 30., 10., 20., 30., 10., 20., 30.}; - //double dataY[9] = {10., 10., 10., 20., 20., 20., 30., 30., 30.}; + // create data for raw memory access + double XRaw[9]; + vv2double(X, XRaw, nrKeys, fixedDim); - std::cout << "size: " << factor.size() << std::endl; - double dataX[9] = {0.,}; - double dataY[9] = {0.,}; - - std::vector ks; - ks.push_back(5);ks.push_back(10);ks.push_back(15); - - // Raw memory access version - factor.multiplyHessianAdd(2.0, dataX, dataY, ks); + // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y) + double actualMHARaw[9]; + vv2double(Y, actualMHARaw, nrKeys, fixedDim); + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw); + VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV)); + // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) + double actualMHARaw2[9]; + vv2double(Y, actualMHARaw2, nrKeys, fixedDim); + vector dims; + size_t accumulatedDim = 0; + for (size_t i = 0; i < nrKeys+1; i++){ + dims.push_back(accumulatedDim); + accumulatedDim += fixedDim; + } + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims); + VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV2)); } /* ************************************************************************* */ From 174f60762a5e61f1cef9b99c704ec00e7d5c6cb2 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 23:23:16 -0500 Subject: [PATCH 15/84] Add gradientAtZero (Raw memory access) --- gtsam/slam/RegularJacobianFactor.h | 87 ++++++++++++++++++------------ 1 file changed, 52 insertions(+), 35 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 23fa219ab..51a4a27a1 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -28,6 +28,13 @@ namespace gtsam { template class RegularJacobianFactor: public JacobianFactor { +private: + + /** Use eigen magic to access raw memory */ + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + public: /** Construct an n-ary factor @@ -50,60 +57,57 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /// y += alpha * A'*A*x + /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { JacobianFactor::multiplyHessianAdd(alpha, x, y); } - // Note: this is not assuming a fixed dimension for the variables, - // but requires the vector accumulatedDims to tell the dimension of - // each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, - // then accumulatedDims is [0 3 9 11 13] - // NOTE: size of accumulatedDims is size of keys + 1!! + /** Raw memory access version of multiplyHessianAdd + * Note: this is not assuming a fixed dimension for the variables, + * but requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! */ void multiplyHessianAdd(double alpha, const double* x, double* y, const std::vector& accumulatedDims) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; + /// Use eigen magic to access raw memory + typedef Eigen::Matrix VectorD; + typedef Eigen::Map MapD; + typedef Eigen::Map ConstMapD; if (empty()) return; Vector Ax = zero(Ab_.rows()); - // Just iterate over all A matrices and multiply in correct config part (looping over keys) - // E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' - // Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); } - // Deal with noise properly, need to Double* whiten as we are dividing by variance + /// Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); } - // multiply with alpha + /// multiply with alpha Ax *= alpha; - // Again iterate over all A matrices and insert Ai^T into y + /// Again iterate over all A matrices and insert Ai^T into y for (size_t pos = 0; pos < size(); ++pos){ - DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } + /** Raw memory access version of multiplyHessianAdd */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -122,19 +126,16 @@ public: DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } - /// Return the diagonal of the Hessian for this factor - VectorValues hessianDiagonal() const { - return JacobianFactor::hessianDiagonal(); - } +// /// Return the diagonal of the Hessian for this factor +// VectorValues hessianDiagonal() const { +// return JacobianFactor::hessianDiagonal(); +// } - /// Raw memory access version of hessianDiagonal - /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n + /** Raw memory access version of hessianDiagonal + * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n + * + */ void hessianDiagonal(double* d) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal @@ -152,12 +153,28 @@ public: } } + /** // Gradient is really -A'*b / sigma^2 */ VectorValues gradientAtZero() const { return JacobianFactor::gradientAtZero(); } + /** Raw memory access version of gradientAtZero */ void gradientAtZero(double* d) const { - //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); + // Get vector b not weighted + Vector b = getb(); + Vector b_sigma = model_ ? (model_->whiten(b)*model_->whiten(b)) : b; + + // gradient -= A'*b/sigma^2 + // Loop over all variables in the factor + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + // Get the diagonal block, and insert its diagonal + DVector dj; + for (size_t k = 0; k < D; ++k){ + Vector column_k = Ab_(j).col(k); + dj(k) = -1.0*dot(b_sigma,column_k); + } + DMap(d + D * j) += dj; + } } }; From 9f1730809be7e516a658450e38058693bb8cebb7 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 23:23:55 -0500 Subject: [PATCH 16/84] Add unit test for gradientAtZero --- .../slam/tests/testRegularJacobianFactor.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index 397e5949c..efa603aca 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -71,7 +71,6 @@ void vv2double(const VectorValues& vv, double* y, y[j] = yvector[j]; } - /* ************************************************************************* */ TEST(RegularJacobianFactor, constructorNway) { @@ -88,6 +87,7 @@ TEST(RegularJacobianFactor, constructorNway) EXPECT(noise == regularFactor.get_model()); } +/* ************************************************************************* */ TEST(RegularJacobianFactor, hessianDiagonal) { using namespace simple; @@ -100,15 +100,15 @@ TEST(RegularJacobianFactor, hessianDiagonal) // we compute hessian diagonal from the regular Jacobian, but still using the // implementation of hessianDiagonal in the base class - VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); + //VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); - EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); + //EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); // we compare against the Raw memory access implementation of hessianDiagonal double actualValue[9]; regularFactor.hessianDiagonal(actualValue); VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); - EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonalRaw)); + EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw)); } /* ************************************************************************* */ @@ -118,10 +118,22 @@ TEST(RegularJacobian, gradientAtZero) JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); RegularJacobianFactor regularFactor(terms, b, noise); - EXPECT(assert_equal(factor.gradientAtZero(),regularFactor.gradientAtZero())); - // create and test raw memory access version - // raw memory access is not available now + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + // we compute gradient at zero from the regular Jacobian, but still using the + // implementation of gradientAtZero in the base class + VectorValues actualGradientAtZero = regularFactor.gradientAtZero(); + + EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); + } /* ************************************************************************* */ From 960d10582d2d96235af32db2c6e8b1234151f036 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 17 Nov 2014 16:17:11 -0500 Subject: [PATCH 17/84] Add pure virtual function in GaussianFactor and empty dummy virtual function in Jacobian/Hessian Factor for the raw memory access functions --- gtsam/linear/GaussianFactor.h | 6 ++++++ gtsam/linear/HessianFactor.cpp | 14 ++++++++++++++ gtsam/linear/HessianFactor.h | 6 ++++++ gtsam/linear/JacobianFactor.cpp | 14 ++++++++++++++ gtsam/linear/JacobianFactor.h | 6 ++++++ 5 files changed, 46 insertions(+) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 58eaf4460..2b5777e94 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -99,6 +99,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; + /// Raw memory access version of hessianDiagonal + virtual void hessianDiagonal(double* d) const = 0; + /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const = 0; @@ -121,6 +124,9 @@ namespace gtsam { /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; + /// Raw memory access version of gradientAtZero + virtual void gradientAtZero(double* d) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index b1f8dc6a6..e42e4a1f7 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -358,6 +358,13 @@ VectorValues HessianFactor::hessianDiagonal() const { return d; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void HessianFactor::hessianDiagonal(double* d) const { + throw std::runtime_error( + "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ map HessianFactor::hessianBlockDiagonal() const { map blocks; @@ -540,6 +547,13 @@ VectorValues HessianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void HessianFactor::gradientAtZero(double* d) const { + throw std::runtime_error( + "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 44118cece..bcc6f4864 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -340,6 +340,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; + /// Raw memory access version of hessianDiagonal + virtual void hessianDiagonal(double* d) const; + /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -380,6 +383,9 @@ namespace gtsam { /// eta for Hessian VectorValues gradientAtZero() const; + /// Raw memory access version of gradientAtZero + virtual void gradientAtZero(double* d) const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 723d84d57..4e75c5bd6 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -460,6 +460,13 @@ VectorValues JacobianFactor::hessianDiagonal() const { return d; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void JacobianFactor::hessianDiagonal(double* d) const { + throw std::runtime_error( + "JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ map JacobianFactor::hessianBlockDiagonal() const { map blocks; @@ -520,6 +527,13 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void JacobianFactor::gradientAtZero(double* d) const { + throw std::runtime_error( + "JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 6057b7819..32f062928 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -185,6 +185,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; + /// Raw memory access version of hessianDiagonal + virtual void hessianDiagonal(double* d) const; + /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -279,6 +282,9 @@ namespace gtsam { /// A'*b for Jacobian VectorValues gradientAtZero() const; + /// Raw memory access version of gradientAtZero + virtual void gradientAtZero(double* d) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; From cad6736b724b5cd675b62906e139024131146d73 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 17 Nov 2014 16:18:41 -0500 Subject: [PATCH 18/84] Remove non raw memory access functions --- gtsam/slam/RegularHessianFactor.h | 32 +++++++------------ gtsam/slam/RegularImplicitSchurFactor.h | 4 +-- gtsam/slam/RegularJacobianFactor.h | 30 ++++------------- gtsam/slam/tests/testRegularHessianFactor.cpp | 9 ------ .../slam/tests/testRegularJacobianFactor.cpp | 18 +---------- 5 files changed, 21 insertions(+), 72 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 92e7d92bc..f3f90dc2d 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -51,16 +51,11 @@ public: HessianFactor(keys, augmentedInformation) { } - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { - HessianFactor::multiplyHessianAdd(alpha, x, y); - } - // Scratch space for multiplyHessianAdd typedef Eigen::Matrix DVector; mutable std::vector y; + /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i y.resize(size()); @@ -134,16 +129,12 @@ public: alpha * y[i]; } - /** Return the diagonal of the Hessian for this factor */ - VectorValues hessianDiagonal() const { - return HessianFactor::hessianDiagonal(); - } - /** Return the diagonal of the Hessian for this factor (raw memory version) */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; + //typedef Eigen::Matrix DVector; + typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor @@ -151,20 +142,18 @@ public: Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal const Matrix& B = info_(pos, pos).selfadjointView(); - DMap(d + 9 * j) += B.diagonal(); + //DMap(d + 9 * j) += B.diagonal(); + DMap(d + D * j) += B.diagonal(); } } - VectorValues gradientAtZero() const { - return HessianFactor::gradientAtZero(); - } - /* ************************************************************************* */ // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; + //typedef Eigen::Matrix DVector; + typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor @@ -172,7 +161,8 @@ public: Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal VectorD dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + 9 * j) += dj; + //DMap(d + 9 * j) += dj; + DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index f1a11e2cd..21508587e 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -160,7 +160,7 @@ public: * @brief add the contribution of this factor to the diagonal of the hessian * d(output) = d(input) + deltaHessianFactor */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -435,7 +435,7 @@ public: /** * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS */ - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 51a4a27a1..a9759cc26 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -57,13 +57,7 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { - JacobianFactor::multiplyHessianAdd(alpha, x, y); - } - - /** Raw memory access version of multiplyHessianAdd + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x * Note: this is not assuming a fixed dimension for the variables, * but requires the vector accumulatedDims to tell the dimension of * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, @@ -126,16 +120,10 @@ public: DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } -// /// Return the diagonal of the Hessian for this factor -// VectorValues hessianDiagonal() const { -// return JacobianFactor::hessianDiagonal(); -// } - /** Raw memory access version of hessianDiagonal * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - * */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // Loop over all variables in the factor for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal @@ -153,22 +141,18 @@ public: } } - /** // Gradient is really -A'*b / sigma^2 */ - VectorValues gradientAtZero() const { - return JacobianFactor::gradientAtZero(); - } - /** Raw memory access version of gradientAtZero */ - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Get vector b not weighted Vector b = getb(); Vector b_sigma = model_ ? (model_->whiten(b)*model_->whiten(b)) : b; - // gradient -= A'*b/sigma^2 - // Loop over all variables in the factor + // Just iterate over all A matrices for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal + DMap(d + D * j) = DVector::Zero(); DVector dj; + // gradient -= A'*b/sigma^2 + // Computing with each column for (size_t k = 0; k < D; ++k){ Vector column_k = Ab_(j).col(k); dj(k) = -1.0*dot(b_sigma,column_k); diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 14de545a6..7e7f877f4 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -77,15 +77,6 @@ TEST(RegularHessianFactor, ConstructorNWay) expected.insert(1, Y.segment<2>(2)); expected.insert(3, Y.segment<2>(4)); - // VectorValues way - VectorValues actual; - factor.multiplyHessianAdd(1, x, actual); - EXPECT(assert_equal(expected, actual)); - - // now, do it with non-zero y - factor.multiplyHessianAdd(1, x, actual); - EXPECT(assert_equal(2*expected, actual)); - // RAW ACCESS Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; Vector fast_y = gtsam::zero(8); diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index efa603aca..c19f6db8f 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -98,12 +98,6 @@ TEST(RegularJacobianFactor, hessianDiagonal) // we compute hessian diagonal from the standard Jacobian VectorValues expectedHessianDiagonal = factor.hessianDiagonal(); - // we compute hessian diagonal from the regular Jacobian, but still using the - // implementation of hessianDiagonal in the base class - //VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); - - //EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); - // we compare against the Raw memory access implementation of hessianDiagonal double actualValue[9]; regularFactor.hessianDiagonal(actualValue); @@ -122,18 +116,13 @@ TEST(RegularJacobian, gradientAtZero) // we compute gradient at zero from the standard Jacobian VectorValues expectedGradientAtZero = factor.gradientAtZero(); - // we compute gradient at zero from the regular Jacobian, but still using the - // implementation of gradientAtZero in the base class - VectorValues actualGradientAtZero = regularFactor.gradientAtZero(); - - EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); + //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); // we compare against the Raw memory access implementation of gradientAtZero double actualValue[9]; regularFactor.gradientAtZero(actualValue); VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); - } /* ************************************************************************* */ @@ -161,11 +150,6 @@ TEST(RegularJacobian, multiplyHessianAdd) VectorValues expectedMHA = Y; factor.multiplyHessianAdd(alpha, X, expectedMHA); - VectorValues actualMHA = Y; - regularFactor.multiplyHessianAdd(alpha, X, actualMHA); - - EXPECT(assert_equal(expectedMHA, actualMHA)); - // create data for raw memory access double XRaw[9]; vv2double(X, XRaw, nrKeys, fixedDim); From 1f827fae43464f8715ebacb4296d05d958701bcd Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 18:59:29 -0500 Subject: [PATCH 19/84] Fix PCG solver parameter initialization --- gtsam/linear/PCGSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 27eb57b44..db17f1844 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -33,6 +33,7 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { + parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } From 6616844d84a247ab68a3d84bd6a9a50c9fd99e99 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 19:01:02 -0500 Subject: [PATCH 20/84] Fix build function in Preconditioner --- gtsam/linear/Preconditioner.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index c7f4a5b68..7683a0efd 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -143,11 +143,9 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - std::map hessianMap = gf->hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); - } + std::map hessianMap =gfg.hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { From f34c2d941cd448f0f83ea3eecc7529f5b32131bc Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 19:01:41 -0500 Subject: [PATCH 21/84] Add temporary getBuffer function and it should be removed later --- gtsam/linear/Preconditioner.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..61df4414d 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -151,6 +151,10 @@ public: const std::map &lambda ) ; + // Should be removed after test + double* getBuffer() { + return buffer_; + } protected: void clean() ; From e72b3465923069ab623b87526b3e60c146e8abc1 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 19:03:20 -0500 Subject: [PATCH 22/84] Add tests for preconditioner and solver --- tests/testPreconditioner.cpp | 176 +++++++++++++++++++++++++++++++++++ 1 file changed, 176 insertions(+) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 46b243913..4b781201f 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -143,6 +143,182 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } +/* ************************************************************************* */ +TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { + // Ax = [4 1][u] = [1] x0 = [2] + // [1 3][v] [2] [1] + // + // exact solution x = [1/11, 7/11]'; + // + + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); + //simpleGFG.print("Factors\n"); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + + // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' + // the cholesky decomposion of each block of the hessian + // In this example there is a single block (i.e., a single value) + // and the corresponding block of the Hessian is + // + // H0 = [17 7; 7 10] + // + EXPECT(assert_equal(it1->second, *it2)); + // TODO: Matrix expectedH0 ... + //EXPECT(assert_equal(it1->second, *it2)); + + // The corresponding Cholesky decomposition is: + // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) + Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); + preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); + boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + double* buf = blockJacobi->getBuffer(); + for(int i=0; i<4; ++i){} + // TODO: EXPECT(assert_equal(number..,buf[i])); + +} +/* ************************************************************************* */ +TEST( PCGsolver, verySimpleLinearSystem) { + + // Ax = [4 1][u] = [1] x0 = [2] + // [1 3][v] [2] [1] + // + // exact solution x = [1/11, 7/11]'; + // + + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); + //simpleGFG.print("Factors\n"); + + // Exact solution already known + VectorValues exactSolution; + exactSolution.insert(0, (Vector(2) << 1./11., 7./11.)); + //exactSolution.print("Exact"); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); + //deltaDirect.print("Direct"); + + // Solve the system using PCG + // With Dummy preconditioner + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); + + // With Block-Jacobi preconditioner + gtsam::PCGSolverParameters::shared_ptr pcgJacobi = boost::make_shared(); + pcgJacobi->preconditioner_ = boost::make_shared(); + pcgJacobi->setMaxIterations(500); + pcgJacobi->setEpsilon_abs(0.0); + pcgJacobi->setEpsilon_rel(0.0); + VectorValues deltaPCGJacobi = PCGSolver(*pcgJacobi).optimize(simpleGFG); + + // Failed! + EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); +} + +/* ************************************************************************* */ +TEST(PCGSolver, simpleLinearSystem) { + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + //simpleGFG.print("Factors\n"); + + // Expected solution + VectorValues expectedSolution; + expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756)); + expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577)); + expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582)); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); + //expectedSolution.print("Expected"); + //deltaCholesky.print("Direct"); + + // Solve the system using PCG + VectorValues initial; + initial.insert(0, (Vector(2) << 0.1, -0.1)); + initial.insert(1, (Vector(2) << -0.1, 0.1)); + initial.insert(2, (Vector(2) << -0.1, -0.1)); + + // With Dummy preconditioner + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); + // Failed! + EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); + //deltaPCGDummy.print("PCG Dummy"); + + // Solve the system using Preconditioned Conjugate Gradient + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); + // Failed! + EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); + + // Test that the retrieval of the diagonal blocks of the Jacobian are correct. + std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); + std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + + // The corresponding Cholesky decomposition is: + // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) + Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); + preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); + boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + double* buf = blockJacobi->getBuffer(); + for(int i=0; i<4; ++i){} + // TODO: EXPECT(assert_equal(number..,buf[i])); + + size_t i = 0; + for(; it1!=expectedHessian.end(); it1++, it2++){ + EXPECT(assert_equal(it1->second, *it2)); + Matrix R_i(2,2); + R_i(0,0) = buf[i+0]; + R_i(0,1) = buf[i+1]; + R_i(1,0) = buf[i+2]; + R_i(1,1) = buf[i+3]; + + Matrix actualH_i = R_i.transpose() * R_i;// i-th diagonal block + EXPECT(assert_equal(it1->second, actualH_i)); + i += 4; + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From df182b45bcbf96074431fc6fcacfc718d8047c70 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 24 Nov 2014 09:20:43 -0500 Subject: [PATCH 23/84] Add temporary function of getBufferSize which will be removed later --- gtsam/linear/Preconditioner.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 61df4414d..efcb28710 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -155,6 +155,10 @@ public: double* getBuffer() { return buffer_; } + size_t getBufferSize() { + return bufferSize_; + } + protected: void clean() ; From 02131057eea9aff29c86ae1b0843b25c19e984f8 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 24 Nov 2014 09:21:16 -0500 Subject: [PATCH 24/84] Add temporary tests --- tests/testPreconditioner.cpp | 57 ++++++++++++++++++++++++++++++------ 1 file changed, 48 insertions(+), 9 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 4b781201f..bcb59afe7 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -100,7 +100,7 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & return blocks; } -/* ************************************************************************* */ +/* ************************************************************************* TEST( Preconditioner, buildBlocks ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); @@ -121,7 +121,7 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* */ +/* ************************************************************************* TEST( Preconditioner, buildBlocks2 ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); @@ -143,7 +143,7 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* */ +/* ************************************************************************* TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { // Ax = [4 1][u] = [1] x0 = [2] // [1 3][v] [2] [1] @@ -168,26 +168,65 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { std::vector::const_iterator it2 = actualHessian.begin(); // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' - // the cholesky decomposion of each block of the hessian + // the cholesky decomposion of each block of the hessian (column major) // In this example there is a single block (i.e., a single value) // and the corresponding block of the Hessian is // // H0 = [17 7; 7 10] // - EXPECT(assert_equal(it1->second, *it2)); - // TODO: Matrix expectedH0 ... - //EXPECT(assert_equal(it1->second, *it2)); + Matrix expectedH0 = it1->second; + Matrix actualH0 = *it2; + EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10) )); + EXPECT(assert_equal(expectedH0, actualH0)); // The corresponding Cholesky decomposition is: // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + Vector expectedR = (Vector(4) << 4.1231, 0, 1.6977, 2.6679); double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){} - // TODO: EXPECT(assert_equal(number..,buf[i])); + for(int i=0; i<4; ++i){ + EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); + } } + +/* ************************************************************************* */ +TEST( BlockJacobiPreconditioner, SimpleLinerSystem) { + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + + + Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); + preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); + boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + double* buf = blockJacobi->getBuffer(); + for(size_t i=0; igetBufferSize(); ++i){ + std::cout << "buf[" << i << "] = " << buf[i] << std::endl; + } + +} + /* ************************************************************************* */ TEST( PCGsolver, verySimpleLinearSystem) { From f6dd14126deef6b795f5b1c4ee56327ce1c0b637 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Sun, 7 Dec 2014 18:26:09 -0500 Subject: [PATCH 25/84] Revert commits related with fixing PCG (reverted from commit 1f827fae43464f8715ebacb4296d05d958701bcd) --- gtsam/linear/PCGSolver.cpp | 1 - gtsam/linear/Preconditioner.cpp | 8 +- gtsam/linear/Preconditioner.h | 8 -- tests/testPreconditioner.cpp | 219 +------------------------------- 4 files changed, 7 insertions(+), 229 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index db17f1844..27eb57b44 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -33,7 +33,6 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { - parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 7683a0efd..c7f4a5b68 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -143,9 +143,11 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - std::map hessianMap =gfg.hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + std::map hessianMap = gf->hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); + } /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index efcb28710..10ceb78a9 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -151,14 +151,6 @@ public: const std::map &lambda ) ; - // Should be removed after test - double* getBuffer() { - return buffer_; - } - size_t getBufferSize() { - return bufferSize_; - } - protected: void clean() ; diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index bcb59afe7..46b243913 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -100,7 +100,7 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & return blocks; } -/* ************************************************************************* +/* ************************************************************************* */ TEST( Preconditioner, buildBlocks ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); @@ -121,7 +121,7 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* +/* ************************************************************************* */ TEST( Preconditioner, buildBlocks2 ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); @@ -143,221 +143,6 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* -TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { - // Ax = [4 1][u] = [1] x0 = [2] - // [1 3][v] [2] [1] - // - // exact solution x = [1/11, 7/11]'; - // - - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); - //simpleGFG.print("Factors\n"); - - // Expected Hessian block diagonal matrices - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' - // the cholesky decomposion of each block of the hessian (column major) - // In this example there is a single block (i.e., a single value) - // and the corresponding block of the Hessian is - // - // H0 = [17 7; 7 10] - // - Matrix expectedH0 = it1->second; - Matrix actualH0 = *it2; - EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10) )); - EXPECT(assert_equal(expectedH0, actualH0)); - - // The corresponding Cholesky decomposition is: - // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - Vector expectedR = (Vector(4) << 4.1231, 0, 1.6977, 2.6679); - double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){ - EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); - } - -} - -/* ************************************************************************* */ -TEST( BlockJacobiPreconditioner, SimpleLinerSystem) { - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - - // Expected Hessian block diagonal matrices - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - double* buf = blockJacobi->getBuffer(); - for(size_t i=0; igetBufferSize(); ++i){ - std::cout << "buf[" << i << "] = " << buf[i] << std::endl; - } - -} - -/* ************************************************************************* */ -TEST( PCGsolver, verySimpleLinearSystem) { - - // Ax = [4 1][u] = [1] x0 = [2] - // [1 3][v] [2] [1] - // - // exact solution x = [1/11, 7/11]'; - // - - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); - //simpleGFG.print("Factors\n"); - - // Exact solution already known - VectorValues exactSolution; - exactSolution.insert(0, (Vector(2) << 1./11., 7./11.)); - //exactSolution.print("Exact"); - - // Solve the system using direct method - VectorValues deltaDirect = simpleGFG.optimize(); - EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); - //deltaDirect.print("Direct"); - - // Solve the system using PCG - // With Dummy preconditioner - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); - pcg->setMaxIterations(500); - pcg->setEpsilon_abs(0.0); - pcg->setEpsilon_rel(0.0); - //pcg->setVerbosity("ERROR"); - VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); - EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); - - // With Block-Jacobi preconditioner - gtsam::PCGSolverParameters::shared_ptr pcgJacobi = boost::make_shared(); - pcgJacobi->preconditioner_ = boost::make_shared(); - pcgJacobi->setMaxIterations(500); - pcgJacobi->setEpsilon_abs(0.0); - pcgJacobi->setEpsilon_rel(0.0); - VectorValues deltaPCGJacobi = PCGSolver(*pcgJacobi).optimize(simpleGFG); - - // Failed! - EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); - //deltaPCGJacobi.print("PCG Jacobi"); -} - -/* ************************************************************************* */ -TEST(PCGSolver, simpleLinearSystem) { - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - //simpleGFG.print("Factors\n"); - - // Expected solution - VectorValues expectedSolution; - expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756)); - expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577)); - expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582)); - - // Solve the system using direct method - VectorValues deltaDirect = simpleGFG.optimize(); - EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); - //expectedSolution.print("Expected"); - //deltaCholesky.print("Direct"); - - // Solve the system using PCG - VectorValues initial; - initial.insert(0, (Vector(2) << 0.1, -0.1)); - initial.insert(1, (Vector(2) << -0.1, 0.1)); - initial.insert(2, (Vector(2) << -0.1, -0.1)); - - // With Dummy preconditioner - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); - pcg->setMaxIterations(500); - pcg->setEpsilon_abs(0.0); - pcg->setEpsilon_rel(0.0); - //pcg->setVerbosity("ERROR"); - - VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); - // Failed! - EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); - //deltaPCGDummy.print("PCG Dummy"); - - // Solve the system using Preconditioned Conjugate Gradient - pcg->preconditioner_ = boost::make_shared(); - VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); - // Failed! - EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); - //deltaPCGJacobi.print("PCG Jacobi"); - - // Test that the retrieval of the diagonal blocks of the Jacobian are correct. - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - // The corresponding Cholesky decomposition is: - // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){} - // TODO: EXPECT(assert_equal(number..,buf[i])); - - size_t i = 0; - for(; it1!=expectedHessian.end(); it1++, it2++){ - EXPECT(assert_equal(it1->second, *it2)); - Matrix R_i(2,2); - R_i(0,0) = buf[i+0]; - R_i(0,1) = buf[i+1]; - R_i(1,0) = buf[i+2]; - R_i(1,1) = buf[i+3]; - - Matrix actualH_i = R_i.transpose() * R_i;// i-th diagonal block - EXPECT(assert_equal(it1->second, actualH_i)); - i += 4; - } -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 0a1e844ae5d1633c62bc67d285cb0ac6930ae0ae Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 8 Dec 2014 00:44:36 -0500 Subject: [PATCH 26/84] Clean up --- gtsam/linear/GaussianFactorGraph.cpp | 51 +++---------------- gtsam/linear/GaussianFactorGraph.h | 4 -- .../linear/tests/testGaussianFactorGraph.cpp | 26 ---------- 3 files changed, 8 insertions(+), 73 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index ad43b675b..4dd1af4e7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -355,14 +355,6 @@ namespace gtsam { f->multiplyHessianAdd(alpha, x, y); } - ///* ************************************************************************* */ - //void GaussianFactorGraph::multiplyHessianAdd(double alpha, - // const double* x, double* y) const { - //vector FactorKeys = getkeydim(); - //BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) - // f->multiplyHessianAdd(alpha, x, y, FactorKeys); - //} - /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { multiplyInPlace(x, e.begin()); @@ -392,42 +384,15 @@ namespace gtsam { /* ************************************************************************* */ // x += alpha*A'*e -void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, - VectorValues& x) const { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha, *(ei++), x); + void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } } -} - - ///* ************************************************************************* */ - //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - // Key i = 0 ; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // r[i] = Ai->getb(); - // i++; - // } - // VectorValues Ax = VectorValues::SameStructure(r); - // multiply(fg,x,Ax); - // axpy(-1.0,Ax,r); - //} - - ///* ************************************************************************* */ - //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - // r.setZero(); - // Key i = 0; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // Vector &y = r[i]; - // for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - // y += Ai->getA(j) * x[*j]; - // } - // ++i; - // } - //} /* ************************************************************************* */ VectorValues GaussianFactorGraph::transposeMultiply(const Errors& e) const diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 1861e2607..9b8a91a6e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -310,10 +310,6 @@ namespace gtsam { void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - ///** y += alpha*A'A*x */ - //void multiplyHessianAdd(double alpha, const double* x, - // double* y) const; - ///** In-place version e <- A*x that overwrites e. */ void multiplyInPlace(const VectorValues& x, Errors& e) const; diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 2fc1e359b..49a6e30e4 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -315,31 +315,6 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(2*expected, actual)); } -/* ************************************************************************* */ -// Raw memory access -//TEST( GaussianFactorGraph, multiplyHessianAdd3 ) -//{ -// GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); - -// // brute force -// Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); -// Vector X(6); X<<1,2,3,4,5,6; -// Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; -// EXPECT(assert_equal(Y,AtA*X)); - -// double* x = &X[0]; - -// Vector fast_y = gtsam::zero(6); -// gfg.multiplyHessianAdd(1.0, x, fast_y.data()); -// EXPECT(assert_equal(Y, fast_y)); - -// // now, do it with non-zero y -// gfg.multiplyHessianAdd(1.0, x, fast_y.data()); -// EXPECT(assert_equal(2*Y, fast_y)); - -//} - - /* ************************************************************************* */ TEST( GaussianFactorGraph, matricesMixed ) { @@ -352,7 +327,6 @@ TEST( GaussianFactorGraph, matricesMixed ) EXPECT(assert_equal(A.transpose()*b, eta)); } - /* ************************************************************************* */ TEST( GaussianFactorGraph, gradientAtZero ) { From d64af0d626f7a44c59b65b5603cb3d7ce61365c2 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Sat, 13 Dec 2014 17:02:38 -0500 Subject: [PATCH 27/84] Modify gradientAtZero in RegularJacobianFactor --- gtsam/slam/RegularJacobianFactor.h | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index a9759cc26..e6e240389 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -143,19 +143,30 @@ public: /** Raw memory access version of gradientAtZero */ virtual void gradientAtZero(double* d) const { - // Get vector b not weighted - Vector b = getb(); - Vector b_sigma = model_ ? (model_->whiten(b)*model_->whiten(b)) : b; - // Just iterate over all A matrices + // Initialize d as a zero vector for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DMap(d + D * j) = DVector::Zero(); + } + + // Get vector b not weighted + Vector b = getb(); + + // Whitening b + if (model_) { + b = model_->whiten(b); + b = model_->whiten(b); + } + + // Just iterate over all A matrices + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column for (size_t k = 0; k < D; ++k){ - Vector column_k = Ab_(j).col(k); - dj(k) = -1.0*dot(b_sigma,column_k); + Vector column_k = Ab_(pos).col(k); + dj(k) = -1.0*dot(b, column_k); } DMap(d + D * j) += dj; } From 1affae697c574e778388ffb65a1a3c1f2e43d36e Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 22 Dec 2014 14:50:26 -0500 Subject: [PATCH 28/84] Correct gradientAtZero: remove zero initialization --- gtsam/slam/RegularJacobianFactor.h | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index e6e240389..bb275ef3f 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -141,14 +141,11 @@ public: } } - /** Raw memory access version of gradientAtZero */ + /** Raw memory access version of gradientAtZero + * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n + */ virtual void gradientAtZero(double* d) const { - // Initialize d as a zero vector - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DMap(d + D * j) = DVector::Zero(); - } - // Get vector b not weighted Vector b = getb(); @@ -159,13 +156,12 @@ public: } // Just iterate over all A matrices - for (size_t pos = 0; pos < size(); ++pos) { - Key j = keys_[pos]; + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column for (size_t k = 0; k < D; ++k){ - Vector column_k = Ab_(pos).col(k); + Vector column_k = Ab_(j).col(k); dj(k) = -1.0*dot(b, column_k); } DMap(d + D * j) += dj; From 82a2d7f029fbeeb247e18dc5f63c08467e4ab648 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 22 Dec 2014 14:51:23 -0500 Subject: [PATCH 29/84] Add a unit test for gradientAtZero with multi-factors --- .../slam/tests/testRegularJacobianFactor.cpp | 48 ++++++++++++++++++- 1 file changed, 46 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index c19f6db8f..16273d23f 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -38,6 +38,17 @@ namespace { const Vector b = (Vector(3) << 1., 2., 3.); const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5)); } + + namespace simple2 { + // Terms + const vector > terms2 = list_of > + (make_pair(0, 2*Matrix3::Identity())) + (make_pair(1, 4*Matrix3::Identity())) + (make_pair(2, 6*Matrix3::Identity())); + + // RHS + const Vector b2 = (Vector(3) << 2., 4., 6.); + } } /* ************************************************************************* */ @@ -99,7 +110,7 @@ TEST(RegularJacobianFactor, hessianDiagonal) VectorValues expectedHessianDiagonal = factor.hessianDiagonal(); // we compare against the Raw memory access implementation of hessianDiagonal - double actualValue[9]; + double actualValue[9]={0}; regularFactor.hessianDiagonal(actualValue); VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw)); @@ -119,12 +130,45 @@ TEST(RegularJacobian, gradientAtZero) //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); // we compare against the Raw memory access implementation of gradientAtZero - double actualValue[9]; + double actualValue[9]={0}; regularFactor.gradientAtZero(actualValue); VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); } +/* ************************************************************************* */ +TEST(RegularJacobian, gradientAtZero_multiFactors) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]={0}; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); + + // One more factor + using namespace simple2; + JacobianFactor factor2(terms2[0].first, terms2[0].second, + terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise); + RegularJacobianFactor regularFactor2(terms2, b2, noise); + + // we accumulate computed gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero()); + + // we compare against the Raw memory access implementation of gradientAtZero + regularFactor2.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2)); + +} + /* ************************************************************************* */ TEST(RegularJacobian, multiplyHessianAdd) { From 7e0033208c2d3b635dbd14246e869ec7fcd254f8 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 29 Dec 2014 14:48:48 -0500 Subject: [PATCH 30/84] JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor. --- gtsam/slam/JacobianFactorQ.h | 18 +++--- gtsam/slam/JacobianFactorQR.h | 16 +++-- gtsam/slam/JacobianFactorSVD.h | 12 ++-- gtsam/slam/JacobianSchurFactor.h | 93 ------------------------------ gtsam/slam/RegularJacobianFactor.h | 57 +++++++++++++++--- 5 files changed, 77 insertions(+), 119 deletions(-) delete mode 100644 gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f4dfb9422..957041abc 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -6,16 +6,20 @@ #pragma once -#include "JacobianSchurFactor.h" +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; +private: + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; public: @@ -25,7 +29,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -37,10 +41,10 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + RegularJacobianFactor() { size_t j = 0, m2 = E.rows(), m = m2 / 2; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -50,7 +54,7 @@ public: std::vector < KeyMatrix > QF; QF.reserve(m); // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index a928106a8..30faf9ec0 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,31 +6,35 @@ */ #pragma once -#include +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; +private: + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + RegularJacobianFactor() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) { gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, b.segment<2>(2 * i), model); i += 1; diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e28185038..0f4c52038 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,27 +5,29 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public RegularJacobianFactor { -public: +private: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; +public: + /// Default constructor JacobianFactorSVD() {} /// Empty constructor with keys JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -37,7 +39,7 @@ public: /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; // PLAIN NULL SPACE TRICK diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 2beecc264..000000000 --- a/gtsam/slam/JacobianSchurFactor.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * @file JacobianSchurFactor.h - * @brief Jacobianfactor that combines and eliminates points - * @date Oct 27, 2013 - * @uthor Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -/** - * JacobianFactor for Schur complement that uses Q noise model - */ -template -class JacobianSchurFactor: public JacobianFactor { - -public: - - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); - if (empty()) return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; pos +#include #include #include #include @@ -30,6 +32,9 @@ class RegularJacobianFactor: public JacobianFactor { private: + typedef JacobianFactor Base; + typedef RegularJacobianFactor This; + /** Use eigen magic to access raw memory */ typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; @@ -37,6 +42,10 @@ private: public: + /// Default constructor + RegularJacobianFactor() { + } + /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the * collection of keys and matrices making up the factor. */ @@ -57,6 +66,33 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhiten(Ax) : Ax; + } + + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const + { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; pos& accumulatedDims) const { - /// Use eigen magic to access raw memory - typedef Eigen::Matrix VectorD; - typedef Eigen::Map MapD; - typedef Eigen::Map ConstMapD; - if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -81,7 +112,7 @@ public: for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); } /// Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { @@ -94,12 +125,15 @@ public: /// Again iterate over all A matrices and insert Ai^T into y for (size_t pos = 0; pos < size(); ++pos){ - MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } - /** Raw memory access version of multiplyHessianAdd */ + /** + * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; @@ -168,6 +202,13 @@ public: } } + /** Non-RAW memory access versions for testRegularImplicitSchurFactor + * TODO: They should be removed from Regular Factors + */ + using Base::multiplyHessianAdd; + using Base::hessianDiagonal; + using Base::gradientAtZero; + }; } From 7091c2bd2ebc1f216c9b31e6fb14afe2a29ba553 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 1 Jan 2015 17:35:58 -0500 Subject: [PATCH 31/84] To resolve conflicts first, revert the commitment of merging JacobianSchurFactor into RegularJacobianFactor. JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor. (reverted from commit 7e0033208c2d3b635dbd14246e869ec7fcd254f8) --- gtsam/slam/JacobianFactorQ.h | 18 +++--- gtsam/slam/JacobianFactorQR.h | 16 ++--- gtsam/slam/JacobianFactorSVD.h | 12 ++-- gtsam/slam/JacobianSchurFactor.h | 93 ++++++++++++++++++++++++++++++ gtsam/slam/RegularJacobianFactor.h | 57 +++--------------- 5 files changed, 119 insertions(+), 77 deletions(-) create mode 100644 gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 957041abc..f4dfb9422 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -6,20 +6,16 @@ #pragma once -#include +#include "JacobianSchurFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public RegularJacobianFactor { +class JacobianFactorQ: public JacobianSchurFactor { -private: - - typedef RegularJacobianFactor Base; - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; + typedef JacobianSchurFactor Base; public: @@ -29,7 +25,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -41,10 +37,10 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - RegularJacobianFactor() { + JacobianSchurFactor() { size_t j = 0, m2 = E.rows(), m = m2 / 2; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -54,7 +50,7 @@ public: std::vector < KeyMatrix > QF; QF.reserve(m); // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 30faf9ec0..a928106a8 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,35 +6,31 @@ */ #pragma once -#include +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public RegularJacobianFactor { +class JacobianFactorQR: public JacobianSchurFactor { -private: - - typedef RegularJacobianFactor Base; - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; + typedef JacobianSchurFactor Base; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - RegularJacobianFactor() { + JacobianSchurFactor() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) { + BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, b.segment<2>(2 * i), model); i += 1; diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 0f4c52038..e28185038 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,29 +5,27 @@ */ #pragma once -#include +#include "gtsam/slam/JacobianSchurFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public RegularJacobianFactor { +class JacobianFactorSVD: public JacobianSchurFactor { -private: +public: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; -public: - /// Default constructor JacobianFactorSVD() {} /// Empty constructor with keys JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -39,7 +37,7 @@ public: /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; // PLAIN NULL SPACE TRICK diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h new file mode 100644 index 000000000..2beecc264 --- /dev/null +++ b/gtsam/slam/JacobianSchurFactor.h @@ -0,0 +1,93 @@ +/* + * @file JacobianSchurFactor.h + * @brief Jacobianfactor that combines and eliminates points + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianSchurFactor: public JacobianFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhiten(Ax) : Ax; + } + + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const + { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); + if (empty()) return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; pos -#include #include #include #include @@ -32,9 +30,6 @@ class RegularJacobianFactor: public JacobianFactor { private: - typedef JacobianFactor Base; - typedef RegularJacobianFactor This; - /** Use eigen magic to access raw memory */ typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; @@ -42,10 +37,6 @@ private: public: - /// Default constructor - RegularJacobianFactor() { - } - /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the * collection of keys and matrices making up the factor. */ @@ -66,33 +57,6 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; pos& accumulatedDims) const { + /// Use eigen magic to access raw memory + typedef Eigen::Matrix VectorD; + typedef Eigen::Map MapD; + typedef Eigen::Map ConstMapD; + if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -112,7 +81,7 @@ public: for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); } /// Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { @@ -125,15 +94,12 @@ public: /// Again iterate over all A matrices and insert Ai^T into y for (size_t pos = 0; pos < size(); ++pos){ - DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } - /** - * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) - * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way - */ + /** Raw memory access version of multiplyHessianAdd */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; @@ -202,13 +168,6 @@ public: } } - /** Non-RAW memory access versions for testRegularImplicitSchurFactor - * TODO: They should be removed from Regular Factors - */ - using Base::multiplyHessianAdd; - using Base::hessianDiagonal; - using Base::gradientAtZero; - }; } From fcd705f716bf53184a1ceea06e69eabefde4dd55 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 1 Jan 2015 18:12:45 -0500 Subject: [PATCH 32/84] Correct Eigen comma initialization --- gtsam/slam/tests/testRegularHessianFactor.cpp | 24 +++++++++---------- .../slam/tests/testRegularJacobianFactor.cpp | 18 +++++++------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 7e7f877f4..8dfb753f4 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -34,18 +34,18 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(RegularHessianFactor, ConstructorNWay) { - Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114); - Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124); - Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134); + Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished(); + Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished(); + Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished(); - Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224); - Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234); + Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished(); + Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished(); - Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334); + Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished(); - Vector g1 = (Vector(2) << -7, -9); - Vector g2 = (Vector(2) << -9, 1); - Vector g3 = (Vector(2) << 2, 3); + Vector g1 = (Vector(2) << -7, -9).finished(); + Vector g2 = (Vector(2) << -9, 1).finished(); + Vector g3 = (Vector(2) << 2, 3).finished(); double f = 10; @@ -68,9 +68,9 @@ TEST(RegularHessianFactor, ConstructorNWay) EXPECT(assert_equal(Y,AtA*X)); VectorValues x = map_list_of - (0, (Vector(2) << 1,2)) - (1, (Vector(2) << 3,4)) - (3, (Vector(2) << 5,6)); + (0, (Vector(2) << 1,2).finished()) + (1, (Vector(2) << 3,4).finished()) + (3, (Vector(2) << 5,6).finished()); VectorValues expected; expected.insert(0, Y.segment<2>(0)); diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index 16273d23f..b56b65d7b 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -35,8 +35,8 @@ namespace { (make_pair(2, 3*Matrix3::Identity())); // RHS and sigmas - const Vector b = (Vector(3) << 1., 2., 3.); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5)); + const Vector b = (Vector(3) << 1., 2., 3.).finished(); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished()); } namespace simple2 { @@ -47,7 +47,7 @@ namespace { (make_pair(2, 6*Matrix3::Identity())); // RHS - const Vector b2 = (Vector(3) << 2., 4., 6.); + const Vector b2 = (Vector(3) << 2., 4., 6.).finished(); } } @@ -179,15 +179,15 @@ TEST(RegularJacobian, multiplyHessianAdd) // arbitrary vector X VectorValues X; - X.insert(0, (Vector(3) << 10.,20.,30.)); - X.insert(1, (Vector(3) << 10.,20.,30.)); - X.insert(2, (Vector(3) << 10.,20.,30.)); + X.insert(0, (Vector(3) << 10.,20.,30.).finished()); + X.insert(1, (Vector(3) << 10.,20.,30.).finished()); + X.insert(2, (Vector(3) << 10.,20.,30.).finished()); // arbitrary vector Y VectorValues Y; - Y.insert(0, (Vector(3) << 10.,10.,10.)); - Y.insert(1, (Vector(3) << 20.,20.,20.)); - Y.insert(2, (Vector(3) << 30.,30.,30.)); + Y.insert(0, (Vector(3) << 10.,10.,10.).finished()); + Y.insert(1, (Vector(3) << 20.,20.,20.).finished()); + Y.insert(2, (Vector(3) << 30.,30.,30.).finished()); // multiplyHessianAdd Y += alpha*A'A*X double alpha = 2.0; From 6732beb1b445c4598ec6d54b254d82a82c1e64d2 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 7 Jan 2015 09:56:18 -0500 Subject: [PATCH 33/84] a cylinder MATLAB object --- matlab/gtsam_examples/CameraFlyingExample.m | 28 +++++++++++++++++++ .../gtsam_examples/cylinderSampleProjection.m | 18 ++++++++++++ matlab/gtsam_examples/cylinderSampling.m | 22 +++++++++++++++ 3 files changed, 68 insertions(+) create mode 100644 matlab/gtsam_examples/CameraFlyingExample.m create mode 100644 matlab/gtsam_examples/cylinderSampleProjection.m create mode 100644 matlab/gtsam_examples/cylinderSampling.m diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m new file mode 100644 index 000000000..018ffbb4a --- /dev/null +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -0,0 +1,28 @@ +clear all; +clc; +import gtsam.* + +cylinder_num = 10; +cylinders = cell(cylinder_num, 1); + +% generate a set of cylinders +for i = 1:cylinder_num + cylinder_center = Point2([10, 5 * i]'); + cylinders{i,1} = cylinderSampling(cylinder_center, 1, 5, 30); +end + +% visibility validation +%camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); + +K = Cal3_S2(525,525,0,320,240); +cam_pose = Pose3(); +cam = SimpleCamera(cam_pose, K); + +% the projections of all visible 3D points +visiblePoints3 = cylinderSampleProjection(cam, cam_pose, cylinders); + +% + +% + + diff --git a/matlab/gtsam_examples/cylinderSampleProjection.m b/matlab/gtsam_examples/cylinderSampleProjection.m new file mode 100644 index 000000000..30b7d97c1 --- /dev/null +++ b/matlab/gtsam_examples/cylinderSampleProjection.m @@ -0,0 +1,18 @@ +function [] = cylinderSampleProjection(Cam, Pose3, cylinders) + + cylinder_num = size(cylinders, 1); + for i = 1:cylinder_num + cylinder = cylinders{i}; + + point_num = size(cylinder.Points, 1); + % to be finished + + % for j = 1:point_num +% +% cylinderPoints = +% +% end + + end + +end \ No newline at end of file diff --git a/matlab/gtsam_examples/cylinderSampling.m b/matlab/gtsam_examples/cylinderSampling.m new file mode 100644 index 000000000..3e6a6d9a8 --- /dev/null +++ b/matlab/gtsam_examples/cylinderSampling.m @@ -0,0 +1,22 @@ +function [cylinder] = cylinderSampling(Point2, radius, height, density) +% + import gtsam.* + % calculate the cylinder area + A = 2 * pi * radius * height; + + PointsNum = round(A * density); + + Points3 = cell(PointsNum, 1); + + % sample the points + for i = 1:PointsNum + theta = 2 * pi * rand; + x = radius * cos(theta) + Point2.x; + y = radius * sin(theta) + Point2.y; + z = height * rand; + Points3{i,1} = Point3([x,y,z]'); + end + + cylinder.area = A; + cylinder.Points = Points3; +end \ No newline at end of file From 9485553d99ec23aec0486a311d4184fca95869a3 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Fri, 9 Jan 2015 10:33:53 -0500 Subject: [PATCH 34/84] random sample cylinders and plot them on the fields --- matlab/+gtsam/plotCylinderSamples.m | 31 ++++++++++++ matlab/gtsam_examples/CameraFlyingExample.m | 28 ++++++----- .../gtsam_examples/cylinderSampleProjection.m | 47 ++++++++++++++----- matlab/gtsam_examples/cylinderSampling.m | 23 +++++---- 4 files changed, 96 insertions(+), 33 deletions(-) create mode 100644 matlab/+gtsam/plotCylinderSamples.m diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m new file mode 100644 index 000000000..d1fe981db --- /dev/null +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -0,0 +1,31 @@ +function plotCylinderSamples(cylinders, fieldSize) + + holdstate = ishold; + hold on + + num = size(cylinders, 1); + + sampleDensity = 120; + figure + + for i = 1:num + %base.z = cylinders{i}.centroid.z - cylinders{i}.height/2; + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); + + X = X + cylinders{i}.centroid.x; + Y = Y + cylinders{i}.centroid.y; + Z = Z * cylinders{i}.height; + + cylinderHandle = surf(X,Y,Z); + set(cylinderHandle, 'FaceAlpha', 0.5); + hold on + end + + axis equal + axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]); + + if ~holdstate + hold off + end + +end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 018ffbb4a..3a2a8991a 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -2,27 +2,33 @@ clear all; clc; import gtsam.* -cylinder_num = 10; +cylinder_num = 20; cylinders = cell(cylinder_num, 1); % generate a set of cylinders +fieldSize = Point2([100, 100]'); + +% random generate cylinders on the fields for i = 1:cylinder_num - cylinder_center = Point2([10, 5 * i]'); - cylinders{i,1} = cylinderSampling(cylinder_center, 1, 5, 30); + baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); + cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 30); end +% plot all the cylinders and sampled points +% now is plotting on a 100 * 100 field +plotCylinderSamples(cylinders, fieldSize); + % visibility validation -%camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); K = Cal3_S2(525,525,0,320,240); -cam_pose = Pose3(); -cam = SimpleCamera(cam_pose, K); +cameraPose = Pose3(); % now set to be default % the projections of all visible 3D points -visiblePoints3 = cylinderSampleProjection(cam, cam_pose, cylinders); - -% - -% +% visiblePoints3 = cylinderSampleProjection(K, cameraPose, cylinders); + + + + + diff --git a/matlab/gtsam_examples/cylinderSampleProjection.m b/matlab/gtsam_examples/cylinderSampleProjection.m index 30b7d97c1..33e031bc8 100644 --- a/matlab/gtsam_examples/cylinderSampleProjection.m +++ b/matlab/gtsam_examples/cylinderSampleProjection.m @@ -1,18 +1,41 @@ -function [] = cylinderSampleProjection(Cam, Pose3, cylinders) - +function [] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) +% Project sampled points on cylinder to camera frame +% Authors: Zhaoyang Lv + cylinder_num = size(cylinders, 1); + + camera = SimpleCamera(cameraPose, K); + for i = 1:cylinder_num - cylinder = cylinders{i}; + + point_num = size( cylinders{i}.Points, 1); - point_num = size(cylinder.Points, 1); - % to be finished - - % for j = 1:point_num -% -% cylinderPoints = -% -% end + % to check point visibility + for j = 1:point_num + sampledPoint3 = cylinders{i}.Poinsts{j}; + measurements2d = camera.project(sampledPoint3); + + % ignore points not visible in the scene + if measurements2d.x < 0 || measurements.x >= imageSize.x ... + || measurements2d.y < 0 || measurements.y >= imageSize.y + continue; + end + % ignore points occluded + % use a simple math hack to check occlusion: + % 1. All points in front of cylinders' surfaces are visible + % 2. For points behind the cylinders' surfaces, the cylinder + for k = 1:cylinder_num + + rayCameraToPoint = sampledPoint3 - cameraPose.t; + rayCameraToCylinder = cylinders{i} - cameraPose.t; + + projectedRay = dot(rayCameraToPoint, rayCameraToCylinder); + distCameraToCylinder = norm(rayCameraToCylinder); + + + end + end end -end \ No newline at end of file +end diff --git a/matlab/gtsam_examples/cylinderSampling.m b/matlab/gtsam_examples/cylinderSampling.m index 3e6a6d9a8..2ee304da1 100644 --- a/matlab/gtsam_examples/cylinderSampling.m +++ b/matlab/gtsam_examples/cylinderSampling.m @@ -1,22 +1,25 @@ -function [cylinder] = cylinderSampling(Point2, radius, height, density) +function [cylinder] = cylinderSampling(baseCentroid, radius, height, density) % import gtsam.* % calculate the cylinder area - A = 2 * pi * radius * height; + area = 2 * pi * radius * height; - PointsNum = round(A * density); + pointsNum = round(area * density); - Points3 = cell(PointsNum, 1); + points3 = cell(pointsNum, 1); % sample the points - for i = 1:PointsNum + for i = 1:pointsNum theta = 2 * pi * rand; - x = radius * cos(theta) + Point2.x; - y = radius * sin(theta) + Point2.y; + x = radius * cos(theta) + baseCentroid.x; + y = radius * sin(theta) + baseCentroid.y; z = height * rand; - Points3{i,1} = Point3([x,y,z]'); + points3{i,1} = Point3([x,y,z]'); end - cylinder.area = A; - cylinder.Points = Points3; + cylinder.area = area; + cylinder.radius = radius; + cylinder.height = height; + cylinder.Points = points3; + cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2); end \ No newline at end of file From 5564aea3328e37e490b0f207d8d2551758d1740c Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:20:37 -0500 Subject: [PATCH 35/84] calculate all the visible points from a camera view --- matlab/+gtsam/cylinderSampleProjection.m | 64 ++++++++++++++++++++++++ matlab/+gtsam/plotCylinderSamples.m | 13 +++-- 2 files changed, 73 insertions(+), 4 deletions(-) create mode 100644 matlab/+gtsam/cylinderSampleProjection.m diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m new file mode 100644 index 000000000..d74726956 --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -0,0 +1,64 @@ +function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) +% Project sampled points on cylinder to camera frame +% Authors: Zhaoyang Lv + + import gtsam.* + + cylinder_num = size(cylinders, 1); + + %camera = SimpleCamera(cameraPose, K); + camera = SimpleCamera.Lookat(cameraPose.translation(), cylinders{10}.centroid, Point3([0,0,1]'), K); + + visiblePoints = {}; + visiblePointsCylinderIdx = []; + + for i = 1:cylinder_num + + point_num = size( cylinders{i}.Points, 1); + + % to check point visibility + for j = 1:point_num + sampledPoint3 = cylinders{i}.Points{j}; + measurements2d = camera.project(sampledPoint3); + + % ignore points not visible in the scene + if measurements2d.x < 0 || measurements2d.x >= imageSize.x ... + || measurements2d.y < 0 || measurements2d.y >= imageSize.y + continue; + end + + % ignore points occluded + % use a simple math hack to check occlusion: + % 1. All points in front of cylinders' surfaces are visible + % 2. For points behind the cylinders' surfaces, the cylinder + for k = 1:cylinder_num + + rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); + rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); + + % Condition 1: all points in front of the cylinders' + % surfaces are visible + if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 + visiblePoints{end+1} = sampledPoint3; + visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; + continue; + end + + % Condition 2 + projectedRay = dot(rayCameraToCylinder, rayCameraToPoint); + if projectedRay > 0 + rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; + if rayCylinderToProjected(1) > cylinders{i}.radius && ... + rayCylinderToProjected(2) > cylinders{i}.radius + visiblePoints{end+1} = sampledPoint3; + visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; + end + end + + end + end + + end + +end diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m index d1fe981db..6c8a2249e 100644 --- a/matlab/+gtsam/plotCylinderSamples.m +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -1,4 +1,8 @@ -function plotCylinderSamples(cylinders, fieldSize) +function plotCylinderSamples(cylinders, fieldSize, figID) +% plot the cylinders on the given field +% @author: Zhaoyang Lv + + figure(figID); holdstate = ishold; hold on @@ -6,10 +10,9 @@ function plotCylinderSamples(cylinders, fieldSize) num = size(cylinders, 1); sampleDensity = 120; - figure + - for i = 1:num - %base.z = cylinders{i}.centroid.z - cylinders{i}.height/2; + for i = 1:num [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); X = X + cylinders{i}.centroid.x; @@ -24,6 +27,8 @@ function plotCylinderSamples(cylinders, fieldSize) axis equal axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]); + grid on + if ~holdstate hold off end From d5bebb93d2069760f67afb99178a798401ed60d6 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:20:50 -0500 Subject: [PATCH 36/84] plot the visible samples on cylinders --- matlab/+gtsam/plotProjectedCylinderSamples.m | 36 ++++++++++++++++++ matlab/+gtsam/points2DTrackMonocular.m | 40 ++++++++++++++++++++ 2 files changed, 76 insertions(+) create mode 100644 matlab/+gtsam/plotProjectedCylinderSamples.m create mode 100644 matlab/+gtsam/points2DTrackMonocular.m diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m new file mode 100644 index 000000000..5d9a06713 --- /dev/null +++ b/matlab/+gtsam/plotProjectedCylinderSamples.m @@ -0,0 +1,36 @@ +function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID) +% plot the visible projected points on the cylinders +% author: Zhaoyang Lv + + import gtsam.* + + figure(figID); + + holdstate = ishold; + hold on + + %plotCamera(cameraPose, 5); + + pointsNum = size(visiblePoints3, 1) + + for i=1:pointsNum + ray = visiblePoints3{i}.between(cameraPose.translation()).vector(); + dist = norm(ray); + + p = plot3(visiblePoints3{i}.x, visiblePoints3{i}.y, visiblePoints3{i}.z, ... + 'o', 'MarkerFaceColor', 'Green'); + + for t=0:0.1:dist + marchingRay = ray * t; + p.XData = visiblePoints3{i}.x + marchingRay(1); + p.YData = visiblePoints3{i}.y + marchingRay(2); + p.ZData = visiblePoints3{i}.z + marchingRay(3); + drawnow update + end + + end + + if ~holdstate + hold off + end +end \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m new file mode 100644 index 000000000..1c1e641bf --- /dev/null +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -0,0 +1,40 @@ +function pts2dTracksmon = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) +% Assess how accurately we can reconstruct points from a particular monocular camera setup. +% After creation of the factor graph for each track, linearize it around ground truth. +% There is no optimization +% @author: Zhaoyang Lv + +import gtsam.* + +%% create graph +graph = NonlinearFactorGraph; + +%% add a constraint on the starting pose +poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; +posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); +firstPose = cameraPoses{1}; +graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise)); + +cameraPosesNum = size(cameraPoses, 1); + +%% add measurements +initialEstimate = Values; +for i = 1:cameraPosesNum + [visiblePoints3, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPoses{i}, imageSize, cylinders); + + pointsNum = size(visiblePoints, 1); + + %% not finished + %for j = 1:pointsNum + % graph.add(); + %end +end + +marginals = Marginals(graph, initialEstimate); + +% should use all the points num to replace the num 100 +for i = 1:100 + marginals.marginalCovariance(symbol('p',i)); +end + +end From 377c46281854958bea5e6047d33da41774d22402 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:21:59 -0500 Subject: [PATCH 37/84] cylinderSampling moved to gtsam+ folder --- matlab/+gtsam/cylinderSampling.m | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 matlab/+gtsam/cylinderSampling.m diff --git a/matlab/+gtsam/cylinderSampling.m b/matlab/+gtsam/cylinderSampling.m new file mode 100644 index 000000000..dcaa9c721 --- /dev/null +++ b/matlab/+gtsam/cylinderSampling.m @@ -0,0 +1,26 @@ +function [cylinder] = cylinderSampling(baseCentroid, radius, height, density) +% +% @author: Zhaoyang Lv + import gtsam.* + % calculate the cylinder area + area = 2 * pi * radius * height; + + pointsNum = round(area * density); + + points3 = cell(pointsNum, 1); + + % sample the points + for i = 1:pointsNum + theta = 2 * pi * rand; + x = radius * cos(theta) + baseCentroid.x; + y = radius * sin(theta) + baseCentroid.y; + z = height * rand; + points3{i,1} = Point3([x,y,z]'); + end + + cylinder.area = area; + cylinder.radius = radius; + cylinder.height = height; + cylinder.Points = points3; + cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2); +end \ No newline at end of file From b45e81725b60460e6a35b5b526c0050da2cc257c Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:22:42 -0500 Subject: [PATCH 38/84] an update of function 1&2 cameraFlyingExample. function3&4 in construction --- matlab/gtsam_examples/CameraFlyingExample.m | 34 +++++++++++++++++---- 1 file changed, 28 insertions(+), 6 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 3a2a8991a..4d6e38720 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -2,7 +2,7 @@ clear all; clc; import gtsam.* -cylinder_num = 20; +cylinder_num = 10; cylinders = cell(cylinder_num, 1); % generate a set of cylinders @@ -11,21 +11,43 @@ fieldSize = Point2([100, 100]'); % random generate cylinders on the fields for i = 1:cylinder_num baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); - cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 30); + cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end % plot all the cylinders and sampled points % now is plotting on a 100 * 100 field -plotCylinderSamples(cylinders, fieldSize); +figID = 1; +figure(figID); +plotCylinderSamples(cylinders, fieldSize, figID); % visibility validation +% generate camera trajectories K = Cal3_S2(525,525,0,320,240); -cameraPose = Pose3(); % now set to be default +imageSize = Point2([640, 480]'); +poseNum = 10; +cameraPoses = cell(poseNum, 1); +cameraPoses{1} = Pose3(); +cameras = cell(poseNum, 1); +for i = 2:poseNum + incRot = Rot3.RzRyRx(0,0,pi/4); + incT = Point3(5*rand, 5*rand, 5*rand); + cameraPoses{i} = cameraPoses{i-1}.compose(Pose3(incRot, incT)); +end -% the projections of all visible 3D points -% visiblePoints3 = cylinderSampleProjection(K, cameraPose, cylinders); +[visiblePoints3, ~] = cylinderSampleProjection(K, cameraPoses{1}, imageSize, cylinders); +plotPose3(cameraPoses{1}, 5 ) +% plot all the projected points +plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); + +pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders); + +%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders); + + +% ToDo: plot the trajectories +%plot3DTrajectory(); From a8bf2a4da1b1b8334ee41151891b1170fbb5ffce Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 16:10:49 -0500 Subject: [PATCH 39/84] function3 add graph measurement and initial estimate --- matlab/+gtsam/cylinderSampleProjection.m | 123 ++++++++++-------- matlab/+gtsam/points2DTrackMonocular.m | 53 ++++++-- matlab/gtsam_examples/CameraFlyingExample.m | 36 ++--- .../gtsam_examples/cylinderSampleProjection.m | 41 ------ matlab/gtsam_examples/cylinderSampling.m | 25 ---- matlab/unstable_examples/.gitignore | 1 + 6 files changed, 131 insertions(+), 148 deletions(-) delete mode 100644 matlab/gtsam_examples/cylinderSampleProjection.m delete mode 100644 matlab/gtsam_examples/cylinderSampling.m diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index d74726956..5cb5c64df 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -1,64 +1,83 @@ -function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) -% Project sampled points on cylinder to camera frame -% Authors: Zhaoyang Lv +function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders) - import gtsam.* +% Input: +% Output: +% visiblePoints: data{k} 3D Point in overal point clouds with index k +% Z{k} 2D measurements in overal point clouds with index k +% index {i}{j} +% i: the cylinder index; +% j: the point index on the cylinder; +% +% @Description: Project sampled points on cylinder to camera frame +% @Authors: Zhaoyang Lv - cylinder_num = size(cylinders, 1); - - %camera = SimpleCamera(cameraPose, K); - camera = SimpleCamera.Lookat(cameraPose.translation(), cylinders{10}.centroid, Point3([0,0,1]'), K); +import gtsam.* - visiblePoints = {}; - visiblePointsCylinderIdx = []; +%% memory allocation +cylinderNum = length(cylinders); +visiblePoints.index = cell(cylinderNum); + +pointCloudNum = 0; +for i = 1:cylinderNum + pointCloudNum = pointCloudNum + length(cylinders{i}.Points); + visiblePoints.index{i} = cell(pointCloudNum); +end +visiblePoints.data = cell(pointCloudNum); + +%% check visiblity of points on each cylinder +pointCloudIndex = 0; +for i = 1:cylinderNum - for i = 1:cylinder_num + pointNum = length(cylinders{i}.Points); - point_num = size( cylinders{i}.Points, 1); + % to check point visibility + for j = 1:pointNum + + pointCloudIndex = pointCloudIndex + 1; - % to check point visibility - for j = 1:point_num - sampledPoint3 = cylinders{i}.Points{j}; - measurements2d = camera.project(sampledPoint3); - - % ignore points not visible in the scene - if measurements2d.x < 0 || measurements2d.x >= imageSize.x ... - || measurements2d.y < 0 || measurements2d.y >= imageSize.y - continue; - end - - % ignore points occluded - % use a simple math hack to check occlusion: - % 1. All points in front of cylinders' surfaces are visible - % 2. For points behind the cylinders' surfaces, the cylinder - for k = 1:cylinder_num - - rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); - rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); - rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); - - % Condition 1: all points in front of the cylinders' - % surfaces are visible - if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 - visiblePoints{end+1} = sampledPoint3; - visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; - continue; - end + sampledPoint3 = cylinders{i}.Points{j}; + Z2d = camera.project(sampledPoint3); - % Condition 2 - projectedRay = dot(rayCameraToCylinder, rayCameraToPoint); - if projectedRay > 0 - rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; - if rayCylinderToProjected(1) > cylinders{i}.radius && ... - rayCylinderToProjected(2) > cylinders{i}.radius - visiblePoints{end+1} = sampledPoint3; - visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; - end - end - + % ignore points not visible in the scene + if Z2d.x < 0 || Z2d.x >= imageSize.x ... + || Z2d.y < 0 || Z2d.y >= imageSize.y + continue; + end + + % ignore points occluded + % use a simple math hack to check occlusion: + % 1. All points in front of cylinders' surfaces are visible + % 2. For points behind the cylinders' surfaces, the cylinder + for k = 1:cylinderNum + + rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); + rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); + + % Condition 1: all points in front of the cylinders' + % surfaces are visible + if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 + visiblePoints.data{pointCloudIndex} = sampledPoint3; + visiblePoints.Z{pointCloudIndex} = Z2d; + visiblePoints.index{i}{j} = pointCloudIndex; + continue; end + + % Condition 2 + projectedRay = dot(rayCameraToCylinder, rayCameraToPoint); + if projectedRay > 0 + rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; + if rayCylinderToProjected(1) > cylinders{i}.radius && ... + rayCylinderToProjected(2) > cylinders{i}.radius + visiblePoints.data{pointCloudIndex} = sampledPoints3; + visiblePoints.Z{pointCloudIndex} = Z2d; + visiblePoints.index{i}{j} = pointCloudIndex; + end + end + end - end + +end end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 1c1e641bf..c0830bde7 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -1,4 +1,4 @@ -function pts2dTracksmon = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) +function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) % Assess how accurately we can reconstruct points from a particular monocular camera setup. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization @@ -9,32 +9,59 @@ import gtsam.* %% create graph graph = NonlinearFactorGraph; -%% add a constraint on the starting pose +pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; + +%% add a constraint on the starting pose posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); firstPose = cameraPoses{1}; graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise)); -cameraPosesNum = size(cameraPoses, 1); +cameraPosesNum = length(cameraPoses); -%% add measurements +%% add measurements and initial camera & points values +pointsNum = 0; +cylinderNum = length(cylinders); +for i = 1:cylinderNum + pointsNum = pointsNum + length(cylinders{i}.Points); +end + +measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); + +pts3d = {}; initialEstimate = Values; for i = 1:cameraPosesNum - [visiblePoints3, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPoses{i}, imageSize, cylinders); + camera = SimpleCamera(K, cameraPoses{i}); - pointsNum = size(visiblePoints, 1); + pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); + pts3d.camera{i} = camera; + + for j = 1:length(pts3d.pts{i}.Z) + graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), camera.K) ); + + point_j = pts3d.pts{i}.data{j}.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', j), point_j); + end + + pose_i = camera.pose.retract(0.1*randn(6,1)); + initialEstimate.insert(symbole('x', i), pose_i); - %% not finished - %for j = 1:pointsNum - % graph.add(); - %end end +%% Print the graph +graph.print(sprintf('\nFactor graph:\n')); + marginals = Marginals(graph, initialEstimate); -% should use all the points num to replace the num 100 -for i = 1:100 - marginals.marginalCovariance(symbol('p',i)); +%% get all the 2d points track information +ptIdx = 0; +for i = 1:pointsNum + if isempty(pts3d.pts{i}) + continue; + end + %pts2dTrackMono.pts2d = pts3d.pts{i} + pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); end end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 4d6e38720..13dcbfbe4 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -2,47 +2,49 @@ clear all; clc; import gtsam.* +%% generate a set of cylinders and Samples +fieldSize = Point2([100, 100]'); cylinder_num = 10; cylinders = cell(cylinder_num, 1); -% generate a set of cylinders -fieldSize = Point2([100, 100]'); - -% random generate cylinders on the fields -for i = 1:cylinder_num +for i = 1:cylinderNum baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end -% plot all the cylinders and sampled points +%% plot all the cylinders and sampled points % now is plotting on a 100 * 100 field figID = 1; figure(figID); plotCylinderSamples(cylinders, fieldSize, figID); -% visibility validation -% generate camera trajectories +%% generate camera trajectories K = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); poseNum = 10; -cameraPoses = cell(poseNum, 1); -cameraPoses{1} = Pose3(); cameras = cell(poseNum, 1); -for i = 2:poseNum - incRot = Rot3.RzRyRx(0,0,pi/4); +trans = Point3(); +% To ensure there are landmarks in view, look at one randomly chosen cylinder +% each time. +for i = 1:poseNum + camera = SimpleCamera.Lookat(trans, cylinders{round(cylinderNum*rand)}.centroid, ... + Point3([0,0,1]'), K); + incT = Point3(5*rand, 5*rand, 5*rand); - cameraPoses{i} = cameraPoses{i-1}.compose(Pose3(incRot, incT)); + trans = trans.compose(incT); end -[visiblePoints3, ~] = cylinderSampleProjection(K, cameraPoses{1}, imageSize, cylinders); +%% visibility validation +visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders); -plotPose3(cameraPoses{1}, 5 ) -% plot all the projected points -plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); +%% plot all the projected points +%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); +%% setp up monocular camera and get measurements pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders); +%% set up stereo camera and get measurements %pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders); diff --git a/matlab/gtsam_examples/cylinderSampleProjection.m b/matlab/gtsam_examples/cylinderSampleProjection.m deleted file mode 100644 index 33e031bc8..000000000 --- a/matlab/gtsam_examples/cylinderSampleProjection.m +++ /dev/null @@ -1,41 +0,0 @@ -function [] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) -% Project sampled points on cylinder to camera frame -% Authors: Zhaoyang Lv - - cylinder_num = size(cylinders, 1); - - camera = SimpleCamera(cameraPose, K); - - for i = 1:cylinder_num - - point_num = size( cylinders{i}.Points, 1); - - % to check point visibility - for j = 1:point_num - sampledPoint3 = cylinders{i}.Poinsts{j}; - measurements2d = camera.project(sampledPoint3); - - % ignore points not visible in the scene - if measurements2d.x < 0 || measurements.x >= imageSize.x ... - || measurements2d.y < 0 || measurements.y >= imageSize.y - continue; - end - % ignore points occluded - % use a simple math hack to check occlusion: - % 1. All points in front of cylinders' surfaces are visible - % 2. For points behind the cylinders' surfaces, the cylinder - for k = 1:cylinder_num - - rayCameraToPoint = sampledPoint3 - cameraPose.t; - rayCameraToCylinder = cylinders{i} - cameraPose.t; - - projectedRay = dot(rayCameraToPoint, rayCameraToCylinder); - distCameraToCylinder = norm(rayCameraToCylinder); - - - end - end - - end - -end diff --git a/matlab/gtsam_examples/cylinderSampling.m b/matlab/gtsam_examples/cylinderSampling.m deleted file mode 100644 index 2ee304da1..000000000 --- a/matlab/gtsam_examples/cylinderSampling.m +++ /dev/null @@ -1,25 +0,0 @@ -function [cylinder] = cylinderSampling(baseCentroid, radius, height, density) -% - import gtsam.* - % calculate the cylinder area - area = 2 * pi * radius * height; - - pointsNum = round(area * density); - - points3 = cell(pointsNum, 1); - - % sample the points - for i = 1:pointsNum - theta = 2 * pi * rand; - x = radius * cos(theta) + baseCentroid.x; - y = radius * sin(theta) + baseCentroid.y; - z = height * rand; - points3{i,1} = Point3([x,y,z]'); - end - - cylinder.area = area; - cylinder.radius = radius; - cylinder.height = height; - cylinder.Points = points3; - cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2); -end \ No newline at end of file diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore index 6d725d9bc..c47168f67 100644 --- a/matlab/unstable_examples/.gitignore +++ b/matlab/unstable_examples/.gitignore @@ -1 +1,2 @@ *.m~ +*.avi From 39f5aa499e6f9009d5acadf5b953b65abf4b803d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 23:27:06 -0500 Subject: [PATCH 40/84] 2D monocular track. Testing with random data now throws indeterminant linear system exception --- matlab/+gtsam/cylinderSampleProjection.m | 19 +++++--- matlab/+gtsam/points2DTrackMonocular.m | 57 +++++++++++++++++------- 2 files changed, 52 insertions(+), 24 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 5cb5c64df..4a8c3b06a 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -15,14 +15,15 @@ import gtsam.* %% memory allocation cylinderNum = length(cylinders); -visiblePoints.index = cell(cylinderNum); +visiblePoints.index = cell(cylinderNum,1); pointCloudNum = 0; for i = 1:cylinderNum pointCloudNum = pointCloudNum + length(cylinders{i}.Points); - visiblePoints.index{i} = cell(pointCloudNum); + visiblePoints.index{i} = cell(pointCloudNum,1); end -visiblePoints.data = cell(pointCloudNum); +visiblePoints.data = cell(pointCloudNum,1); +visiblePoints.Z = cell(pointCloudNum, 1); %% check visiblity of points on each cylinder pointCloudIndex = 0; @@ -34,8 +35,12 @@ for i = 1:cylinderNum for j = 1:pointNum pointCloudIndex = pointCloudIndex + 1; - + sampledPoint3 = cylinders{i}.Points{j}; + sampledPoint3local = camera.pose.transform_to(sampledPoint3); + if sampledPoint3local.z < 0 + continue; % Cheirality Exception + end Z2d = camera.project(sampledPoint3); % ignore points not visible in the scene @@ -50,8 +55,8 @@ for i = 1:cylinderNum % 2. For points behind the cylinders' surfaces, the cylinder for k = 1:cylinderNum - rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); - rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); + rayCameraToPoint = camera.pose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = camera.pose.translation().between(cylinders{i}.centroid).vector(); rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); % Condition 1: all points in front of the cylinders' @@ -69,7 +74,7 @@ for i = 1:cylinderNum rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; if rayCylinderToProjected(1) > cylinders{i}.radius && ... rayCylinderToProjected(2) > cylinders{i}.radius - visiblePoints.data{pointCloudIndex} = sampledPoints3; + visiblePoints.data{pointCloudIndex} = sampledPoint3; visiblePoints.Z{pointCloudIndex} = Z2d; visiblePoints.index{i}{j} = pointCloudIndex; end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index c0830bde7..9bdd746ae 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -1,4 +1,4 @@ -function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) +function pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders) % Assess how accurately we can reconstruct points from a particular monocular camera setup. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization @@ -9,15 +9,15 @@ import gtsam.* %% create graph graph = NonlinearFactorGraph; +%% create the noise factors pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; - -%% add a constraint on the starting pose +measurementNoiseSigma = 1.0; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -firstPose = cameraPoses{1}; -graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise)); +pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); +measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); -cameraPosesNum = length(cameraPoses); +cameraPosesNum = length(cameras); %% add measurements and initial camera & points values pointsNum = 0; @@ -26,27 +26,50 @@ for i = 1:cylinderNum pointsNum = pointsNum + length(cylinders{i}.Points); end -measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); - pts3d = {}; initialEstimate = Values; +initialized = false; for i = 1:cameraPosesNum - camera = SimpleCamera(K, cameraPoses{i}); + % add a constraint on the starting pose + camera = cameras{i}; pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); pts3d.camera{i} = camera; - for j = 1:length(pts3d.pts{i}.Z) - graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', j), camera.K) ); + if ~initialized + graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + k = 0; + if ~isempty(pts3d.pts{i}.data{1+k}) + graph.add(PriorFactorPoint3(symbol('p', 1), ... + pts3d.pts{i}.data{1+k}, pointPriorNoise)); + else + k = k+1; + end + initialized = true; + end - point_j = pts3d.pts{i}.data{j}.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', j), point_j); + for j = 1:length(pts3d.pts{i}.Z) + if isempty(pts3d.pts{i}.Z{j}) + continue; + end + graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) ); end +end + +%% initialize cameras and points close to ground truth +for i = 1:cameraPosesNum pose_i = camera.pose.retract(0.1*randn(6,1)); - initialEstimate.insert(symbole('x', i), pose_i); - + initialEstimate.insert(symbol('x', i), pose_i); +end +ptsIdx = 0; +for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + ptsIdx = ptsIdx + 1; + point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', ptsIdx), point_j); + end end %% Print the graph @@ -55,12 +78,12 @@ graph.print(sprintf('\nFactor graph:\n')); marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information +% currently throws the Indeterminant linear system exception ptIdx = 0; for i = 1:pointsNum if isempty(pts3d.pts{i}) continue; end - %pts2dTrackMono.pts2d = pts3d.pts{i} pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); end From 678aabce3eb9ecd1aaa1fab79d7bb1aada865529 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 23:27:21 -0500 Subject: [PATCH 41/84] add stereo set up --- matlab/+gtsam/points2DTrackStereo.m | 90 +++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 matlab/+gtsam/points2DTrackStereo.m diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m new file mode 100644 index 000000000..41f6668b1 --- /dev/null +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -0,0 +1,90 @@ +function pts2dTracksStereo = points2DTrackStereo(cameras, imageSize, cylinders) +% Assess how accurately we can reconstruct points from a particular monocular camera setup. +% After creation of the factor graph for each track, linearize it around ground truth. +% There is no optimization +% @author: Zhaoyang Lv + +import gtsam.* + +%% create graph +graph = NonlinearFactorGraph; + +%% create the noise factors +pointNoiseSigma = 0.1; +poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; +measurementNoiseSigma = 1.0; +posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); +pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); +measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); + +cameraPosesNum = length(cameras); + +%% add measurements and initial camera & points values +pointsNum = 0; +cylinderNum = length(cylinders); +for i = 1:cylinderNum + pointsNum = pointsNum + length(cylinders{i}.Points); +end + +pts3d = {}; +initialEstimate = Values; +initialized = false; +for i = 1:cameraPosesNum + % add a constraint on the starting pose + camera = cameras{i}; + + pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); + pts3d.camera{i} = camera; + + if ~initialized + graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + k = 0; + if ~isempty(pts3d.pts{i}.data{1+k}) + graph.add(PriorFactorPoint3(symbol('p', 1), ... + pts3d.pts{i}.data{1+k}, pointPriorNoise)); + else + k = k+1; + end + initialized = true; + end + + for j = 1:length(pts3d.pts{i}.Z) + if isempty(pts3d.pts{i}.Z{j}) + continue; + end + graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) ); + end + +end + +%% initialize cameras and points close to ground truth +for i = 1:cameraPosesNum + pose_i = camera.pose.retract(0.1*randn(6,1)); + initialEstimate.insert(symbol('x', i), pose_i); +end +ptsIdx = 0; +for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + ptsIdx = ptsIdx + 1; + point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', ptsIdx), point_j); + end +end + +%% Print the graph +graph.print(sprintf('\nFactor graph:\n')); + +marginals = Marginals(graph, initialEstimate); + +%% get all the 2d points track information +% currently throws the Indeterminant linear system exception +ptIdx = 0; +for i = 1:pointsNum + if isempty(pts3d.pts{i}) + continue; + end + pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); +end + +end From 10dc767eda622637d565b6df010b4000b671f31d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 23:27:50 -0500 Subject: [PATCH 42/84] change monocular set up and add stereo test. Still under test --- matlab/gtsam_examples/CameraFlyingExample.m | 34 ++++++++++++++++----- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 13dcbfbe4..258bd968b 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -1,12 +1,15 @@ clear all; clc; +clf; import gtsam.* %% generate a set of cylinders and Samples fieldSize = Point2([100, 100]'); -cylinder_num = 10; -cylinders = cell(cylinder_num, 1); +cylinderNum = 10; +cylinders = cell(cylinderNum, 1); +% ToDo: it seems random generated cylinders doesn't work that well +% use fixed parameters instead for i = 1:cylinderNum baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); @@ -18,7 +21,6 @@ figID = 1; figure(figID); plotCylinderSamples(cylinders, fieldSize, figID); - %% generate camera trajectories K = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); @@ -28,7 +30,8 @@ trans = Point3(); % To ensure there are landmarks in view, look at one randomly chosen cylinder % each time. for i = 1:poseNum - camera = SimpleCamera.Lookat(trans, cylinders{round(cylinderNum*rand)}.centroid, ... + cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); + cameras{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... Point3([0,0,1]'), K); incT = Point3(5*rand, 5*rand, 5*rand); @@ -36,17 +39,34 @@ for i = 1:poseNum end %% visibility validation -visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders); +% for a simple test, it will be removed later +visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders); %% plot all the projected points %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); %% setp up monocular camera and get measurements -pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders); +%pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders); %% set up stereo camera and get measurements -%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders); +% load stereo calibration +calib = dlmread(findExampleDataFile('VO_calibration.txt')); +KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); +poseNum = 10; +camerasStereo = cell(poseNum, 1); +trans = Point3(); +for i = 1:poseNum + cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); + camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... + Point3([0,0,1]'), KStereo); + + incT = Point3(5*rand, 5*rand, 5*rand); + trans = trans.compose(incT); +end +pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders); + +% plot the 2D tracks % ToDo: plot the trajectories %plot3DTrajectory(); From 6ab95f60c2a40b6b1008dc912e1ade7de078d6be Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 01:32:59 -0500 Subject: [PATCH 43/84] use circle generator to replace the random data generator. This can fix the indeterminant system error. --- matlab/gtsam_examples/CameraFlyingExample.m | 69 ++++++++++++--------- 1 file changed, 38 insertions(+), 31 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 258bd968b..1eb301b38 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -3,15 +3,26 @@ clc; clf; import gtsam.* +%% define the options +options.fieldSize = Point2([100, 100]'); +options.cylinderNum = 10; +options.poseNum = 20; +options.monoK = Cal3_S2(525,525,0,320,240); +options.stereoK = Cal3_S2Stereo(721,721,0.0,609,172,0.53715); % read from the VO calibration file +options.imageSize = Point2([640, 480]'); + %% generate a set of cylinders and Samples -fieldSize = Point2([100, 100]'); -cylinderNum = 10; +cylinderNum = options.cylinderNum; cylinders = cell(cylinderNum, 1); -% ToDo: it seems random generated cylinders doesn't work that well -% use fixed parameters instead +% It seems random generated cylinders doesn't work that well +% Now it set up a circle of cylinders +theta = 0; for i = 1:cylinderNum - baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); + theta = theta + 2*pi / 10; + x = 20 * cos(theta) + options.fieldSize.x/2; + y = 20 * sin(theta) + options.fieldSize.y/2; + baseCentroid = Point2([x, y]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end @@ -19,23 +30,21 @@ end % now is plotting on a 100 * 100 field figID = 1; figure(figID); -plotCylinderSamples(cylinders, fieldSize, figID); +plotCylinderSamples(cylinders, options.fieldSize, figID); %% generate camera trajectories K = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); -poseNum = 10; -cameras = cell(poseNum, 1); -trans = Point3(); -% To ensure there are landmarks in view, look at one randomly chosen cylinder -% each time. -for i = 1:poseNum - cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); - cameras{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... - Point3([0,0,1]'), K); - - incT = Point3(5*rand, 5*rand, 5*rand); - trans = trans.compose(incT); +cameras = cell(options.poseNum, 1); +% Generate ground truth trajectory r.w.t. the field center +theta = 0; +r = 30; +for i = 1:options.poseNum + theta = (i-1)*2*pi/options.poseNum; + t = Point3([30*cos(theta), 30*sin(theta), 10]'); + cameras{i} = SimpleCamera.Lookat(t, ... + Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... + Point3([0,0,1]'), K); end %% visibility validation @@ -46,25 +55,23 @@ visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders); %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); %% setp up monocular camera and get measurements -%pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders); +pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders); %% set up stereo camera and get measurements % load stereo calibration calib = dlmread(findExampleDataFile('VO_calibration.txt')); KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); -poseNum = 10; -camerasStereo = cell(poseNum, 1); -trans = Point3(); -for i = 1:poseNum - cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); - camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... - Point3([0,0,1]'), KStereo); - - incT = Point3(5*rand, 5*rand, 5*rand); - trans = trans.compose(incT); -end +camerasStereo = cell(options.poseNum, 1); +% for i = 1:options.poseNum +% cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); +% camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... +% Point3([0,0,1]'), KStereo); +% +% incT = Point3(5*rand, 5*rand, 5*rand); +% trans = trans.compose(incT); +% end -pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders); +%pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders); % plot the 2D tracks From 1094739680ddc1961a90511b620eab46574e6797 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 02:01:28 -0500 Subject: [PATCH 44/84] small fix of empty return points values --- matlab/+gtsam/points2DTrackMonocular.m | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 9bdd746ae..33a6b34f2 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -79,12 +79,15 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception -ptIdx = 0; +ptx = 0; for i = 1:pointsNum + ptx = ptx + 1; if isempty(pts3d.pts{i}) continue; end - pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); + % cylinder index and measurements + pts2dTracksMono.Points{ptx} = pts3d.pts{i}; + pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',i)); end end From f4da1f874b2046a327127bba1a8da7856acdb404 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 11:34:24 -0500 Subject: [PATCH 45/84] get points track and visualize --- matlab/+gtsam/points2DTrackMonocular.m | 31 ++++++++++++++++++-------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 33a6b34f2..184408f26 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -77,17 +77,30 @@ graph.print(sprintf('\nFactor graph:\n')); marginals = Marginals(graph, initialEstimate); -%% get all the 2d points track information +%% get all the points track information % currently throws the Indeterminant linear system exception ptx = 0; -for i = 1:pointsNum - ptx = ptx + 1; - if isempty(pts3d.pts{i}) - continue; - end - % cylinder index and measurements - pts2dTracksMono.Points{ptx} = pts3d.pts{i}; - pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',i)); +for k = 1:cameraPosesNum + + for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + if isempty(pts3d.pts{k}.index{i}{j}) + continue; + end + ptx = ptx + 1; + + idx = pts3d.pts{k}.index{i}{j}; + pts2dTracksMono.pt3d{ptx} = pts3d.pts{k}.data{idx}; + pts2dTracksMono.Z{ptx} = pts3d.pts{k}.Z{idx}; + pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); + end + end + end +%% plot the result with covariance ellipses +hold on; +plot3DPoints(initialEstimate, [], marginals); +plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); + end From a2c8f69c3deccb84b68611ae8aed20a1f64a5239 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 13 Jan 2015 20:26:48 +0100 Subject: [PATCH 46/84] Compile with latest Boost version on Mac (compile issues with boost::lambda) --- .cproject | 106 ++++++++++++-------- gtsam/inference/LabeledSymbol.cpp | 22 ++-- gtsam/inference/Symbol.cpp | 19 +--- gtsam/inference/tests/testKey.cpp | 17 +++- gtsam/inference/tests/testLabeledSymbol.cpp | 26 +++-- 5 files changed, 104 insertions(+), 86 deletions(-) diff --git a/.cproject b/.cproject index ff704cea8..ce0ab3c4c 100644 --- a/.cproject +++ b/.cproject @@ -584,6 +584,7 @@ make + tests/testBayesTree.run true false @@ -591,6 +592,7 @@ make + testBinaryBayesNet.run true false @@ -638,6 +640,7 @@ make + testSymbolicBayesNet.run true false @@ -645,6 +648,7 @@ make + tests/testSymbolicFactor.run true false @@ -652,6 +656,7 @@ make + testSymbolicFactorGraph.run true false @@ -667,6 +672,7 @@ make + tests/testBayesTree true false @@ -1098,6 +1104,7 @@ make + testErrors.run true false @@ -1327,6 +1334,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1409,7 +1456,6 @@ make - testSimulated2DOriented.run true false @@ -1449,7 +1495,6 @@ make - testSimulated2D.run true false @@ -1457,7 +1502,6 @@ make - testSimulated3D.run true false @@ -1471,46 +1515,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1551,6 +1555,14 @@ false true + + make + -j4 + testLabeledSymbol.run + true + true + true + make -j2 @@ -1768,6 +1780,7 @@ cpack + -G DEB true false @@ -1775,6 +1788,7 @@ cpack + -G RPM true false @@ -1782,6 +1796,7 @@ cpack + -G TGZ true false @@ -1789,6 +1804,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2659,6 +2675,7 @@ make + testGraph.run true false @@ -2666,6 +2683,7 @@ make + testJunctionTree.run true false @@ -2673,6 +2691,7 @@ make + testSymbolicBayesNetB.run true false @@ -3272,7 +3291,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 7e573c13f..bd142a72c 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -17,12 +17,8 @@ #include -#include #include -#include -#include -#include -#include +#include #include @@ -111,23 +107,21 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const { } /* ************************************************************************* */ +static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);} + boost::function LabeledSymbol::TypeTest(unsigned char c) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c; + return bind(&LabeledSymbol::chr, bind(make, _1)) == c; } -/* ************************************************************************* */ boost::function LabeledSymbol::LabelTest(unsigned char label) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; + return bind(&LabeledSymbol::label, bind(make, _1)) == label; } -/* ************************************************************************* */ boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c && - bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; + return bind(&LabeledSymbol::chr, bind(make, _1)) == c && + bind(&LabeledSymbol::label, bind(make, _1)) == label; } +/* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index 37a6d0897..f8b37d429 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -18,19 +18,8 @@ #include -#include #include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif +#include #include #include @@ -71,10 +60,10 @@ Symbol::operator std::string() const { return str(boost::format("%c%d") % c_ % j_); } +static Symbol make(gtsam::Key key) { return Symbol(key);} + boost::function Symbol::ChrTest(unsigned char c) { - namespace bl = boost::lambda; - return bl::bind(&Symbol::chr, bl::bind(bl::constructor(), bl::_1)) - == c; + return bind(&Symbol::chr, bind(make, _1)) == c; } } // namespace gtsam diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 5b57096cb..1033c0cc9 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -14,14 +14,14 @@ * @author Alex Cunningham */ -#include // for operator += -using namespace boost::assign; - -#include +#include #include #include -#include +#include + +#include // for operator += +using namespace boost::assign; using namespace std; using namespace gtsam; @@ -65,6 +65,13 @@ TEST(Key, KeySymbolEncoding) { EXPECT(assert_equal(symbol, Symbol(key))); } +/* ************************************************************************* */ +TEST(Key, ChrTest) { + Key key = Symbol('c',3); + EXPECT(Symbol::ChrTest('c')(key)); + EXPECT(!Symbol::ChrTest('d')(key)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index 07727c8dc..18216453d 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -14,20 +14,19 @@ * @author Alex Cunningham */ -#include // for operator += -using namespace boost::assign; - -#include +#include #include #include -#include - +#include +#include // for operator += + +using namespace boost::assign; using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) { +TEST(LabeledSymbol, KeyLabeledSymbolConversion ) { LabeledSymbol expected('x', 'A', 4); Key key(expected); LabeledSymbol actual(key); @@ -36,7 +35,7 @@ TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) { } /* ************************************************************************* */ -TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { +TEST(LabeledSymbol, KeyLabeledSymbolEncoding ) { // Test encoding of LabeledSymbol <-> size_t <-> string // Encoding scheme: @@ -69,6 +68,17 @@ TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { } } +/* ************************************************************************* */ +TEST(LabeledSymbol, ChrTest) { + Key key = LabeledSymbol('c','A',3); + EXPECT(LabeledSymbol::TypeTest('c')(key)); + EXPECT(!LabeledSymbol::TypeTest('d')(key)); + EXPECT(LabeledSymbol::LabelTest('A')(key)); + EXPECT(!LabeledSymbol::LabelTest('D')(key)); + EXPECT(LabeledSymbol::TypeLabelTest('c','A')(key)); + EXPECT(!LabeledSymbol::TypeLabelTest('c','D')(key)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From d62cb440db758cd1dc2aee9adc45f91151489501 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 16:33:47 -0500 Subject: [PATCH 47/84] interface update --- matlab/+gtsam/cylinderSampleProjection.m | 4 +- matlab/+gtsam/plotCylinderSamples.m | 1 - matlab/+gtsam/points2DTrackMonocular.m | 50 +++++++++++---------- matlab/gtsam_examples/CameraFlyingExample.m | 34 ++++++-------- 4 files changed, 43 insertions(+), 46 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 4a8c3b06a..eb8369e42 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -1,4 +1,4 @@ -function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders) +function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders) % Input: % Output: @@ -13,6 +13,8 @@ function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders import gtsam.* +camera = SimpleCamera(pose, K); + %% memory allocation cylinderNum = length(cylinders); visiblePoints.index = cell(cylinderNum,1); diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m index 6c8a2249e..74768f641 100644 --- a/matlab/+gtsam/plotCylinderSamples.m +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -11,7 +11,6 @@ function plotCylinderSamples(cylinders, fieldSize, figID) sampleDensity = 120; - for i = 1:num [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 184408f26..0001261ef 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -1,4 +1,4 @@ -function pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders) +function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) % Assess how accurately we can reconstruct points from a particular monocular camera setup. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization @@ -17,7 +17,7 @@ posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); -cameraPosesNum = length(cameras); +cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; @@ -26,41 +26,40 @@ for i = 1:cylinderNum pointsNum = pointsNum + length(cylinders{i}.Points); end -pts3d = {}; +pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; for i = 1:cameraPosesNum % add a constraint on the starting pose - camera = cameras{i}; + cameraPose = cameraPoses{i}; - pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); - pts3d.camera{i} = camera; + pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise)); k = 0; - if ~isempty(pts3d.pts{i}.data{1+k}) + if ~isempty(pts3d{i}.data{1+k}) graph.add(PriorFactorPoint3(symbol('p', 1), ... - pts3d.pts{i}.data{1+k}, pointPriorNoise)); + pts3d{i}.data{1+k}, pointPriorNoise)); else k = k+1; end initialized = true; end - for j = 1:length(pts3d.pts{i}.Z) - if isempty(pts3d.pts{i}.Z{j}) + for j = 1:length(pts3d{i}.Z) + if isempty(pts3d{i}.Z{j}) continue; end - graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) ); + graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), K) ); end end %% initialize cameras and points close to ground truth for i = 1:cameraPosesNum - pose_i = camera.pose.retract(0.1*randn(6,1)); + pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); end ptsIdx = 0; @@ -75,24 +74,24 @@ end %% Print the graph graph.print(sprintf('\nFactor graph:\n')); -marginals = Marginals(graph, initialEstimate); +%marginals = Marginals(graph, initialEstimate); %% get all the points track information % currently throws the Indeterminant linear system exception -ptx = 0; +ptx = 1; for k = 1:cameraPosesNum for i = 1:length(cylinders) for j = 1:length(cylinders{i}.Points) - if isempty(pts3d.pts{k}.index{i}{j}) + if isempty(pts3d{k}.index{i}{j}) continue; end + idx = pts3d{k}.index{i}{j}; + pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx}; + pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx}; + %pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); + ptx = ptx + 1; - - idx = pts3d.pts{k}.index{i}{j}; - pts2dTracksMono.pt3d{ptx} = pts3d.pts{k}.data{idx}; - pts2dTracksMono.Z{ptx} = pts3d.pts{k}.Z{idx}; - pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); end end @@ -100,7 +99,10 @@ end %% plot the result with covariance ellipses hold on; -plot3DPoints(initialEstimate, [], marginals); -plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +%plot3DPoints(initialEstimate, [], marginals); +%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +plot3DTrajectory(initialEstimate, '*', 1, 8); +view(3); + end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 1eb301b38..f899eb3b3 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -20,8 +20,8 @@ cylinders = cell(cylinderNum, 1); theta = 0; for i = 1:cylinderNum theta = theta + 2*pi / 10; - x = 20 * cos(theta) + options.fieldSize.x/2; - y = 20 * sin(theta) + options.fieldSize.y/2; + x = 10 * cos(theta) + options.fieldSize.x/2; + y = 10 * sin(theta) + options.fieldSize.y/2; baseCentroid = Point2([x, y]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end @@ -32,46 +32,40 @@ figID = 1; figure(figID); plotCylinderSamples(cylinders, options.fieldSize, figID); -%% generate camera trajectories -K = Cal3_S2(525,525,0,320,240); +%% generate camera trajectories: a circle +KMono = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); -cameras = cell(options.poseNum, 1); +cameraPoses = cell(options.poseNum, 1); % Generate ground truth trajectory r.w.t. the field center theta = 0; -r = 30; +r = 40; for i = 1:options.poseNum theta = (i-1)*2*pi/options.poseNum; - t = Point3([30*cos(theta), 30*sin(theta), 10]'); - cameras{i} = SimpleCamera.Lookat(t, ... + t = Point3([r*cos(theta) + options.fieldSize.x/2, ... + r*sin(theta) + options.fieldSize.y/2, 10]'); + camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... - Point3([0,0,1]'), K); + Point3([0,0,1]'), KMono); + cameraPoses{i} = camera.pose; end %% visibility validation % for a simple test, it will be removed later -visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders); +%visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders); %% plot all the projected points %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); %% setp up monocular camera and get measurements -pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders); +pts2dTracksMono = points2DTrackMonocular(KMono, cameraPoses, imageSize, cylinders); %% set up stereo camera and get measurements % load stereo calibration calib = dlmread(findExampleDataFile('VO_calibration.txt')); KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); camerasStereo = cell(options.poseNum, 1); -% for i = 1:options.poseNum -% cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); -% camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... -% Point3([0,0,1]'), KStereo); -% -% incT = Point3(5*rand, 5*rand, 5*rand); -% trans = trans.compose(incT); -% end -%pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders); +%pts2dTracksStereo = points2DTrackStereo(KStereo, cameraPoses, imageSize, cylinders); % plot the 2D tracks From 8bd03e7e50da354cc4242f1a534e56361f1bb34c Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Tue, 13 Jan 2015 19:22:46 -0500 Subject: [PATCH 48/84] dllimport does not allow definition in header --- gtsam/base/Matrix.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 20a4a6bc4..c3cbfa341 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -238,7 +238,7 @@ Eigen::Block sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, * @param j is the column of the upper left corner insert location */ template -GTSAM_EXPORT void insertSub(Eigen::MatrixBase& fullMatrix, const Eigen::MatrixBase& subMatrix, size_t i, size_t j) { +void insertSub(Eigen::MatrixBase& fullMatrix, const Eigen::MatrixBase& subMatrix, size_t i, size_t j) { fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; } From 9c4942021a74ef58565f4f077038692a48979d77 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Tue, 13 Jan 2015 19:23:23 -0500 Subject: [PATCH 49/84] fixed some coding error --- gtsam/geometry/Pose3.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d930c815e..d30bd4167 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -323,7 +323,6 @@ typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> -struct traits : public internal::LieGroupTraits { -}; +struct traits : public internal::LieGroupTraits {}; } // namespace gtsam From 69deb225b8aac4198d0d552d01fdeebe59ef3871 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Tue, 13 Jan 2015 19:23:52 -0500 Subject: [PATCH 50/84] changed bind to boost::bind --- gtsam/inference/LabeledSymbol.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index bd142a72c..b9e93ceb1 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -110,16 +110,16 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const { static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);} boost::function LabeledSymbol::TypeTest(unsigned char c) { - return bind(&LabeledSymbol::chr, bind(make, _1)) == c; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c; } boost::function LabeledSymbol::LabelTest(unsigned char label) { - return bind(&LabeledSymbol::label, bind(make, _1)) == label; + return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - return bind(&LabeledSymbol::chr, bind(make, _1)) == c && - bind(&LabeledSymbol::label, bind(make, _1)) == label; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && + boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } /* ************************************************************************* */ From da06689677780e7a6fbe690192a795cac7a689d3 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 21:21:48 -0500 Subject: [PATCH 51/84] update the stereo model and occlusion detection --- .../+gtsam/cylinderSampleProjectionStereo.m | 84 +++++++++++++++++++ matlab/+gtsam/points2DTrackStereo.m | 46 +++++----- 2 files changed, 105 insertions(+), 25 deletions(-) create mode 100644 matlab/+gtsam/cylinderSampleProjectionStereo.m diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m new file mode 100644 index 000000000..917068da0 --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -0,0 +1,84 @@ +function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders) + +import gtsam.* + +%% memory allocation +cylinderNum = length(cylinders); +visiblePoints.index = cell(cylinderNum,1); + +pointCloudNum = 0; +for i = 1:cylinderNum + pointCloudNum = pointCloudNum + length(cylinders{i}.Points); + visiblePoints.index{i} = cell(pointCloudNum,1); +end +visiblePoints.data = cell(pointCloudNum,1); +visiblePoints.Z = cell(pointCloudNum, 1); + +%% check visiblity of points on each cylinder +pointCloudIndex = 0; +for i = 1:cylinderNum + + pointNum = length(cylinders{i}.Points); + + % to check point visibility + for j = 1:pointNum + + pointCloudIndex = pointCloudIndex + 1; + + % For Cheirality Exception + sampledPoint3 = cylinders{i}.Points{j}; + sampledPoint3local = pose.transform_to(sampledPoint3); + if sampledPoint3local.z < 0 + continue; + end + + % measurements + Z.du = K.fx() * K.baseline() / samplePoint3.z; + Z.uL = K.fx() * samplePoint3.x / samplePoint3.z + K.px(); + Z.uR = uL + du; + Z.v = K.fy() / samplePoint3.z + K.py(); + + % ignore points not visible in the scene + if Z.uL < 0 || Z.uL >= imageSize.x || ... + Z.uR < 0 || Z.uR >= imageSize.x || ... + Z.v < 0 || Z.v >= imageSize.y + continue; + end + + % ignore points occluded + % use a simple math hack to check occlusion: + % 1. All points in front of cylinders' surfaces are visible + % 2. For points behind the cylinders' surfaces, the cylinder + for k = 1:cylinderNum + + rayCameraToPoint = pose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = pose.translation().between(cylinders{i}.centroid).vector(); + rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); + + % Condition 1: all points in front of the cylinders' + % surfaces are visible + if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 + visiblePoints.data{pointCloudIndex} = sampledPoint3; + visiblePoints.Z{pointCloudIndex} = Z; + visiblePoints.index{i}{j} = pointCloudIndex; + continue; + end + + % Condition 2 + projectedRay = dot(rayCameraToCylinder, rayCameraToPoint); + if projectedRay > 0 + rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; + if rayCylinderToProjected(1) > cylinders{i}.radius && ... + rayCylinderToProjected(2) > cylinders{i}.radius + visiblePoints.data{pointCloudIndex} = sampledPoint3; + visiblePoints.Z{pointCloudIndex} = Z; + visiblePoints.index{i}{j} = pointCloudIndex; + end + end + + end + end + +end + +end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 41f6668b1..b3fea3d58 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -1,4 +1,4 @@ -function pts2dTracksStereo = points2DTrackStereo(cameras, imageSize, cylinders) +function pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders) % Assess how accurately we can reconstruct points from a particular monocular camera setup. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization @@ -16,8 +16,9 @@ measurementNoiseSigma = 1.0; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); +stereoNoise = noiseModel.Isotropic.Sigma(3,1); -cameraPosesNum = length(cameras); +cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; @@ -26,25 +27,14 @@ for i = 1:cylinderNum pointsNum = pointsNum + length(cylinders{i}.Points); end -pts3d = {}; +pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; -for i = 1:cameraPosesNum - % add a constraint on the starting pose - camera = cameras{i}; +for i = 1:cameraPosesNum + pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders); - pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); - pts3d.camera{i} = camera; - if ~initialized graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); - k = 0; - if ~isempty(pts3d.pts{i}.data{1+k}) - graph.add(PriorFactorPoint3(symbol('p', 1), ... - pts3d.pts{i}.data{1+k}, pointPriorNoise)); - else - k = k+1; - end initialized = true; end @@ -52,10 +42,9 @@ for i = 1:cameraPosesNum if isempty(pts3d.pts{i}.Z{j}) continue; end - graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) ); + graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... + stereoNoise, symbol('x', i), symbol('p', j), K)); end - end %% initialize cameras and points close to ground truth @@ -79,12 +68,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception -ptIdx = 0; -for i = 1:pointsNum - if isempty(pts3d.pts{i}) - continue; - end - pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); +ptx = 1; +for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + if isempty(pts3d{k}.index{i}{j}) + continue; + end + idx = pts3d{k}.index{i}{j}; + pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx}; + pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx}; + pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); + + ptx = ptx + 1; + end end end From 4a5d94ea59923361b5b106739098ffaf88a37479 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 23:25:44 -0500 Subject: [PATCH 52/84] test visibility. monocular camera visibility tests passed --- matlab/+gtsam/cylinderSampleProjection.m | 54 ++++++++------- matlab/+gtsam/points2DTrackMonocular.m | 16 ++--- matlab/gtsam_examples/CameraFlyingExample.m | 76 +++++++++++++++------ 3 files changed, 90 insertions(+), 56 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index eb8369e42..890f174b7 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -38,16 +38,16 @@ for i = 1:cylinderNum pointCloudIndex = pointCloudIndex + 1; + % Cheirality Exception sampledPoint3 = cylinders{i}.Points{j}; - sampledPoint3local = camera.pose.transform_to(sampledPoint3); - if sampledPoint3local.z < 0 - continue; % Cheirality Exception + sampledPoint3local = pose.transform_to(sampledPoint3); + if sampledPoint3local.z <= 0 + continue; end Z2d = camera.project(sampledPoint3); % ignore points not visible in the scene - if Z2d.x < 0 || Z2d.x >= imageSize.x ... - || Z2d.y < 0 || Z2d.y >= imageSize.y + if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y continue; end @@ -55,36 +55,40 @@ for i = 1:cylinderNum % use a simple math hack to check occlusion: % 1. All points in front of cylinders' surfaces are visible % 2. For points behind the cylinders' surfaces, the cylinder + visible = true; for k = 1:cylinderNum - rayCameraToPoint = camera.pose.translation().between(sampledPoint3).vector(); - rayCameraToCylinder = camera.pose.translation().between(cylinders{i}.centroid).vector(); - rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); + rayCameraToPoint = pose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector(); + rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector(); % Condition 1: all points in front of the cylinders' % surfaces are visible if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z2d; - visiblePoints.index{i}{j} = pointCloudIndex; - continue; - end - - % Condition 2 - projectedRay = dot(rayCameraToCylinder, rayCameraToPoint); - if projectedRay > 0 - rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; - if rayCylinderToProjected(1) > cylinders{i}.radius && ... - rayCylinderToProjected(2) > cylinders{i}.radius - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z2d; - visiblePoints.index{i}{j} = pointCloudIndex; + continue; + else + projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder); + if projectedRay > 0 + %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; + if rayCylinderToPoint(1) > cylinders{i}.radius && ... + rayCylinderToPoint(2) > cylinders{i}.radius + continue; + else + visible = false; + break; + end end end - + + end + + if visible + visiblePoints.data{pointCloudIndex} = sampledPoint3; + visiblePoints.Z{pointCloudIndex} = Z2d; + visiblePoints.index{i}{j} = pointCloudIndex; end end - + end end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 0001261ef..0f209bb43 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -10,11 +10,10 @@ import gtsam.* graph = NonlinearFactorGraph; %% create the noise factors -pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; -measurementNoiseSigma = 1.0; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); + +measurementNoiseSigma = 1.0; measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); cameraPosesNum = length(cameraPoses); @@ -37,13 +36,6 @@ for i = 1:cameraPosesNum if ~initialized graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise)); - k = 0; - if ~isempty(pts3d{i}.data{1+k}) - graph.add(PriorFactorPoint3(symbol('p', 1), ... - pts3d{i}.data{1+k}, pointPriorNoise)); - else - k = k+1; - end initialized = true; end @@ -74,7 +66,7 @@ end %% Print the graph graph.print(sprintf('\nFactor graph:\n')); -%marginals = Marginals(graph, initialEstimate); +marginals = Marginals(graph, initialEstimate); %% get all the points track information % currently throws the Indeterminant linear system exception @@ -89,7 +81,7 @@ for k = 1:cameraPosesNum idx = pts3d{k}.index{i}{j}; pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx}; pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx}; - %pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); + pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); ptx = ptx + 1; end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index f899eb3b3..4b379d7a4 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -1,15 +1,59 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief A camera flying example through a field of cylinder landmarks +% @author Zhaoyang Lv +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + clear all; clc; clf; import gtsam.* %% define the options +% the testing field size options.fieldSize = Point2([100, 100]'); +% the number of cylinders options.cylinderNum = 10; +% The number of camera poses options.poseNum = 20; +% Monocular Camera Calibration options.monoK = Cal3_S2(525,525,0,320,240); -options.stereoK = Cal3_S2Stereo(721,721,0.0,609,172,0.53715); % read from the VO calibration file +% Stereo Camera Calibration +options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); +% the image size of camera options.imageSize = Point2([640, 480]'); +% use Monocular camera or Stereo camera +options.Mono = false; + +%% test1: visibility +cylinders{1}.centroid = Point3(30, 50, 5); +cylinders{2}.centroid = Point3(50, 50, 5); +cylinders{3}.centroid = Point3(70, 50, 5); + +for i = 1:3 + cylinders{i}.radius = 5; + cylinders{i}.height = 10; + + cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0)); + cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); +end + +camera = SimpleCamera.Lookat(Point3(40, 50, 10), ... + Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... + Point3([0,0,1]'), options.monoK); + +pose = camera.pose; +pts3d = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders); + +figID = 1; +figure(figID); +plotCylinderSamples(cylinders, options.fieldSize, figID); %% generate a set of cylinders and Samples cylinderNum = options.cylinderNum; @@ -32,11 +76,9 @@ figID = 1; figure(figID); plotCylinderSamples(cylinders, options.fieldSize, figID); -%% generate camera trajectories: a circle +%% generate ground truth camera trajectories: a circle KMono = Cal3_S2(525,525,0,320,240); -imageSize = Point2([640, 480]'); cameraPoses = cell(options.poseNum, 1); -% Generate ground truth trajectory r.w.t. the field center theta = 0; r = 40; for i = 1:options.poseNum @@ -45,27 +87,23 @@ for i = 1:options.poseNum r*sin(theta) + options.fieldSize.y/2, 10]'); camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... - Point3([0,0,1]'), KMono); + Point3([0,0,1]'), options.monoK); cameraPoses{i} = camera.pose; end -%% visibility validation -% for a simple test, it will be removed later -%visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders); - %% plot all the projected points %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); -%% setp up monocular camera and get measurements -pts2dTracksMono = points2DTrackMonocular(KMono, cameraPoses, imageSize, cylinders); - -%% set up stereo camera and get measurements -% load stereo calibration -calib = dlmread(findExampleDataFile('VO_calibration.txt')); -KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); -camerasStereo = cell(options.poseNum, 1); - -%pts2dTracksStereo = points2DTrackStereo(KStereo, cameraPoses, imageSize, cylinders); +%% set up camera and get measurements +if options.Mono + % use Monocular Camera + pts2dTracksMono = points2DTrackMonocular(options.monoK, cameraPoses, ... + options.imageSize, cylinders); +else + % use Stereo Camera + pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ... + options.imageSize, cylinders); +end % plot the 2D tracks From ea556c71d70e9d66d3d7492ce5693cb69c3f9017 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 23:36:19 -0500 Subject: [PATCH 53/84] Stereo camera visibility tests passed --- .../+gtsam/cylinderSampleProjectionStereo.m | 49 ++++++++++--------- matlab/+gtsam/points2DTrackStereo.m | 36 +++++++------- matlab/gtsam_examples/CameraFlyingExample.m | 14 +++--- 3 files changed, 52 insertions(+), 47 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 917068da0..99cb83f87 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -33,10 +33,10 @@ for i = 1:cylinderNum end % measurements - Z.du = K.fx() * K.baseline() / samplePoint3.z; - Z.uL = K.fx() * samplePoint3.x / samplePoint3.z + K.px(); - Z.uR = uL + du; - Z.v = K.fy() / samplePoint3.z + K.py(); + Z.du = K.fx() * K.baseline() / sampledPoint3local.z; + Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px(); + Z.uR = Z.uL + Z.du; + Z.v = K.fy() / sampledPoint3local.z + K.py(); % ignore points not visible in the scene if Z.uL < 0 || Z.uL >= imageSize.x || ... @@ -49,34 +49,39 @@ for i = 1:cylinderNum % use a simple math hack to check occlusion: % 1. All points in front of cylinders' surfaces are visible % 2. For points behind the cylinders' surfaces, the cylinder + visible = true; for k = 1:cylinderNum rayCameraToPoint = pose.translation().between(sampledPoint3).vector(); - rayCameraToCylinder = pose.translation().between(cylinders{i}.centroid).vector(); - rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); + rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector(); + rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector(); % Condition 1: all points in front of the cylinders' % surfaces are visible if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z; - visiblePoints.index{i}{j} = pointCloudIndex; - continue; - end - - % Condition 2 - projectedRay = dot(rayCameraToCylinder, rayCameraToPoint); - if projectedRay > 0 - rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; - if rayCylinderToProjected(1) > cylinders{i}.radius && ... - rayCylinderToProjected(2) > cylinders{i}.radius - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z; - visiblePoints.index{i}{j} = pointCloudIndex; + continue; + else + projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder); + if projectedRay > 0 + %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; + if rayCylinderToPoint(1) > cylinders{i}.radius && ... + rayCylinderToPoint(2) > cylinders{i}.radius + continue; + else + visible = false; + break; + end end end - + end + + if visible + visiblePoints.data{pointCloudIndex} = sampledPoint3; + visiblePoints.Z{pointCloudIndex} = Z; + visiblePoints.index{i}{j} = pointCloudIndex; + end + end end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index b3fea3d58..14b3b5cd2 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -10,12 +10,8 @@ import gtsam.* graph = NonlinearFactorGraph; %% create the noise factors -pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; -measurementNoiseSigma = 1.0; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); -measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); stereoNoise = noiseModel.Isotropic.Sigma(3,1); cameraPosesNum = length(cameraPoses); @@ -31,15 +27,15 @@ pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; for i = 1:cameraPosesNum - pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders); + pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders); if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise)); initialized = true; end - for j = 1:length(pts3d.pts{i}.Z) - if isempty(pts3d.pts{i}.Z{j}) + for j = 1:length(pts3d{i}.Z) + if isempty(pts3d{i}.Z{j}) continue; end graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... @@ -49,7 +45,7 @@ end %% initialize cameras and points close to ground truth for i = 1:cameraPosesNum - pose_i = camera.pose.retract(0.1*randn(6,1)); + pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); end ptsIdx = 0; @@ -69,17 +65,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception ptx = 1; -for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - if isempty(pts3d{k}.index{i}{j}) - continue; - end - idx = pts3d{k}.index{i}{j}; - pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx}; - pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx}; - pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); +for k = 1:cameraPosesNum + for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + if isempty(pts3d{k}.index{i}{j}) + continue; + end + idx = pts3d{k}.index{i}{j}; + pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; + pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; + pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); - ptx = ptx + 1; + ptx = ptx + 1; + end end end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 4b379d7a4..3a66bb498 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -31,7 +31,7 @@ options.imageSize = Point2([640, 480]'); % use Monocular camera or Stereo camera options.Mono = false; -%% test1: visibility +%% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); cylinders{2}.centroid = Point3(50, 50, 5); cylinders{3}.centroid = Point3(70, 50, 5); @@ -44,16 +44,18 @@ for i = 1:3 cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); end -camera = SimpleCamera.Lookat(Point3(40, 50, 10), ... +camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.monoK); pose = camera.pose; -pts3d = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders); +prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders); + +%% test2: visibility test in stereo camera +prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); + + -figID = 1; -figure(figID); -plotCylinderSamples(cylinders, options.fieldSize, figID); %% generate a set of cylinders and Samples cylinderNum = options.cylinderNum; From 2378d59632e35d75bb8c47ac2e105fc85a5dbfb4 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 00:08:35 -0500 Subject: [PATCH 54/84] remove the redudant empty cells --- matlab/+gtsam/cylinderSampleProjection.m | 23 +++++-------- .../+gtsam/cylinderSampleProjectionStereo.m | 22 +++++-------- matlab/+gtsam/points2DTrackMonocular.m | 33 ++++++------------- matlab/+gtsam/points2DTrackStereo.m | 25 ++++---------- matlab/gtsam_examples/CameraFlyingExample.m | 11 +++---- 5 files changed, 37 insertions(+), 77 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 890f174b7..0abd9cc3c 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -17,18 +17,10 @@ camera = SimpleCamera(pose, K); %% memory allocation cylinderNum = length(cylinders); -visiblePoints.index = cell(cylinderNum,1); - -pointCloudNum = 0; -for i = 1:cylinderNum - pointCloudNum = pointCloudNum + length(cylinders{i}.Points); - visiblePoints.index{i} = cell(pointCloudNum,1); -end -visiblePoints.data = cell(pointCloudNum,1); -visiblePoints.Z = cell(pointCloudNum, 1); %% check visiblity of points on each cylinder pointCloudIndex = 0; +visiblePointIdx = 1; for i = 1:cylinderNum pointNum = length(cylinders{i}.Points); @@ -70,8 +62,8 @@ for i = 1:cylinderNum projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder); if projectedRay > 0 %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; - if rayCylinderToPoint(1) > cylinders{i}.radius && ... - rayCylinderToPoint(2) > cylinders{i}.radius + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius continue; else visible = false; @@ -83,10 +75,13 @@ for i = 1:cylinderNum end if visible - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z2d; - visiblePoints.index{i}{j} = pointCloudIndex; + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z2d; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; end + end end diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 99cb83f87..ae02c879c 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -4,18 +4,10 @@ import gtsam.* %% memory allocation cylinderNum = length(cylinders); -visiblePoints.index = cell(cylinderNum,1); - -pointCloudNum = 0; -for i = 1:cylinderNum - pointCloudNum = pointCloudNum + length(cylinders{i}.Points); - visiblePoints.index{i} = cell(pointCloudNum,1); -end -visiblePoints.data = cell(pointCloudNum,1); -visiblePoints.Z = cell(pointCloudNum, 1); %% check visiblity of points on each cylinder pointCloudIndex = 0; +visiblePointIdx = 1; for i = 1:cylinderNum pointNum = length(cylinders{i}.Points); @@ -64,8 +56,8 @@ for i = 1:cylinderNum projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder); if projectedRay > 0 %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; - if rayCylinderToPoint(1) > cylinders{i}.radius && ... - rayCylinderToPoint(2) > cylinders{i}.radius + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius continue; else visible = false; @@ -77,9 +69,11 @@ for i = 1:cylinderNum end if visible - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z; - visiblePoints.index{i}{j} = pointCloudIndex; + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; end end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 0f209bb43..7cdd99fd3 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -28,10 +28,9 @@ end pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; -for i = 1:cameraPosesNum - % add a constraint on the starting pose - cameraPose = cameraPoses{i}; +for i = 1:cameraPosesNum + cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); if ~initialized @@ -39,12 +38,10 @@ for i = 1:cameraPosesNum initialized = true; end - for j = 1:length(pts3d{i}.Z) - if isempty(pts3d{i}.Z{j}) - continue; - end + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', j), K) ); + measurementNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K) ); end end @@ -70,23 +67,13 @@ marginals = Marginals(graph, initialEstimate); %% get all the points track information % currently throws the Indeterminant linear system exception -ptx = 1; for k = 1:cameraPosesNum - - for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - if isempty(pts3d{k}.index{i}{j}) - continue; - end - idx = pts3d{k}.index{i}{j}; - pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx}; - pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx}; - pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); - - ptx = ptx + 1; - end + num = length(pts3d{k}.data); + for i = 1:num + pts2dTracksMono.pt3d{i} = pts3d{k}.data{i}; + pts2dTracksMono.Z{i} = pts3d{k}.Z{i}; + pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); end - end %% plot the result with covariance ellipses diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 14b3b5cd2..0a27cf90b 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -34,12 +34,10 @@ for i = 1:cameraPosesNum initialized = true; end - for j = 1:length(pts3d{i}.Z) - if isempty(pts3d{i}.Z{j}) - continue; - end + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... - stereoNoise, symbol('x', i), symbol('p', j), K)); + stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); end end @@ -64,21 +62,10 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception -ptx = 1; for k = 1:cameraPosesNum - for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - if isempty(pts3d{k}.index{i}{j}) - continue; - end - idx = pts3d{k}.index{i}{j}; - pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; - pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; - pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); - - ptx = ptx + 1; - end - end + pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; + pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; + pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); end end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 3a66bb498..1e5981614 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -29,7 +29,7 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); % the image size of camera options.imageSize = Point2([640, 480]'); % use Monocular camera or Stereo camera -options.Mono = false; +options.Mono = true; %% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); @@ -54,9 +54,6 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, %% test2: visibility test in stereo camera prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); - - - %% generate a set of cylinders and Samples cylinderNum = options.cylinderNum; cylinders = cell(cylinderNum, 1); @@ -93,9 +90,6 @@ for i = 1:options.poseNum cameraPoses{i} = camera.pose; end -%% plot all the projected points -%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); - %% set up camera and get measurements if options.Mono % use Monocular Camera @@ -107,6 +101,9 @@ else options.imageSize, cylinders); end +%% plot all the projected points +%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); + % plot the 2D tracks % ToDo: plot the trajectories From 86f580b9ae3cd6c560e077a62ef1ed737883fb92 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 00:19:17 -0500 Subject: [PATCH 55/84] stereo view works fine. but monocular camera still sufferes from the indeterminant system problem --- matlab/+gtsam/points2DTrackStereo.m | 16 +++++++++++++--- matlab/gtsam_examples/CameraFlyingExample.m | 7 +++++-- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 0a27cf90b..a65276e38 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -63,9 +63,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception for k = 1:cameraPosesNum - pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; - pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; - pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); + num = length(pts3d{k}.data); + for i = 1:num + pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i}; + pts2dTracksStereo.Z{i} = pts3d{k}.Z{i}; + pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i})); + end end +%% plot the result with covariance ellipses +hold on; +%plot3DPoints(initialEstimate, [], marginals); +%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +view(3); + end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 1e5981614..c86c8cb83 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -20,6 +20,8 @@ import gtsam.* options.fieldSize = Point2([100, 100]'); % the number of cylinders options.cylinderNum = 10; +% point density on cylinder +options.density = 1; % The number of camera poses options.poseNum = 20; % Monocular Camera Calibration @@ -29,7 +31,8 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); % the image size of camera options.imageSize = Point2([640, 480]'); % use Monocular camera or Stereo camera -options.Mono = true; +options.Mono = false; + %% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); @@ -66,7 +69,7 @@ for i = 1:cylinderNum x = 10 * cos(theta) + options.fieldSize.x/2; y = 10 * sin(theta) + options.fieldSize.y/2; baseCentroid = Point2([x, y]'); - cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); + cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density); end %% plot all the cylinders and sampled points From e575f27e4a64dc489a263245df19e0714647b003 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Wed, 14 Jan 2015 14:43:42 +0100 Subject: [PATCH 56/84] removed dead code from old default charts in Values --- gtsam/nonlinear/Values-inl.h | 4 ++-- gtsam/nonlinear/Values.h | 18 ------------------ 2 files changed, 2 insertions(+), 20 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 2128b6bb7..fe2c3f3ca 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -289,13 +289,13 @@ namespace gtsam { } /* ************************************************************************* */ - // insert a plain value using the default chart + // insert a templated value template void Values::insert(Key j, const ValueType& val) { insert(j, static_cast(GenericValue(val))); } - // update with default chart + // update with templated value template void Values::update(Key j, const ValueType& val) { update(j, static_cast(GenericValue(val))); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 539302779..73d0711be 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -260,7 +260,6 @@ namespace gtsam { /** Templated version to add a variable with the given j, * throws KeyAlreadyExists if j is already present - * if no chart is specified, the DefaultChart is used */ template void insert(Key j, const ValueType& val); @@ -272,15 +271,6 @@ namespace gtsam { /// version for double void insertDouble(Key j, double c) { insert(j,c); } - /// overloaded insert version that also specifies a chart - template - void insert(Key j, const ValueType& val); - - /// overloaded insert version that also specifies a chart initializer - template - void insert(Key j, const ValueType& val, Chart chart); - - /** insert that mimics the STL map insert - if the value already exists, the map is not modified * and an iterator to the existing value is returned, along with 'false'. If the value did not * exist, it is inserted and an iterator pointing to the new element, along with 'true', is @@ -297,14 +287,6 @@ namespace gtsam { template void update(Key j, const T& val); - /// overloaded insert version that also specifies a chart - template - void update(Key j, const T& val); - - /// overloaded insert version that also specifies a chart initializer - template - void update(Key j, const T& val, Chart chart); - /** update the current available values without adding new ones */ void update(const Values& values); From 3cb1f96371c147737af0f8b332cb32f5648ce11d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 23:36:15 -0500 Subject: [PATCH 57/84] to make a straight line trajectory --- matlab/gtsam_examples/CameraFlyingExample.m | 64 +++++++++++++++++---- 1 file changed, 53 insertions(+), 11 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index c86c8cb83..727374288 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -19,11 +19,17 @@ import gtsam.* % the testing field size options.fieldSize = Point2([100, 100]'); % the number of cylinders -options.cylinderNum = 10; +options.cylinderNum = 20; % point density on cylinder options.density = 1; + % The number of camera poses options.poseNum = 20; +% covariance scaling factor +options.scale = 1; + + +%% Camera Setup % Monocular Camera Calibration options.monoK = Cal3_S2(525,525,0,320,240); % Stereo Camera Calibration @@ -31,7 +37,13 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); % the image size of camera options.imageSize = Point2([640, 480]'); % use Monocular camera or Stereo camera -options.Mono = false; +options.Mono = true; +% fps for image +options.fps = 20; +% camera flying speed +options.speed = 20; + + %% test1: visibility test in monocular camera @@ -57,7 +69,7 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, %% test2: visibility test in stereo camera prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); -%% generate a set of cylinders and Samples +%% generate a set of cylinders and samples cylinderNum = options.cylinderNum; cylinders = cell(cylinderNum, 1); @@ -65,34 +77,49 @@ cylinders = cell(cylinderNum, 1); % Now it set up a circle of cylinders theta = 0; for i = 1:cylinderNum - theta = theta + 2*pi / 10; - x = 10 * cos(theta) + options.fieldSize.x/2; - y = 10 * sin(theta) + options.fieldSize.y/2; + theta = theta + 2*pi/10; + x = 30 * rand * cos(theta) + options.fieldSize.x/2; + y = 20 * rand * sin(theta) + options.fieldSize.y/2; baseCentroid = Point2([x, y]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density); end + %% plot all the cylinders and sampled points % now is plotting on a 100 * 100 field figID = 1; figure(figID); plotCylinderSamples(cylinders, options.fieldSize, figID); -%% generate ground truth camera trajectories: a circle +% %% generate ground truth camera trajectories: a circle +% KMono = Cal3_S2(525,525,0,320,240); +% cameraPoses = cell(options.poseNum, 1); +% theta = 0; +% r = 40; +% for i = 1:options.poseNum +% theta = (i-1)*2*pi/options.poseNum; +% t = Point3([r*cos(theta) + options.fieldSize.x/2, ... +% r*sin(theta) + options.fieldSize.y/2, 10]'); +% camera = SimpleCamera.Lookat(t, ... +% Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... +% Point3([0,0,1]'), options.monoK); +% cameraPoses{i} = camera.pose; +% end + +%% generate ground truth camera trajectories: a line KMono = Cal3_S2(525,525,0,320,240); cameraPoses = cell(options.poseNum, 1); theta = 0; -r = 40; for i = 1:options.poseNum - theta = (i-1)*2*pi/options.poseNum; - t = Point3([r*cos(theta) + options.fieldSize.x/2, ... - r*sin(theta) + options.fieldSize.y/2, 10]'); + t = Point3([(i-1)*(options.fieldSize.x - 20)/options.poseNum + 20, ... + 15, 10]'); camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.monoK); cameraPoses{i} = camera.pose; end + %% set up camera and get measurements if options.Mono % use Monocular Camera @@ -102,9 +129,24 @@ else % use Stereo Camera pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ... options.imageSize, cylinders); + + figID = 2; + figure(figID) + + axis equal; + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + + ptsSize = length(pts2dTracksStereo.pt3d{i}); + for i = 1:ptsSize + plotPoint3(pts2dTracksStereo.pt3d{i}, 'red', pts2dTracksStereo.cov{i}); + hold on + end + + hold off end %% plot all the projected points + %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); % plot the 2D tracks From 7eec7f7b455ed5b61d471adb314ff2f7e08f33f9 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 23:37:10 -0500 Subject: [PATCH 58/84] to check single measurement constraint. --- matlab/+gtsam/points2DTrackMonocular.m | 62 +++++++++++++++++++------- 1 file changed, 45 insertions(+), 17 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 7cdd99fd3..83bf45fbd 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -21,43 +21,58 @@ cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; cylinderNum = length(cylinders); +points3d = cell(0); for i = 1:cylinderNum - pointsNum = pointsNum + length(cylinders{i}.Points); + cylinderPointsNum = length(cylinders{i}.Points); + pointsNum = pointsNum + cylinderPointsNum; + for j = 1:cylinderPointsNum + points3d{end+1}.data = cylinders{i}.Points{j}; + points3d{end}.Z = cell(0); + points3d{end}.cameraConstraint = cell(0); + points3d{end}.visiblity = false; + end end +graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); + pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; -initialized = false; for i = 1:cameraPosesNum cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); - - if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise)); - initialized = true; - end measurementNum = length(pts3d{i}.Z); for j = 1:measurementNum - graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K) ); + index = pts3d{i}.overallIdx{j}; + points3d{index}.Z{end+1} = pts3d{i}.Z{j}; + points3d{index}.cameraConstraint{end+1} = i; + points3d{index}.visiblity = true; end - + end -%% initialize cameras and points close to ground truth +%% initialize graph and values for i = 1:cameraPosesNum pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); end -ptsIdx = 0; -for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - ptsIdx = ptsIdx + 1; - point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', ptsIdx), point_j); + +for i = 1:pointsNum + % single measurement. not added to graph + factorNum = length(points3d{i}.Z); + if factorNum > 1 + for j = 1:factorNum + cameraIdx = points3d{i}.cameraConstraint{j}; + graph.add(GenericProjectionFactorCal3_S2(points3d{i}.Z{j}, ... + measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) ); + end end + + % add in values + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); + end %% Print the graph @@ -67,6 +82,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the points track information % currently throws the Indeterminant linear system exception +for i = 1:pointsNum + if points3d{i}.visiblity + pts2dTracksMono.pt3d{i} = points3d{i}.data; + pts2dTracksMono.Z = points3d{i}.Z; + + if length(points3d{i}.Z) == 1 + %pts2dTracksMono.cov{i} singular matrix + else + pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p', i)); + end + end +end + for k = 1:cameraPosesNum num = length(pts3d{k}.data); for i = 1:num From 27b3b5ebedbc5c73f3efd9e70e3707763db0ccab Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 15 Jan 2015 00:20:06 -0500 Subject: [PATCH 59/84] plot the points covariance --- .../+gtsam/cylinderSampleProjectionStereo.m | 5 +++ matlab/+gtsam/plotProjectedCylinderSamples.m | 45 ++++++++++--------- matlab/+gtsam/points2DTrackStereo.m | 8 +++- matlab/gtsam_examples/CameraFlyingExample.m | 17 +------ 4 files changed, 39 insertions(+), 36 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index ae02c879c..51cda12ac 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -5,6 +5,11 @@ import gtsam.* %% memory allocation cylinderNum = length(cylinders); +visiblePoints.data = cell(1); +visiblePoints.Z = cell(1); +visiblePoints.cylinderIdx = cell(1); +visiblePoints.overallIdx = cell(1); + %% check visiblity of points on each cylinder pointCloudIndex = 0; visiblePointIdx = 1; diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m index 5d9a06713..a58526632 100644 --- a/matlab/+gtsam/plotProjectedCylinderSamples.m +++ b/matlab/+gtsam/plotProjectedCylinderSamples.m @@ -1,4 +1,4 @@ -function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID) +function plotProjectedCylinderSamples(pts3d, covariance, figID) % plot the visible projected points on the cylinders % author: Zhaoyang Lv @@ -9,27 +9,32 @@ function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID) holdstate = ishold; hold on - %plotCamera(cameraPose, 5); - - pointsNum = size(visiblePoints3, 1) - - for i=1:pointsNum - ray = visiblePoints3{i}.between(cameraPose.translation()).vector(); - dist = norm(ray); - - p = plot3(visiblePoints3{i}.x, visiblePoints3{i}.y, visiblePoints3{i}.z, ... - 'o', 'MarkerFaceColor', 'Green'); - - for t=0:0.1:dist - marchingRay = ray * t; - p.XData = visiblePoints3{i}.x + marchingRay(1); - p.YData = visiblePoints3{i}.y + marchingRay(2); - p.ZData = visiblePoints3{i}.z + marchingRay(3); - drawnow update - end - + pointsNum = length(pts3d); + for i = 1:pointsNum + plotPoint3(pts3d{i}, 'green', covariance{i}); + hold on end +% for i=1:pointsNum +% ray = pts2dTracksStereo{i}.between(cameraPose.translation()).vector(); +% dist = norm(ray); +% +% p = plot3(pts2dTracksStereo{i}.x, pts2dTracksStereo{i}.y, pts2dTracksStereo{i}.z, ... +% 'o', 'MarkerFaceColor', 'Green'); +% +% for t=0:0.1:dist +% marchingRay = ray * t; +% p.XData = pts2dTracksStereo{i}.x + marchingRay(1); +% p.YData = pts2dTracksStereo{i}.y + marchingRay(2); +% p.ZData = pts2dTracksStereo{i}.z + marchingRay(3); +% drawnow update +% end +% +% end + + axis equal; + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + if ~holdstate hold off end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index a65276e38..141596cea 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -12,7 +12,7 @@ graph = NonlinearFactorGraph; %% create the noise factors poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -stereoNoise = noiseModel.Isotropic.Sigma(3,1); +stereoNoise = noiseModel.Isotropic.Sigma(3,0.2); cameraPosesNum = length(cameraPoses); @@ -35,6 +35,9 @@ for i = 1:cameraPosesNum end measurementNum = length(pts3d{i}.Z); + if measurementNum < 1 + continue; + end for j = 1:measurementNum graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); @@ -64,6 +67,9 @@ marginals = Marginals(graph, initialEstimate); % currently throws the Indeterminant linear system exception for k = 1:cameraPosesNum num = length(pts3d{k}.data); + if num < 1 + continue; + end for i = 1:num pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i}; pts2dTracksStereo.Z{i} = pts3d{k}.Z{i}; diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 727374288..eb047b50a 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -37,15 +37,13 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); % the image size of camera options.imageSize = Point2([640, 480]'); % use Monocular camera or Stereo camera -options.Mono = true; +options.Mono = false; % fps for image options.fps = 20; % camera flying speed options.speed = 20; - - %% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); cylinders{2}.centroid = Point3(50, 50, 5); @@ -131,23 +129,12 @@ else options.imageSize, cylinders); figID = 2; - figure(figID) + plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, figID); - axis equal; - axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - - ptsSize = length(pts2dTracksStereo.pt3d{i}); - for i = 1:ptsSize - plotPoint3(pts2dTracksStereo.pt3d{i}, 'red', pts2dTracksStereo.cov{i}); - hold on - end - - hold off end %% plot all the projected points -%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); % plot the 2D tracks From 26df490c55b1750496728c1a4ebb39640d9fea4d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 15 Jan 2015 01:19:21 -0500 Subject: [PATCH 60/84] remove dulipcate points in stereo camera set up --- matlab/+gtsam/plotCylinderSamples.m | 4 +- matlab/+gtsam/plotProjectedCylinderSamples.m | 7 +- matlab/+gtsam/points2DTrackMonocular.m | 50 ++++++------ matlab/+gtsam/points2DTrackStereo.m | 82 ++++++++++++++------ matlab/gtsam_examples/CameraFlyingExample.m | 4 +- 5 files changed, 95 insertions(+), 52 deletions(-) diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m index 74768f641..ec1795dea 100644 --- a/matlab/+gtsam/plotCylinderSamples.m +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -1,4 +1,4 @@ -function plotCylinderSamples(cylinders, fieldSize, figID) +function plotCylinderSamples(cylinders, options, figID) % plot the cylinders on the given field % @author: Zhaoyang Lv @@ -24,7 +24,7 @@ function plotCylinderSamples(cylinders, fieldSize, figID) end axis equal - axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]); + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); grid on diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m index a58526632..f59434d96 100644 --- a/matlab/+gtsam/plotProjectedCylinderSamples.m +++ b/matlab/+gtsam/plotProjectedCylinderSamples.m @@ -1,5 +1,5 @@ -function plotProjectedCylinderSamples(pts3d, covariance, figID) -% plot the visible projected points on the cylinders +function plotProjectedCylinderSamples(pts3d, covariance, options, figID) +% plot the visible points on the cylinders % author: Zhaoyang Lv import gtsam.* @@ -11,7 +11,8 @@ function plotProjectedCylinderSamples(pts3d, covariance, figID) pointsNum = length(pts3d); for i = 1:pointsNum - plotPoint3(pts3d{i}, 'green', covariance{i}); + %gtsam.plotPoint3(p, 'g', covariance{i}); + plotPoint3(pts3d{i}, 'r', covariance{i}); hold on end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 83bf45fbd..a4e1a2e5c 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -59,6 +59,13 @@ for i = 1:cameraPosesNum end for i = 1:pointsNum + % add in values + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); + + if ~points3d{i}.visiblity + continue; + end % single measurement. not added to graph factorNum = length(points3d{i}.Z); if factorNum > 1 @@ -68,41 +75,40 @@ for i = 1:pointsNum measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) ); end end - - % add in values - point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); - + end %% Print the graph graph.print(sprintf('\nFactor graph:\n')); +%% linearize the graph +% currently throws the Indeterminant linear system exception marginals = Marginals(graph, initialEstimate); %% get all the points track information -% currently throws the Indeterminant linear system exception for i = 1:pointsNum - if points3d{i}.visiblity - pts2dTracksMono.pt3d{i} = points3d{i}.data; - pts2dTracksMono.Z = points3d{i}.Z; + if ~points3d{i}.visiblity + continue; + end + + pts2dTracksMono.pt3d{end+1} = points3d{i}.data; + pts2dTracksMono.Z{end+1} = points3d{i}.Z; - if length(points3d{i}.Z) == 1 - %pts2dTracksMono.cov{i} singular matrix - else - pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p', i)); - end + if length(points3d{i}.Z) == 1 + %pts2dTracksMono.cov{i} singular matrix + else + pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); end end -for k = 1:cameraPosesNum - num = length(pts3d{k}.data); - for i = 1:num - pts2dTracksMono.pt3d{i} = pts3d{k}.data{i}; - pts2dTracksMono.Z{i} = pts3d{k}.Z{i}; - pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); - end -end +% for k = 1:cameraPosesNum +% num = length(pts3d{k}.data); +% for i = 1:num +% pts2dTracksMono.pt3d{i} = pts3d{k}.data{i}; +% pts2dTracksMono.Z{i} = pts3d{k}.Z{i}; +% pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); +% end +% end %% plot the result with covariance ellipses hold on; diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 141596cea..2359654d3 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -19,64 +19,100 @@ cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; cylinderNum = length(cylinders); +points3d = cell(0); for i = 1:cylinderNum + cylinderPointsNum = length(cylinders{i}.Points); pointsNum = pointsNum + length(cylinders{i}.Points); + for j = 1:cylinderPointsNum + points3d{end+1}.data = cylinders{i}.Points{j}; + points3d{end}.Z = cell(0); + points3d{end}.cameraConstraint = cell(0); + points3d{end}.visiblity = false; + end end +graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); + pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; -initialized = false; for i = 1:cameraPosesNum pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders); - if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise)); - initialized = true; + if isempty(pts3d{i}.Z) + continue; end measurementNum = length(pts3d{i}.Z); - if measurementNum < 1 - continue; - end for j = 1:measurementNum + index = pts3d{i}.overallIdx{j}; + points3d{index}.Z{end+1} = pts3d{i}.Z{j}; + points3d{index}.cameraConstraint{end+1} = i; + points3d{index}.visiblity = true; + end + + for j = 1:length(pts3d{i}.Z) graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); end end -%% initialize cameras and points close to ground truth +%% initialize graph and values for i = 1:cameraPosesNum pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); end -ptsIdx = 0; -for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - ptsIdx = ptsIdx + 1; - point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', ptsIdx), point_j); + +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); + + if ~points3d{i}.visiblity + continue; end + + factorNum = length(points3d{i}.Z); + for j = 1:factorNum + cameraIdx = points3d{i}.cameraConstraint{j}; + graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ... + stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K)); + end end + %% Print the graph graph.print(sprintf('\nFactor graph:\n')); +%% linearize the graph marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information -% currently throws the Indeterminant linear system exception -for k = 1:cameraPosesNum - num = length(pts3d{k}.data); - if num < 1 +pts2dTracksStereo.pt3d = cell(0); +pts2dTracksStereo.Z = cell(0); +pts2dTracksStereo.cov = cell(0); +for i = 1:pointsNum + if ~points3d{i}.visiblity continue; end - for i = 1:num - pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i}; - pts2dTracksStereo.Z{i} = pts3d{k}.Z{i}; - pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i})); - end + + pts2dTracksStereo.pt3d{end+1} = points3d{i}.data; + pts2dTracksStereo.Z{end+1} = points3d{i}.Z; + pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); + end + +% for k = 1:cameraPosesNum +% if isempty(pts3d{k}.data) +% continue; +% end +% +% for i = 1:length(pts3d{k}.data) +% pts2dTracksStereo.pt3d{end+1} = pts3d{k}.data{i}; +% pts2dTracksStereo.Z{end+1} = pts3d{k}.Z{i}; +% pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i})); +% end +% end + %% plot the result with covariance ellipses hold on; %plot3DPoints(initialEstimate, [], marginals); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index eb047b50a..ba9434080 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -87,7 +87,7 @@ end % now is plotting on a 100 * 100 field figID = 1; figure(figID); -plotCylinderSamples(cylinders, options.fieldSize, figID); +plotCylinderSamples(cylinders, options, figID); % %% generate ground truth camera trajectories: a circle % KMono = Cal3_S2(525,525,0,320,240); @@ -129,7 +129,7 @@ else options.imageSize, cylinders); figID = 2; - plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, figID); + plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID); end From d0579dff50221c9fa79eed0456137ad31af3f253 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 15 Jan 2015 16:17:53 +0100 Subject: [PATCH 61/84] Wrapping Cal3Unified as a derived class of Cal3DS2_Base --- gtsam.h | 79 +++++++++++++++++++++++++---------- gtsam/geometry/Cal3DS2.h | 14 ++++++- gtsam/geometry/Cal3DS2_Base.h | 28 ++++++++++--- gtsam/geometry/Cal3Unified.h | 10 +++-- 4 files changed, 100 insertions(+), 31 deletions(-) diff --git a/gtsam.h b/gtsam.h index 1fbc0f907..c5528309e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -629,28 +629,13 @@ class Cal3_S2 { void serialize() const; }; -#include -class Cal3DS2 { +#include +virtual class Cal3DS2_Base { // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); - Cal3DS2(Vector v); + Cal3DS2_Base(); // Testable void print(string s) const; - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // TODO: D2d functions that start with an uppercase letter // Standard Interface double fx() const; @@ -658,14 +643,66 @@ class Cal3DS2 { double skew() const; double px() const; double py() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase + double k1() const; + double k2() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; // enabling serialization functionality void serialize() const; }; +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include class Cal3_S2Stereo { // Standard Constructors Cal3_S2Stereo(); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0c77eebbc..2fb0c8653 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -68,7 +68,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; @@ -89,10 +89,20 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 9; } //TODO: make a final dimension variable + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3DS2(*this)); + } + + /// @} + private: - /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 37c156743..d4f4cabe5 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -45,9 +45,6 @@ protected: double p1_, p2_ ; // tangential distortion public: - Matrix3 K() const ; - Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } - Vector9 vector() const ; /// @name Standard Constructors /// @{ @@ -59,6 +56,8 @@ public: double k1, double k2, double p1 = 0.0, double p2 = 0.0) : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + virtual ~Cal3DS2_Base() {} + /// @} /// @name Advanced Constructors /// @{ @@ -70,7 +69,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; @@ -106,6 +105,15 @@ public: /// Second tangential distortion coefficient inline double p2() const { return p2_;} + /// return calibration matrix -- not really applicable + Matrix3 K() const; + + /// return distortion parameter vector + Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } + + /// Return all parameters as a vector + Vector9 vector() const; + /** * convert intrinsic coordinates xy to (distorted) image coordinates uv * @param p point in intrinsic coordinates @@ -126,9 +134,19 @@ public: /// Derivative of uncalibrate wrpt the calibration parameters Matrix29 D2d_calibration(const Point2& p) const ; -private: + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3DS2_Base(*this)); + } /// @} + +private: + /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d0e0f3891..8e4394c0d 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,9 +50,8 @@ private: double xi_; // mirror parameter public: - enum { dimension = 10 }; - Vector10 vector() const ; + enum { dimension = 10 }; /// @name Standard Constructors /// @{ @@ -77,7 +76,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3Unified& K, double tol = 10e-9) const; @@ -125,6 +124,11 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 10; } //TODO: make a final dimension variable + /// Return all parameters as a vector + Vector10 vector() const ; + + /// @} + private: /** Serialization function */ From d42391d28d7b67787017c395a765fe800286fe28 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 15 Jan 2015 16:20:22 +0100 Subject: [PATCH 62/84] Added test of Cal3Unified and cleaned up a bit --- matlab/gtsam_tests/testCal3Unified.m | 7 +++++++ matlab/gtsam_tests/test_gtsam.m | 21 +++++++++++++++------ 2 files changed, 22 insertions(+), 6 deletions(-) create mode 100644 matlab/gtsam_tests/testCal3Unified.m diff --git a/matlab/gtsam_tests/testCal3Unified.m b/matlab/gtsam_tests/testCal3Unified.m new file mode 100644 index 000000000..498c65343 --- /dev/null +++ b/matlab/gtsam_tests/testCal3Unified.m @@ -0,0 +1,7 @@ +% test Cal3Unified +import gtsam.*; + +K = Cal3Unified; +EXPECT('fx',K.fx()==1); +EXPECT('fy',K.fy()==1); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 1a6856a9a..4cbe42364 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,17 +1,25 @@ % Test runner script - runs each test -% display 'Starting: testPriorFactor' -% testPriorFactor +%% geometry +display 'Starting: testCal3Unified' +testCal3Unified -display 'Starting: testValues' -testValues +%% linear +display 'Starting: testKalmanFilter' +testKalmanFilter display 'Starting: testJacobianFactor' testJacobianFactor -display 'Starting: testKalmanFilter' -testKalmanFilter +%% nonlinear +display 'Starting: testValues' +testValues +%% SLAM +display 'Starting: testPriorFactor' +testPriorFactor + +%% examples display 'Starting: testLocalizationExample' testLocalizationExample @@ -36,6 +44,7 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample +%% MATLAB specific display 'Starting: testUtilities' testUtilities From 7cdbac4b5cef2a5c1bb07e9a715fdb4565aa51b4 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Fri, 16 Jan 2015 09:33:02 -0500 Subject: [PATCH 63/84] Fix warnings by latest CMake 3.1. Variables should be unquoted: Policy CMP0054 is not set: Only interpret if() arguments as variables or keywords when unquoted. --- cmake/GtsamBuildTypes.cmake | 2 +- gtsam/3rdparty/metis/CMakeLists.txt | 4 ++-- tests/CMakeLists.txt | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 2f11df94e..1bead58d8 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -54,7 +54,7 @@ if(NOT FIRST_PASS_DONE) endif() # Clang on Mac uses a template depth that is less than standard and is too small -if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") +if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") endif() diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index fd9c7eaf7..6ba17787f 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.8) project(METIS) # Add flags for currect directory and below -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") add_definitions(-Wno-unused-variable) if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0) add_definitions(-Wno-sometimes-uninitialized) endif() endif() -if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC")) +if(NOT (${CMAKE_C_COMPILER_ID} MATCHES "MSVC" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC")) #add_definitions(-Wno-unknown-pragmas) endif() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 008dbb78d..068b39eca 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -2,7 +2,7 @@ # note the source dir on each set (tests_exclude "") -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Richard & Jason & Frank +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richard & Jason & Frank # clang linker segfaults on large testSerializationSLAM list (APPEND tests_exclude "testSerializationSLAM.cpp") endif() From aba22015099769afac1bca683b96f40e3e05af89 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Fri, 16 Jan 2015 15:43:42 +0100 Subject: [PATCH 64/84] Fix for the OSX/clang c++ narrowing error --- gtsam/base/Manifold.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index b07bd3575..ba26d267d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -81,7 +81,7 @@ struct ManifoldImpl { /// Extra manifold traits for variable-dimension types template -struct ManifoldImpl { + struct ManifoldImpl { // Run-time dimensionality static int GetDimension(const Class& m) { return m.dim(); From 7382f38de4a5e058ae3bf3217fbb1d541cb7f45a Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 16 Jan 2015 19:15:57 +0100 Subject: [PATCH 65/84] Avoid warning on windows --- .cproject | 106 +++++++++++++++++----------------- gtsam/base/OptionalJacobian.h | 4 +- 2 files changed, 54 insertions(+), 56 deletions(-) diff --git a/.cproject b/.cproject index ce0ab3c4c..8a2eaaf1c 100644 --- a/.cproject +++ b/.cproject @@ -584,7 +584,6 @@ make - tests/testBayesTree.run true false @@ -592,7 +591,6 @@ make - testBinaryBayesNet.run true false @@ -640,7 +638,6 @@ make - testSymbolicBayesNet.run true false @@ -648,7 +645,6 @@ make - tests/testSymbolicFactor.run true false @@ -656,7 +652,6 @@ make - testSymbolicFactorGraph.run true false @@ -672,7 +667,6 @@ make - tests/testBayesTree true false @@ -1104,7 +1098,6 @@ make - testErrors.run true false @@ -1334,46 +1327,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1456,6 +1409,7 @@ make + testSimulated2DOriented.run true false @@ -1495,6 +1449,7 @@ make + testSimulated2D.run true false @@ -1502,6 +1457,7 @@ make + testSimulated3D.run true false @@ -1515,6 +1471,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1780,7 +1776,6 @@ cpack - -G DEB true false @@ -1788,7 +1783,6 @@ cpack - -G RPM true false @@ -1796,7 +1790,6 @@ cpack - -G TGZ true false @@ -1804,7 +1797,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2537,6 +2529,14 @@ true true + + make + -j4 + testOptionalJacobian.run + true + true + true + make -j5 @@ -2675,7 +2675,6 @@ make - testGraph.run true false @@ -2683,7 +2682,6 @@ make - testJunctionTree.run true false @@ -2691,7 +2689,6 @@ make - testSymbolicBayesNetB.run true false @@ -3291,6 +3288,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 08b8151dc..2ea9d672e 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -100,7 +100,7 @@ public: /// Return true is allocated, false if default constructor was used operator bool() const { - return map_.data(); + return map_.data() != NULL; } /// De-reference, like boost optional @@ -156,7 +156,7 @@ public: /// Return true is allocated, false if default constructor was used operator bool() const { - return pointer_; + return pointer_!=NULL; } /// De-reference, like boost optional From a30f7436a497fbcc5cab00cc609ce40b3bd612db Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 19 Jan 2015 12:39:43 +0100 Subject: [PATCH 66/84] Addressing code review --- gtsam/base/Manifold.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index ba26d267d..1791ff131 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -71,7 +71,7 @@ struct HasManifoldPrereqs { }; /// Extra manifold traits for fixed-dimension types -template +template struct ManifoldImpl { // Compile-time dimensionality static int GetDimension(const Class&) { @@ -81,7 +81,7 @@ struct ManifoldImpl { /// Extra manifold traits for variable-dimension types template - struct ManifoldImpl { + struct ManifoldImpl { // Run-time dimensionality static int GetDimension(const Class& m) { return m.dim(); From ab23788d5fc99dae8a3f317dcb05b889d8e2571b Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 19 Jan 2015 12:41:04 +0100 Subject: [PATCH 67/84] Formatting --- gtsam/base/Manifold.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 1791ff131..6998b3b18 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -81,7 +81,7 @@ struct ManifoldImpl { /// Extra manifold traits for variable-dimension types template - struct ManifoldImpl { +struct ManifoldImpl { // Run-time dimensionality static int GetDimension(const Class& m) { return m.dim(); From f8e8729c8dad516fedb1a90023e0eab9117863dd Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 19 Jan 2015 12:11:21 -0500 Subject: [PATCH 68/84] fix Unit3 and EssentialMatrix serialization --- gtsam/geometry/Unit3.h | 8 +++++++- gtsam/geometry/tests/testSerializationGeometry.cpp | 14 ++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 79c3977cd..8784d0eb8 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -155,7 +155,13 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(B_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); } /// @} diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index a7e792cb8..bbc8be1ad 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include #include @@ -41,6 +43,9 @@ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); static Pose3 pose3(rt3, pt3); +static Unit3 unit3(1.0, 2.1, 3.4); +static EssentialMatrix ematrix(rt3, unit3); + static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); @@ -59,6 +64,9 @@ TEST (Serialization, text_geometry) { EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); + EXPECT(equalsObj(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsObj(EssentialMatrix(rt3, unit3))); + EXPECT(equalsObj(pt3)); EXPECT(equalsObj(rt3)); EXPECT(equalsObj(Pose3(rt3, pt3))); @@ -81,6 +89,9 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); + EXPECT(equalsXML(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsXML(EssentialMatrix(rt3, unit3))); + EXPECT(equalsXML(pt3)); EXPECT(equalsXML(rt3)); EXPECT(equalsXML(Pose3(rt3, pt3))); @@ -102,6 +113,9 @@ TEST (Serialization, binary_geometry) { EXPECT(equalsBinary(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsBinary(Rot2::fromDegrees(30.0))); + EXPECT(equalsBinary(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsBinary(EssentialMatrix(rt3, unit3))); + EXPECT(equalsBinary(pt3)); EXPECT(equalsBinary(rt3)); EXPECT(equalsBinary(Pose3(rt3, pt3))); From b202bbd5f1c6d589270e90d421bcd7fd74184780 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 19 Jan 2015 16:18:18 -0500 Subject: [PATCH 69/84] add in simulated camera options --- matlab/+gtsam/plotFlyingResults.m | 41 ++++ matlab/+gtsam/plotProjectedCylinderSamples.m | 42 ---- matlab/+gtsam/points2DTrackMonocular.m | 12 -- matlab/+gtsam/points2DTrackStereo.m | 22 +-- matlab/gtsam_examples/CameraFlyingExample.m | 193 +++++++++++-------- 5 files changed, 160 insertions(+), 150 deletions(-) create mode 100644 matlab/+gtsam/plotFlyingResults.m delete mode 100644 matlab/+gtsam/plotProjectedCylinderSamples.m diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m new file mode 100644 index 000000000..22b6a2de5 --- /dev/null +++ b/matlab/+gtsam/plotFlyingResults.m @@ -0,0 +1,41 @@ +function plotFlyingResults(pts3d, covariance, values, marginals) +% plot the visible points on the cylinders and trajectories +% author: Zhaoyang Lv + +import gtsam.* + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +keys = KeyVector(values.keys); + +%% plot trajectories +for i = 0:keys.size - 1 + if exist('h_result', 'var') + delete(h_result); + end + + key = keys.at(i); + pose = keys.at(key); + + P = marginals.marginalCovariance(key); + + h_result = gtsam.plotPose3(pose, P, 1); +end + +%% plot point covariance + +if exist('h_result', 'var') + delete(h_result); +end + + + +if ~holdstate + hold off +end + +end \ No newline at end of file diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m deleted file mode 100644 index f59434d96..000000000 --- a/matlab/+gtsam/plotProjectedCylinderSamples.m +++ /dev/null @@ -1,42 +0,0 @@ -function plotProjectedCylinderSamples(pts3d, covariance, options, figID) -% plot the visible points on the cylinders -% author: Zhaoyang Lv - - import gtsam.* - - figure(figID); - - holdstate = ishold; - hold on - - pointsNum = length(pts3d); - for i = 1:pointsNum - %gtsam.plotPoint3(p, 'g', covariance{i}); - plotPoint3(pts3d{i}, 'r', covariance{i}); - hold on - end - -% for i=1:pointsNum -% ray = pts2dTracksStereo{i}.between(cameraPose.translation()).vector(); -% dist = norm(ray); -% -% p = plot3(pts2dTracksStereo{i}.x, pts2dTracksStereo{i}.y, pts2dTracksStereo{i}.z, ... -% 'o', 'MarkerFaceColor', 'Green'); -% -% for t=0:0.1:dist -% marchingRay = ray * t; -% p.XData = pts2dTracksStereo{i}.x + marchingRay(1); -% p.YData = pts2dTracksStereo{i}.y + marchingRay(2); -% p.ZData = pts2dTracksStereo{i}.z + marchingRay(3); -% drawnow update -% end -% -% end - - axis equal; - axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - - if ~holdstate - hold off - end -end \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index a4e1a2e5c..04c896188 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -101,20 +101,8 @@ for i = 1:pointsNum end end -% for k = 1:cameraPosesNum -% num = length(pts3d{k}.data); -% for i = 1:num -% pts2dTracksMono.pt3d{i} = pts3d{k}.data{i}; -% pts2dTracksMono.Z{i} = pts3d{k}.Z{i}; -% pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); -% end -% end - -%% plot the result with covariance ellipses -hold on; %plot3DPoints(initialEstimate, [], marginals); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -plot3DTrajectory(initialEstimate, '*', 1, 8); view(3); diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 2359654d3..95f225ea5 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -1,4 +1,4 @@ -function pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders) +function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, imageSize, cylinders) % Assess how accurately we can reconstruct points from a particular monocular camera setup. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization @@ -100,24 +100,10 @@ for i = 1:pointsNum end - -% for k = 1:cameraPosesNum -% if isempty(pts3d{k}.data) -% continue; -% end -% -% for i = 1:length(pts3d{k}.data) -% pts2dTracksStereo.pt3d{end+1} = pts3d{k}.data{i}; -% pts2dTracksStereo.Z{end+1} = pts3d{k}.Z{i}; -% pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i})); -% end -% end - %% plot the result with covariance ellipses -hold on; -%plot3DPoints(initialEstimate, [], marginals); +plotFlyingResults(pts2dTracksStereo.pts3d, pts2dTracksStereo.cov, initialiEstimate, marginals); + %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -view(3); +%view(3); end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index ba9434080..08986f83c 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -10,100 +10,135 @@ % @author Zhaoyang Lv %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + clear all; clc; clf; + import gtsam.* -%% define the options -% the testing field size -options.fieldSize = Point2([100, 100]'); -% the number of cylinders -options.cylinderNum = 20; -% point density on cylinder -options.density = 1; +% test or run +options.enableTests = false; -% The number of camera poses -options.poseNum = 20; -% covariance scaling factor +% the number of cylinders in the field +options.cylinder.cylinderNum = 15; % pls be smaller than 20 +% cylinder size +options.cylinder.radius = 3; % pls be smaller than 5 +options.cylinder.height = 10; +% point density on cylinder +options.cylinder.pointDensity = 1; + +%% set up the camera +% parameters set according to the stereo camera: +% http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html + +% set up monocular camera or stereo camera +options.camera.IS_MONO = false; +% the field of view of camera +options.camera.fov = 120; +% fps for image +options.camera.fps = 25; +% camera pixel resolution +options.camera.resolution = Point2(752, 480); +% camera horizon +options.camera.horizon = 60; +% camera baseline +options.camera.baseline = 0.05; +% camera focal length +options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ... + options.camera.fov); +% camera focal baseline +options.camera.fB = options.camera.f * options.camera.baseline; +% camera disparity +options.camera.disparity = options.camera.fB / options.camera.horizon; +% Monocular Camera Calibration +options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ... + options.camera.resolution.x/2, options.camera.resolution.y/2); +% Stereo Camera Calibration +options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ... + options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity); + +% write video output +options.writeVideo = true; +% the testing field size (unit: meter) +options.fieldSize = Point2([100, 100]'); +% camera flying speed (unit: meter / second) +options.speed = 20; +% number of camera poses in the simulated trajectory +options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps); +% display covariance scaling factor options.scale = 1; - -%% Camera Setup -% Monocular Camera Calibration -options.monoK = Cal3_S2(525,525,0,320,240); -% Stereo Camera Calibration -options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); -% the image size of camera -options.imageSize = Point2([640, 480]'); -% use Monocular camera or Stereo camera -options.Mono = false; -% fps for image -options.fps = 20; -% camera flying speed -options.speed = 20; +% if(options.writeVideo) +% videoObj = VideoWriter('Camera_Flying_Example.avi'); +% videoObj.Quality = 100; +% videoObj.FrameRate = options.fps; +% end -%% test1: visibility test in monocular camera -cylinders{1}.centroid = Point3(30, 50, 5); -cylinders{2}.centroid = Point3(50, 50, 5); -cylinders{3}.centroid = Point3(70, 50, 5); +%% This is for tests +if options.enableTests + % test1: visibility test in monocular camera + cylinders{1}.centroid = Point3(30, 50, 5); + cylinders{2}.centroid = Point3(50, 50, 5); + cylinders{3}.centroid = Point3(70, 50, 5); -for i = 1:3 - cylinders{i}.radius = 5; - cylinders{i}.height = 10; - - cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0)); - cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); + for i = 1:3 + cylinders{i}.radius = 5; + cylinders{i}.height = 10; + + cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0)); + cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); + end + + camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... + Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... + Point3([0,0,1]'), options.monoK); + + pose = camera.pose; + prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ... + options.camera.resolution, cylinders); + + % test2: visibility test in stereo camera + prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ... + pose, options.camera.resolution, cylinders); end -camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... - Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... - Point3([0,0,1]'), options.monoK); - -pose = camera.pose; -prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders); - -%% test2: visibility test in stereo camera -prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); - -%% generate a set of cylinders and samples -cylinderNum = options.cylinderNum; +%% generate a set of cylinders and point samples on cylinders +cylinderNum = options.cylinder.cylinderNum; cylinders = cell(cylinderNum, 1); - -% It seems random generated cylinders doesn't work that well -% Now it set up a circle of cylinders +baseCentroid = cell(cylinderNum, 1); theta = 0; -for i = 1:cylinderNum +i = 1; +while i <= cylinderNum theta = theta + 2*pi/10; - x = 30 * rand * cos(theta) + options.fieldSize.x/2; + x = 40 * rand * cos(theta) + options.fieldSize.x/2; y = 20 * rand * sin(theta) + options.fieldSize.y/2; - baseCentroid = Point2([x, y]'); - cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density); + baseCentroid{i} = Point2([x, y]'); + + % prevent two cylinders interact with each other + regenerate = false; + for j = 1:i-1 + if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2 + regenerate = true; + break; + end + end + if regenerate + continue; + end + + cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ... + options.cylinder.height, options.cylinder.pointDensity); + i = i+1; end - %% plot all the cylinders and sampled points % now is plotting on a 100 * 100 field figID = 1; figure(figID); plotCylinderSamples(cylinders, options, figID); -% %% generate ground truth camera trajectories: a circle -% KMono = Cal3_S2(525,525,0,320,240); -% cameraPoses = cell(options.poseNum, 1); -% theta = 0; -% r = 40; -% for i = 1:options.poseNum -% theta = (i-1)*2*pi/options.poseNum; -% t = Point3([r*cos(theta) + options.fieldSize.x/2, ... -% r*sin(theta) + options.fieldSize.y/2, 10]'); -% camera = SimpleCamera.Lookat(t, ... -% Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... -% Point3([0,0,1]'), options.monoK); -% cameraPoses{i} = camera.pose; -% end - %% generate ground truth camera trajectories: a line KMono = Cal3_S2(525,525,0,320,240); cameraPoses = cell(options.poseNum, 1); @@ -113,24 +148,22 @@ for i = 1:options.poseNum 15, 10]'); camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... - Point3([0,0,1]'), options.monoK); + Point3([0,0,1]'), options.camera.monoK); cameraPoses{i} = camera.pose; end %% set up camera and get measurements -if options.Mono +if options.camera.IS_MONO % use Monocular Camera - pts2dTracksMono = points2DTrackMonocular(options.monoK, cameraPoses, ... - options.imageSize, cylinders); + pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ... + options.camera.resolution, cylinders); else % use Stereo Camera - pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ... - options.imageSize, cylinders); + [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... + cameraPoses, options.camera.resolution, cylinders); - figID = 2; - plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID); - + plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, estimateValuesStereo, options, figID); end %% plot all the projected points @@ -142,6 +175,10 @@ end %plot3DTrajectory(); +%%close video +% if(options.writeVideo) +% close(videoObj); +% end From 1c5cdb830b3cbcb955ae9ae51425e8c286fd1241 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 19 Jan 2015 16:52:48 -0500 Subject: [PATCH 70/84] change of point density to make it plotable --- matlab/+gtsam/plotFlyingResults.m | 5 +++-- matlab/+gtsam/points2DTrackStereo.m | 2 +- matlab/gtsam_examples/CameraFlyingExample.m | 4 +--- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 22b6a2de5..c214d948d 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -24,16 +24,17 @@ for i = 0:keys.size - 1 P = marginals.marginalCovariance(key); h_result = gtsam.plotPose3(pose, P, 1); + end %% plot point covariance + + if exist('h_result', 'var') delete(h_result); end - - if ~holdstate hold off end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 95f225ea5..3651bff6d 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -101,7 +101,7 @@ for i = 1:pointsNum end %% plot the result with covariance ellipses -plotFlyingResults(pts2dTracksStereo.pts3d, pts2dTracksStereo.cov, initialiEstimate, marginals); +plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, initialEstimate, marginals); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); %view(3); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 08986f83c..99fe4ca80 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -26,7 +26,7 @@ options.cylinder.cylinderNum = 15; % pls be smaller than 20 options.cylinder.radius = 3; % pls be smaller than 5 options.cylinder.height = 10; % point density on cylinder -options.cylinder.pointDensity = 1; +options.cylinder.pointDensity = 0.05; %% set up the camera % parameters set according to the stereo camera: @@ -162,8 +162,6 @@ else % use Stereo Camera [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... cameraPoses, options.camera.resolution, cylinders); - - plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, estimateValuesStereo, options, figID); end %% plot all the projected points From 73455833fc23889104049e225ca632291660f2f4 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 19 Jan 2015 23:56:04 -0500 Subject: [PATCH 71/84] ploting trajectories animation --- matlab/+gtsam/covarianceEllipse3D.m | 2 +- matlab/+gtsam/plotFlyingResults.m | 86 ++++++++++++++++----- matlab/+gtsam/points2DTrackStereo.m | 34 ++++---- matlab/gtsam_examples/CameraFlyingExample.m | 27 +------ 4 files changed, 83 insertions(+), 66 deletions(-) diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 9682a9fc1..99a164068 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -1,4 +1,4 @@ -function covarianceEllipse3D(c,P) +function sc = covarianceEllipse3D(c,P) % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index c214d948d..3c3e1e670 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -1,42 +1,90 @@ -function plotFlyingResults(pts3d, covariance, values, marginals) +function plotFlyingResults(pts3d, pts3dCov, poses, posesCov, cylinders, options) % plot the visible points on the cylinders and trajectories % author: Zhaoyang Lv import gtsam.* -haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); - holdstate = ishold; hold on -keys = KeyVector(values.keys); +if(options.writeVideo) + videoObj = VideoWriter('Camera_Flying_Example.avi'); + videoObj.Quality = 100; + videoObj.FrameRate = options.camera.fps; +end + +%% plot all the cylinders and sampled points +% now is plotting on a 100 * 100 field +figID = 1; +figure(figID); + +axis equal +axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + +view(3); + +sampleDensity = 120; +cylinderNum = length(cylinders); +for i = 1:cylinderNum + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); + + X = X + cylinders{i}.centroid.x; + Y = Y + cylinders{i}.centroid.y; + Z = Z * cylinders{i}.height; + + cylinderHandle = surf(X,Y,Z); + set(cylinderHandle, 'FaceAlpha', 0.5); + hold on +end %% plot trajectories -for i = 0:keys.size - 1 - if exist('h_result', 'var') - delete(h_result); +posesSize = length(poses); +for i = 1:posesSize + if i > 1 + plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b'); end - - key = keys.at(i); - pose = keys.at(key); - P = marginals.marginalCovariance(key); + if exist('h_cov', 'var') + delete(h_cov); + end - h_result = gtsam.plotPose3(pose, P, 1); - + gRp = poses{i}.rotation().matrix(); % rotation from pose to global + C = poses{i}.translation().vector(); + axisLength = 2; + + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); + + pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame + gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + h_cov = gtsam.covarianceEllipse3D(C,gPp); + + drawnow; end %% plot point covariance - - -if exist('h_result', 'var') - delete(h_result); +pointSize = length(pts3d); +for i = 1:pointSize + end if ~holdstate hold off end - + +%%close video +if(options.writeVideo) + close(videoObj); +end + end \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 3651bff6d..c80c90d67 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -1,4 +1,4 @@ -function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, imageSize, cylinders) +function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders) % Assess how accurately we can reconstruct points from a particular monocular camera setup. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization @@ -36,7 +36,7 @@ graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; for i = 1:cameraPosesNum - pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders); + pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders); if isempty(pts3d{i}.Z) continue; @@ -48,12 +48,11 @@ for i = 1:cameraPosesNum points3d{index}.Z{end+1} = pts3d{i}.Z{j}; points3d{index}.cameraConstraint{end+1} = i; points3d{index}.visiblity = true; + + graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... + stereoNoise, symbol('x', i), symbol('p', index), K)); end - for j = 1:length(pts3d{i}.Z) - graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... - stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); - end end %% initialize graph and values @@ -64,18 +63,7 @@ end for i = 1:pointsNum point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); - - if ~points3d{i}.visiblity - continue; - end - - factorNum = length(points3d{i}.Z); - for j = 1:factorNum - cameraIdx = points3d{i}.cameraConstraint{j}; - graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ... - stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K)); - end + initialEstimate.insert(symbol('p', i), point_j); end @@ -96,12 +84,16 @@ for i = 1:pointsNum pts2dTracksStereo.pt3d{end+1} = points3d{i}.data; pts2dTracksStereo.Z{end+1} = points3d{i}.Z; - pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); - + pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); +end + +cameraPosesCov = cell(cameraPosesNum, 1); +for i = 1:cameraPosesNum + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i)); end %% plot the result with covariance ellipses -plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, initialEstimate, marginals); +plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); %view(3); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 99fe4ca80..bcb3861f3 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -37,7 +37,7 @@ options.camera.IS_MONO = false; % the field of view of camera options.camera.fov = 120; % fps for image -options.camera.fps = 25; +options.camera.fps = 10; % camera pixel resolution options.camera.resolution = Point2(752, 480); % camera horizon @@ -69,12 +69,6 @@ options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps); % display covariance scaling factor options.scale = 1; -% if(options.writeVideo) -% videoObj = VideoWriter('Camera_Flying_Example.avi'); -% videoObj.Quality = 100; -% videoObj.FrameRate = options.fps; -% end - %% This is for tests if options.enableTests @@ -133,12 +127,6 @@ while i <= cylinderNum i = i+1; end -%% plot all the cylinders and sampled points -% now is plotting on a 100 * 100 field -figID = 1; -figure(figID); -plotCylinderSamples(cylinders, options, figID); - %% generate ground truth camera trajectories: a line KMono = Cal3_S2(525,525,0,320,240); cameraPoses = cell(options.poseNum, 1); @@ -161,22 +149,11 @@ if options.camera.IS_MONO else % use Stereo Camera [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... - cameraPoses, options.camera.resolution, cylinders); + cameraPoses, options, cylinders); end -%% plot all the projected points -% plot the 2D tracks - -% ToDo: plot the trajectories -%plot3DTrajectory(); - - -%%close video -% if(options.writeVideo) -% close(videoObj); -% end From 47c68f678c51be0668c92eda79f66c066f78bcb1 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 20 Jan 2015 14:06:39 -0500 Subject: [PATCH 72/84] generate a flying camera video --- matlab/+gtsam/plotFlyingResults.m | 37 +++++++++++++++++++++++++++++-- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 3c3e1e670..77a096f17 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -11,6 +11,7 @@ if(options.writeVideo) videoObj = VideoWriter('Camera_Flying_Example.avi'); videoObj.Quality = 100; videoObj.FrameRate = options.camera.fps; + open(videoObj); end %% plot all the cylinders and sampled points @@ -32,11 +33,18 @@ for i = 1:cylinderNum Y = Y + cylinders{i}.centroid.y; Z = Z * cylinders{i}.height; - cylinderHandle = surf(X,Y,Z); - set(cylinderHandle, 'FaceAlpha', 0.5); + h_cylinder = surf(X,Y,Z); + set(h_cylinder, 'FaceAlpha', 0.5); hold on end +drawnow; + +if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); +end + %% plot trajectories posesSize = length(poses); for i = 1:posesSize @@ -69,13 +77,38 @@ for i = 1:posesSize h_cov = gtsam.covarianceEllipse3D(C,gPp); drawnow; + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end end + +if exist('h_cov', 'var') + delete(h_cov); +end + +% wait for two seconds +pause(2); + + %% plot point covariance +% if exist('h_cylinder', 'var') +% delete(h_cylinder); +% end + pointSize = length(pts3d); for i = 1:pointSize + plot3(pts3d{i}.x, pts3d{i}.y, pts3d{i}.z); + gtsam.covarianceEllipse3D([pts3d{i}.x; pts3d{i}.y; pts3d{i}.z], pts3dCov{i}); + %drawnow; + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end end if ~holdstate From 5cde63acd26968924186927e64960b74aebb2c32 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 21 Jan 2015 00:14:37 -0500 Subject: [PATCH 73/84] plot incremental position --- matlab/+gtsam/plotFlyingResults.m | 46 +++++++++++------ matlab/+gtsam/points2DTrackStereo.m | 57 ++++++++++----------- matlab/gtsam_examples/CameraFlyingExample.m | 15 ++++-- 3 files changed, 67 insertions(+), 51 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 77a096f17..7a0ca77c8 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -1,4 +1,4 @@ -function plotFlyingResults(pts3d, pts3dCov, poses, posesCov, cylinders, options) +function plotFlyingResults(pts3d, poses, posesCov, cylinders, options) % plot the visible points on the cylinders and trajectories % author: Zhaoyang Lv @@ -45,15 +45,16 @@ if options.writeVideo writeVideo(videoObj, currFrame); end -%% plot trajectories +%% plot trajectories and points posesSize = length(poses); +pointSize = length(pts3d); for i = 1:posesSize if i > 1 plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b'); end if exist('h_cov', 'var') - delete(h_cov); + delete(h_pose_cov); end gRp = poses{i}.rotation().matrix(); % rotation from pose to global @@ -74,7 +75,28 @@ for i = 1:posesSize pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - h_cov = gtsam.covarianceEllipse3D(C,gPp); + h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); + + + if exist('h_point', 'var') + for j = 1:pointSize + delete(h_point{j}); + end + end + if exist('h_point_cov', 'var') + for j = 1:pointSize + delete(h_point_cov{j}); + end + end + + h_point = cell(pointSize, 1); + h_point_cov = cell(pointSize, 1); + for j = 1:pointSize + if ~isempty(pts3d{j}.cov{i}) + h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z); + h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], pts3d{j}.cov{i}); + end + end drawnow; @@ -85,8 +107,8 @@ for i = 1:posesSize end -if exist('h_cov', 'var') - delete(h_cov); +if exist('h_pose_cov', 'var') + delete(h_pose_cov); end % wait for two seconds @@ -99,17 +121,7 @@ pause(2); % delete(h_cylinder); % end -pointSize = length(pts3d); -for i = 1:pointSize - plot3(pts3d{i}.x, pts3d{i}.y, pts3d{i}.z); - gtsam.covarianceEllipse3D([pts3d{i}.x; pts3d{i}.y; pts3d{i}.z], pts3dCov{i}); - %drawnow; - - if options.writeVideo - currFrame = getframe(gcf); - writeVideo(videoObj, currFrame); - end -end + if ~holdstate hold off diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index c80c90d67..295b4d18e 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -12,7 +12,7 @@ graph = NonlinearFactorGraph; %% create the noise factors poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -stereoNoise = noiseModel.Isotropic.Sigma(3,0.2); +stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05); cameraPosesNum = length(cameraPoses); @@ -28,13 +28,21 @@ for i = 1:cylinderNum points3d{end}.Z = cell(0); points3d{end}.cameraConstraint = cell(0); points3d{end}.visiblity = false; + points3d{end}.cov = cell(cameraPosesNum); end end graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); -pts3d = cell(cameraPosesNum, 1); +%% initialize graph and values initialEstimate = Values; +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.05*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); +end + +pts3d = cell(cameraPosesNum, 1); +cameraPosesCov = cell(cameraPosesNum, 1); for i = 1:cameraPosesNum pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders); @@ -52,26 +60,24 @@ for i = 1:cameraPosesNum graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... stereoNoise, symbol('x', i), symbol('p', index), K)); end + + pose_i = cameraPoses{i}.retract(poseNoiseSigmas); + initialEstimate.insert(symbol('x', i), pose_i); + + %% linearize the graph + marginals = Marginals(graph, initialEstimate); + + for j = 1:pointsNum + if points3d{j}.visiblity + points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j)); + end + end + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i)); end -%% initialize graph and values -for i = 1:cameraPosesNum - pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); - initialEstimate.insert(symbol('x', i), pose_i); -end - -for i = 1:pointsNum - point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); -end - - -%% Print the graph -graph.print(sprintf('\nFactor graph:\n')); - -%% linearize the graph -marginals = Marginals(graph, initialEstimate); +%% Plot the result +plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options); %% get all the 2d points track information pts2dTracksStereo.pt3d = cell(0); @@ -87,15 +93,8 @@ for i = 1:pointsNum pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); end -cameraPosesCov = cell(cameraPosesNum, 1); -for i = 1:cameraPosesNum - cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i)); -end - -%% plot the result with covariance ellipses -plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options); - -%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -%view(3); +% +% %% plot the result with covariance ellipses +% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options); end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index bcb3861f3..01e335d15 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -20,6 +20,7 @@ import gtsam.* % test or run options.enableTests = false; +%% cylinder options % the number of cylinders in the field options.cylinder.cylinderNum = 15; % pls be smaller than 20 % cylinder size @@ -28,7 +29,7 @@ options.cylinder.height = 10; % point density on cylinder options.cylinder.pointDensity = 0.05; -%% set up the camera +%% camera options % parameters set according to the stereo camera: % http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html @@ -66,9 +67,14 @@ options.fieldSize = Point2([100, 100]'); options.speed = 20; % number of camera poses in the simulated trajectory options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps); -% display covariance scaling factor -options.scale = 1; +%% ploting options +% display covariance scaling factor +options.plot.scale = 1; +% plot the trajectory covariance +options.plot.DISP_TRAJ_COV = true; +% plot points covariance +options.plot.POINTS_COV = true; %% This is for tests if options.enableTests @@ -140,7 +146,6 @@ for i = 1:options.poseNum cameraPoses{i} = camera.pose; end - %% set up camera and get measurements if options.camera.IS_MONO % use Monocular Camera @@ -148,7 +153,7 @@ if options.camera.IS_MONO options.camera.resolution, cylinders); else % use Stereo Camera - [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... + pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ... cameraPoses, options, cylinders); end From be37e1ed0540e403d4d51bd6f058379cefcf0cde Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Wed, 21 Jan 2015 13:02:35 -0500 Subject: [PATCH 74/84] Fix for Issue #201 It seems like MSVC was unable to identify the template specialization for the 'const' keyword. So added a specialization in each of these files for that --- gtsam/geometry/Cal3Bundler.h | 3 +++ gtsam/geometry/Cal3DS2.h | 3 +++ gtsam/geometry/Cal3Unified.h | 3 +++ gtsam/geometry/Cal3_S2.h | 3 +++ gtsam/geometry/Cal3_S2Stereo.h | 4 ++++ gtsam/geometry/CalibratedCamera.h | 3 +++ gtsam/geometry/EssentialMatrix.h | 3 +++ gtsam/geometry/PinholeCamera.h | 3 +++ gtsam/geometry/Point3.h | 3 +++ gtsam/geometry/Pose2.h | 3 +++ gtsam/geometry/Pose3.h | 4 ++++ gtsam/geometry/Rot2.h | 3 +++ gtsam/geometry/Rot3.h | 3 +++ gtsam/geometry/SO3.h | 3 ++- gtsam/geometry/StereoCamera.h | 2 ++ gtsam/geometry/StereoPoint2.h | 3 +++ gtsam/geometry/Unit3.h | 2 ++ gtsam/slam/dataset.cpp | 1 + 18 files changed, 51 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index ce9f94c48..e90ae32a4 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -174,4 +174,7 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0c77eebbc..0cb914ce3 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -112,5 +112,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d0e0f3891..624d7dbb4 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -142,5 +142,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 0c5386822..b9e2ef581 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -226,4 +226,7 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } // \ namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 3585fb156..651a7fabb 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -158,4 +158,8 @@ namespace gtsam { struct traits : public internal::Manifold { }; + template<> + struct traits : public internal::Manifold { + }; + } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index cda01b600..9e907f1d5 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -204,5 +204,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index c9e702078..606f62f87 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -201,5 +201,8 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; + } // gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index b409c956d..12eb0235d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -499,4 +499,7 @@ private: template struct traits< PinholeCamera > : public internal::Manifold > {}; +template +struct traits< const PinholeCamera > : public internal::Manifold > {}; + } // \ gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index c9dee9003..95f728e39 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -197,4 +197,7 @@ namespace gtsam { template<> struct traits : public internal::VectorSpace {}; + + template<> + struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index be860107e..d4b647949 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -293,5 +293,8 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> struct traits : public internal::LieGroupTraits {}; +template<> +struct traits : public internal::LieGroupTraits {}; + } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d30bd4167..4130a252e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -325,4 +325,8 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> struct traits : public internal::LieGroupTraits {}; +template<> +struct traits : public internal::LieGroupTraits {}; + + } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 95f025622..deae1f3a0 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -210,4 +210,7 @@ namespace gtsam { template<> struct traits : public internal::LieGroupTraits {}; + template<> + struct traits : public internal::LieGroupTraits {}; + } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e9568fb26..4e42d7efb 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -463,5 +463,8 @@ namespace gtsam { template<> struct traits : public internal::LieGroupTraits {}; + + template<> + struct traits : public internal::LieGroupTraits {}; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index bed2f1212..e52eaae1e 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -85,6 +85,7 @@ public: template<> struct traits : public internal::LieGroupTraits {}; - +template<> +struct traits : public internal::LieGroupTraits {}; } // end namespace gtsam diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 49abcf36b..913b1eab3 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -149,4 +149,6 @@ private: template<> struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 2ec745fd8..803c59a4b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -176,4 +176,7 @@ namespace gtsam { template<> struct traits : public internal::Manifold {}; + + template<> + struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8784d0eb8..50ffb55b7 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -171,5 +171,7 @@ private: // Define GTSAM traits template <> struct traits : public internal::Manifold {}; +template <> struct traits : public internal::Manifold {}; + } // namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index a6fb2d830..d3a7c1e84 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include From e1ae980d455945ec401af1ec762b74b4f8d4a2e7 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Wed, 21 Jan 2015 13:16:13 -0500 Subject: [PATCH 75/84] Fix for #204 int to size_t conversions and few others --- examples/RangeISAMExample_plaza2.cpp | 3 ++- examples/TimeTBB.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 252372f39..04632e9e3 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -82,7 +82,8 @@ vector readTriples() { ifstream is(data_file.c_str()); while (is) { - double t, sender, receiver, range; + double t, sender, range; + size_t receiver; is >> t >> sender >> receiver >> range; triples.push_back(RangeTriple(t, receiver, range)); } diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index 077331848..a35980836 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -134,7 +134,7 @@ map testWithMemoryAllocation() tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithAllocation(results)); tbb::tick_count t1 = tbb::tick_count::now(); cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl; - timingResults[grainSize] = (t1 - t0).seconds(); + timingResults[(int)grainSize] = (t1 - t0).seconds(); } return timingResults; @@ -152,9 +152,9 @@ int main(int argc, char* argv[]) BOOST_FOREACH(size_t n, numThreads) { cout << "With " << n << " threads:" << endl; - tbb::task_scheduler_init init(n); - results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); - results[n].grainSizesWithAllocation = testWithMemoryAllocation(); + tbb::task_scheduler_init init((int)n); + results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); + results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation(); cout << endl; } From 2627f9a9cd23b3b4aace8232f26de6a745baa715 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 21 Jan 2015 16:18:03 -0500 Subject: [PATCH 76/84] flying camera view changes --- matlab/+gtsam/plotFlyingResults.m | 15 ++++++++++----- matlab/+gtsam/points2DTrackMonocular.m | 2 -- matlab/gtsam_examples/CameraFlyingExample.m | 2 +- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 7a0ca77c8..d09842934 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -19,10 +19,7 @@ end figID = 1; figure(figID); -axis equal -axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - -view(3); +view([30, 0]); sampleDensity = 120; cylinderNum = length(cylinders); @@ -34,7 +31,7 @@ for i = 1:cylinderNum Z = Z * cylinders{i}.height; h_cylinder = surf(X,Y,Z); - set(h_cylinder, 'FaceAlpha', 0.5); + set(h_cylinder, 'FaceColor', [0 0 0.5], 'FaceAlpha', 0.2, 'EdgeColor', [0 0 1]); hold on end @@ -98,6 +95,9 @@ for i = 1:posesSize end end + axis equal + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + drawnow; if options.writeVideo @@ -114,6 +114,11 @@ end % wait for two seconds pause(2); +% change views +for i = 0 : 0.5 : 60 + view([i + 30, i]); +end + %% plot point covariance diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 04c896188..3d552aaa2 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -103,7 +103,5 @@ end %plot3DPoints(initialEstimate, [], marginals); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -view(3); - end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 01e335d15..1d779b135 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -138,7 +138,7 @@ KMono = Cal3_S2(525,525,0,320,240); cameraPoses = cell(options.poseNum, 1); theta = 0; for i = 1:options.poseNum - t = Point3([(i-1)*(options.fieldSize.x - 20)/options.poseNum + 20, ... + t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... 15, 10]'); camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... From fa023aac1af4d59200a84206b38753845ce3636d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 00:25:06 -0500 Subject: [PATCH 77/84] change lightings and add flying through sequence --- matlab/+gtsam/plotCamera.m | 8 ++--- matlab/+gtsam/plotFlyingResults.m | 59 +++++++++++++++++-------------- 2 files changed, 37 insertions(+), 30 deletions(-) diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index ba352b757..f5f164678 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -4,15 +4,15 @@ function plotCamera(pose, axisLength) xAxis = C+R(:,1)*axisLength; L = [C xAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','r'); + h_x = line(L(:,1),L(:,2),L(:,3),'Color','r'); yAxis = C+R(:,2)*axisLength; L = [C yAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','g'); + h_y = line(L(:,1),L(:,2),L(:,3),'Color','g'); zAxis = C+R(:,3)*axisLength; L = [C zAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','b'); + h_z = line(L(:,1),L(:,2),L(:,3),'Color','b'); axis equal -end \ No newline at end of file +end diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index d09842934..57a656a7f 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -19,7 +19,10 @@ end figID = 1; figure(figID); -view([30, 0]); +view([30, 30]); + +hlight = camlight('headlight'); +lighting gouraud sampleDensity = 120; cylinderNum = length(cylinders); @@ -31,7 +34,8 @@ for i = 1:cylinderNum Z = Z * cylinders{i}.height; h_cylinder = surf(X,Y,Z); - set(h_cylinder, 'FaceColor', [0 0 0.5], 'FaceAlpha', 0.2, 'EdgeColor', [0 0 1]); + set(h_cylinder, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); + h_cylinder.AmbientStrength = 1; hold on end @@ -53,23 +57,25 @@ for i = 1:posesSize if exist('h_cov', 'var') delete(h_pose_cov); end + + plotCamera(poses{i}, 2); - gRp = poses{i}.rotation().matrix(); % rotation from pose to global - C = poses{i}.translation().vector(); - axisLength = 2; - - xAxis = C+gRp(:,1)*axisLength; - L = [C xAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','r'); - - yAxis = C+gRp(:,2)*axisLength; - L = [C yAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','g'); - - zAxis = C+gRp(:,3)*axisLength; - L = [C zAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','b'); - + gRp = poses{i}.rotation().matrix(); % rotation from pose to global + C = poses{i}.translation().vector(); +% axisLength = 2; +% +% xAxis = C+gRp(:,1)*axisLength; +% L = [C xAxis]'; +% line(L(:,1),L(:,2),L(:,3),'Color','r'); +% +% yAxis = C+gRp(:,2)*axisLength; +% L = [C yAxis]'; +% line(L(:,1),L(:,2),L(:,3),'Color','g'); +% +% zAxis = C+gRp(:,3)*axisLength; +% L = [C zAxis]'; +% line(L(:,1),L(:,2),L(:,3),'Color','b'); +% pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); @@ -114,18 +120,19 @@ end % wait for two seconds pause(2); -% change views +% change views angle for i = 0 : 0.5 : 60 view([i + 30, i]); end - -%% plot point covariance - -% if exist('h_cylinder', 'var') -% delete(h_cylinder); -% end - +% camera flying through +for i = 1 : posesSize + campos([poses{i}.x, poses{i}.y, poses{i}.z]); + camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]); + camlight(hlight, 'headlight'); + + drawnow +end if ~holdstate From e74d64c90bbd922a85189f0b6bf71014a612a475 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 11:24:43 -0500 Subject: [PATCH 78/84] add labels --- matlab/+gtsam/covarianceEllipse3D.m | 2 + matlab/+gtsam/plotCamera.m | 2 + matlab/+gtsam/plotFlyingResults.m | 124 +++++++++++++++++----------- matlab/+gtsam/points2DTrackStereo.m | 1 + 4 files changed, 81 insertions(+), 48 deletions(-) diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 99a164068..c99b19117 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -6,6 +6,8 @@ function sc = covarianceEllipse3D(c,P) % % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966 +hold on + [e,s] = svd(P); k = 11.82; radii = k*sqrt(diag(s)); diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index f5f164678..d0d1f45b7 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -1,4 +1,6 @@ function plotCamera(pose, axisLength) + hold on + C = pose.translation().vector(); R = pose.rotation().matrix(); diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 57a656a7f..85042f5a8 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -1,11 +1,35 @@ function plotFlyingResults(pts3d, poses, posesCov, cylinders, options) % plot the visible points on the cylinders and trajectories +% % author: Zhaoyang Lv import gtsam.* -holdstate = ishold; -hold on +figID = 1; +figure(figID); +set(gcf, 'Position', [80,1,1800,1000]); + + +%% plot all the cylinders and sampled points + +axis equal +axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 30]); +xlabel('X (m)'); +ylabel('Y (m)'); +zlabel('Height (m)'); + +if options.camera.IS_MONO + h_title = title('Quadrotor Flight Simulation with Monocular Camera'); +else + h_title = title('Quadrotor Flight Simulation with Stereo Camera'); +end + +text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed)) + +view([30, 30]); + +hlight = camlight('headlight'); +lighting gouraud if(options.writeVideo) videoObj = VideoWriter('Camera_Flying_Example.avi'); @@ -14,36 +38,24 @@ if(options.writeVideo) open(videoObj); end -%% plot all the cylinders and sampled points -% now is plotting on a 100 * 100 field -figID = 1; -figure(figID); - -view([30, 30]); - -hlight = camlight('headlight'); -lighting gouraud sampleDensity = 120; cylinderNum = length(cylinders); -for i = 1:cylinderNum +h_cylinder = cell(cylinderNum); +for i = 1:cylinderNum + + hold on + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); X = X + cylinders{i}.centroid.x; Y = Y + cylinders{i}.centroid.y; Z = Z * cylinders{i}.height; - h_cylinder = surf(X,Y,Z); - set(h_cylinder, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); - h_cylinder.AmbientStrength = 1; - hold on -end - -drawnow; - -if options.writeVideo - currFrame = getframe(gcf); - writeVideo(videoObj, currFrame); + h_cylinder{i} = surf(X,Y,Z); + set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); + h_cylinder{i}.AmbientStrength = 0.8; + end %% plot trajectories and points @@ -51,35 +63,35 @@ posesSize = length(poses); pointSize = length(pts3d); for i = 1:posesSize if i > 1 + hold on plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b'); end - if exist('h_cov', 'var') + if exist('h_pose_cov', 'var') delete(h_pose_cov); end - plotCamera(poses{i}, 2); + %plotCamera(poses{i}, 3); + + gRp = poses{i}.rotation().matrix(); % rotation from pose to global + C = poses{i}.translation().vector(); + axisLength = 3; + + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); - gRp = poses{i}.rotation().matrix(); % rotation from pose to global - C = poses{i}.translation().vector(); -% axisLength = 2; -% -% xAxis = C+gRp(:,1)*axisLength; -% L = [C xAxis]'; -% line(L(:,1),L(:,2),L(:,3),'Color','r'); -% -% yAxis = C+gRp(:,2)*axisLength; -% L = [C yAxis]'; -% line(L(:,1),L(:,2),L(:,3),'Color','g'); -% -% zAxis = C+gRp(:,3)*axisLength; -% L = [C zAxis]'; -% line(L(:,1),L(:,2),L(:,3),'Color','b'); -% pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); - if exist('h_point', 'var') for j = 1:pointSize @@ -96,15 +108,16 @@ for i = 1:posesSize h_point_cov = cell(pointSize, 1); for j = 1:pointSize if ~isempty(pts3d{j}.cov{i}) + hold on h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z); h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], pts3d{j}.cov{i}); end end axis equal - axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - drawnow; + drawnow if options.writeVideo currFrame = getframe(gcf); @@ -120,16 +133,31 @@ end % wait for two seconds pause(2); -% change views angle -for i = 0 : 0.5 : 60 - view([i + 30, i]); +%% change views angle +for i = 30 : i : 90 + view([30, i]); + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end + + drawnow end -% camera flying through +% changing perspective + + +%% camera flying through video for i = 1 : posesSize campos([poses{i}.x, poses{i}.y, poses{i}.z]); camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]); camlight(hlight, 'headlight'); + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end drawnow end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 295b4d18e..60c9f1295 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -2,6 +2,7 @@ function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPos % Assess how accurately we can reconstruct points from a particular monocular camera setup. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization +% % @author: Zhaoyang Lv import gtsam.* From d47e4d4853e9acdef27e956cd8585073f530fa3a Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 15:00:21 -0500 Subject: [PATCH 79/84] changes to monocular camera --- matlab/+gtsam/points2DTrackMonocular.m | 55 +++++++++++--------------- 1 file changed, 23 insertions(+), 32 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 3d552aaa2..3ac501904 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -12,7 +12,6 @@ graph = NonlinearFactorGraph; %% create the noise factors poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); - measurementNoiseSigma = 1.0; measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); @@ -30,15 +29,22 @@ for i = 1:cylinderNum points3d{end}.Z = cell(0); points3d{end}.cameraConstraint = cell(0); points3d{end}.visiblity = false; + points3d{end}.cov = cell(cameraPosesNum); end end graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); -pts3d = cell(cameraPosesNum, 1); +%% initialize graph and values initialEstimate = Values; -for i = 1:cameraPosesNum - +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); +end + +pts3d = cell(cameraPosesNum, 1); +cameraPosesCov = cell(cameraPosesNum, 1); +for i = 1:cameraPosesNum cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); @@ -47,43 +53,31 @@ for i = 1:cameraPosesNum index = pts3d{i}.overallIdx{j}; points3d{index}.Z{end+1} = pts3d{i}.Z{j}; points3d{index}.cameraConstraint{end+1} = i; - points3d{index}.visiblity = true; + points3d{index}.visiblity = true; + + graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', index), K) ); end - -end -%% initialize graph and values -for i = 1:cameraPosesNum pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); -end -for i = 1:pointsNum - % add in values - point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); + marginals = Marginals(graph, initialEstimate); - if ~points3d{i}.visiblity - continue; - end - % single measurement. not added to graph - factorNum = length(points3d{i}.Z); - if factorNum > 1 - for j = 1:factorNum - cameraIdx = points3d{i}.cameraConstraint{j}; - graph.add(GenericProjectionFactorCal3_S2(points3d{i}.Z{j}, ... - measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) ); + for j = 1:pointsNum + if points3d{j}.visiblity + points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j)); end end - + + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i)); end - + %% Print the graph graph.print(sprintf('\nFactor graph:\n')); -%% linearize the graph -% currently throws the Indeterminant linear system exception -marginals = Marginals(graph, initialEstimate); +%% Plot the result +plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options); %% get all the points track information for i = 1:pointsNum @@ -101,7 +95,4 @@ for i = 1:pointsNum end end -%plot3DPoints(initialEstimate, [], marginals); -%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); - end From 34ae976f6ab18f276c7452519247569a6ca4e2fa Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 15:35:28 -0500 Subject: [PATCH 80/84] fix the indeterminant system in stereo case --- matlab/+gtsam/cylinderSampleProjectionStereo.m | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 51cda12ac..6512231e8 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -42,6 +42,11 @@ for i = 1:cylinderNum continue; end + % too small disparity may call indeterminant system exception + if Z.du < 0.6 + continue; + end + % ignore points occluded % use a simple math hack to check occlusion: % 1. All points in front of cylinders' surfaces are visible From fd6b377d4b738dd0c58919025f51fcb06baf4559 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 16:53:36 -0500 Subject: [PATCH 81/84] check under constrained measurement in monocular camera setup --- matlab/+gtsam/points2DTrackMonocular.m | 29 +++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 3ac501904..fc48f587e 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -27,7 +27,8 @@ for i = 1:cylinderNum for j = 1:cylinderPointsNum points3d{end+1}.data = cylinders{i}.Points{j}; points3d{end}.Z = cell(0); - points3d{end}.cameraConstraint = cell(0); + points3d{end}.camConstraintIdx = cell(0); + points3d{end}.added = cell(0); points3d{end}.visiblity = false; points3d{end}.cov = cell(cameraPosesNum); end @@ -44,6 +45,7 @@ end pts3d = cell(cameraPosesNum, 1); cameraPosesCov = cell(cameraPosesNum, 1); +marginals = Values; for i = 1:cameraPosesNum cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); @@ -52,25 +54,38 @@ for i = 1:cameraPosesNum for j = 1:measurementNum index = pts3d{i}.overallIdx{j}; points3d{index}.Z{end+1} = pts3d{i}.Z{j}; - points3d{index}.cameraConstraint{end+1} = i; - points3d{index}.visiblity = true; + points3d{index}.camConstraintIdx{end+1} = i; + points3d{index}.added{end+1} = false; - graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', index), K) ); + if length(points3d{index}.Z) < 2 + continue; + else + for k = 1:length(points3d{index}.Z) + if ~points3d{index}.added{k} + graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ... + measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ... + symbol('p', index), K) ); + points3d{index}.added{k} = true; + end + end + end + + points3d{index}.visiblity = true; end pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); marginals = Marginals(graph, initialEstimate); - + for j = 1:pointsNum if points3d{j}.visiblity points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j)); end end - + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i)); + end %% Print the graph From cf26dec49ef471576ede3d5fb4a298b0debf5991 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 20:25:01 -0500 Subject: [PATCH 82/84] add noise to incremental poses. more realistic --- matlab/gtsam_examples/CameraFlyingExample.m | 31 +++++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 1d779b135..3538f38b6 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -27,7 +27,7 @@ options.cylinder.cylinderNum = 15; % pls be smaller than 20 options.cylinder.radius = 3; % pls be smaller than 5 options.cylinder.height = 10; % point density on cylinder -options.cylinder.pointDensity = 0.05; +options.cylinder.pointDensity = 0.1; %% camera options % parameters set according to the stereo camera: @@ -38,7 +38,7 @@ options.camera.IS_MONO = false; % the field of view of camera options.camera.fov = 120; % fps for image -options.camera.fps = 10; +options.camera.fps = 25; % camera pixel resolution options.camera.resolution = Point2(752, 480); % camera horizon @@ -65,8 +65,8 @@ options.writeVideo = true; options.fieldSize = Point2([100, 100]'); % camera flying speed (unit: meter / second) options.speed = 20; -% number of camera poses in the simulated trajectory -options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps); +% camera flying height +options.height = 30; %% ploting options % display covariance scaling factor @@ -135,15 +135,28 @@ end %% generate ground truth camera trajectories: a line KMono = Cal3_S2(525,525,0,320,240); -cameraPoses = cell(options.poseNum, 1); +cameraPoses = cell(0); theta = 0; -for i = 1:options.poseNum - t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... - 15, 10]'); +t = Point3(5, 5, options.height); +i = 0; +while 1 + i = i+1; + distance = options.speed / options.camera.fps; + angle = 0.1*pi*(rand-0.5); + inc_t = Point3(distance * cos(angle), ... + distance * sin(angle), 0); + t = t.compose(inc_t); + + if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10; + break; + end + + %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... + % 15, 10]'); camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.camera.monoK); - cameraPoses{i} = camera.pose; + cameraPoses{end+1} = camera.pose; end %% set up camera and get measurements From 0f100e8228a347352e4487b0d89725e94f9a9ab2 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 20:39:02 -0500 Subject: [PATCH 83/84] add the scale of visualization for covariance matrix --- matlab/+gtsam/covarianceEllipse.m | 4 +++- matlab/+gtsam/covarianceEllipse3D.m | 6 ++++- matlab/+gtsam/plotFlyingResults.m | 26 +++++++++++---------- matlab/gtsam_examples/CameraFlyingExample.m | 5 ++-- 4 files changed, 25 insertions(+), 16 deletions(-) diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m index cdc239bb2..829487dc6 100644 --- a/matlab/+gtsam/covarianceEllipse.m +++ b/matlab/+gtsam/covarianceEllipse.m @@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k) % it is assumed x and y are the first two components of state x % k is scaling for std deviations, defaults to 1 std +hold on + [e,s] = eig(P(1:2,1:2)); s1 = s(1,1); s2 = s(2,2); @@ -32,4 +34,4 @@ h = line(ex,ey,'color',color); y = c(2) + points(2,:); end -end \ No newline at end of file +end diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index c99b19117..4364e0fe4 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -1,4 +1,4 @@ -function sc = covarianceEllipse3D(c,P) +function sc = covarianceEllipse3D(c,P,scale) % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability @@ -12,6 +12,10 @@ hold on k = 11.82; radii = k*sqrt(diag(s)); +if exist('scale', 'var') + radii = radii * scale; +end + % generate data for "unrotated" ellipsoid [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 85042f5a8..5d4a287c4 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -13,11 +13,13 @@ set(gcf, 'Position', [80,1,1800,1000]); %% plot all the cylinders and sampled points axis equal -axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 30]); +axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]); xlabel('X (m)'); ylabel('Y (m)'); zlabel('Height (m)'); +h = cameratoolbar('Show'); + if options.camera.IS_MONO h_title = title('Quadrotor Flight Simulation with Monocular Camera'); else @@ -91,7 +93,7 @@ for i = 1:posesSize pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); + h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale); if exist('h_point', 'var') for j = 1:pointSize @@ -110,13 +112,14 @@ for i = 1:posesSize if ~isempty(pts3d{j}.cov{i}) hold on h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z); - h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], pts3d{j}.cov{i}); + h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ... + pts3d{j}.cov{i}, options.plot.covarianceScale); end - end + end axis equal - axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]); + drawnow if options.writeVideo @@ -149,11 +152,15 @@ end %% camera flying through video +camzoom(0.8); for i = 1 : posesSize + + hold on + campos([poses{i}.x, poses{i}.y, poses{i}.z]); camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]); camlight(hlight, 'headlight'); - + if options.writeVideo currFrame = getframe(gcf); writeVideo(videoObj, currFrame); @@ -162,11 +169,6 @@ for i = 1 : posesSize drawnow end - -if ~holdstate - hold off -end - %%close video if(options.writeVideo) close(videoObj); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 3538f38b6..36b7635e2 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -69,8 +69,9 @@ options.speed = 20; options.height = 30; %% ploting options -% display covariance scaling factor -options.plot.scale = 1; +% display covariance scaling factor, default to be 1. +% The covariance visualization default models 99% of all probablity +options.plot.covarianceScale = 1; % plot the trajectory covariance options.plot.DISP_TRAJ_COV = true; % plot points covariance From b1cea2bee7880bd2e6313e366f9c54a851953604 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 23 Jan 2015 14:29:25 -0500 Subject: [PATCH 84/84] overlooked Dim -> dimension because this factor has no unit test! --- gtsam_unstable/slam/PartialPriorFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 8d6fcc33b..9cd9479e1 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -78,7 +78,7 @@ namespace gtsam { /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(Key key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { + Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); this->fillH();