Merge pull request #1407 from borglab/feature/deprecate_boost

examples/Pose3Localization.cpp

@@ -43,9 +43,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
@@ -74,10 +74,8 @@ int main(const int argc, const char* argv[]) {
 
   // Calculate and print marginal covariances for all variables
   Marginals marginals(*graph, result);
-  for (const auto key_value : result) {
-    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
-    if (!p) continue;
-    std::cout << marginals.marginalCovariance(key_value.key) << endl;
+  for (const auto& key_pose : result.extract<Pose3>()) {
+    std::cout << marginals.marginalCovariance(key_pose.first) << endl;
   }
   return 0;
 }

examples/Pose3SLAMExample_changeKeys.cpp

@@ -55,14 +55,14 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
     // Additional: rewrite input with simplified keys 0,1,...
     Values simpleInitial;
-    for(const auto key_value: *initial) {
+    for (const auto k : initial->keys()) {
       Key key;
-      if(add)
-        key = key_value.key + firstKey;
+      if (add)
+        key = k + firstKey;
       else
-        key = key_value.key - firstKey;
+        key = k - firstKey;
 
-      simpleInitial.insert(key, initial->at(key_value.key));
+      simpleInitial.insert(key, initial->at(k));
     }
     NonlinearFactorGraph simpleGraph;
     for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {

examples/Pose3SLAMExample_g2o.cpp

@@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }

examples/SolverComparer.cpp

@@ -559,12 +559,12 @@ void runPerturb()
 
   // Perturb values
   VectorValues noise;
-  for(const Values::KeyValuePair key_val: initial)
+  for(const auto& key_dim: initial.dims())
   {
-    Vector noisev(key_val.value.dim());
+    Vector noisev(key_dim.second);
     for(Vector::Index i = 0; i < noisev.size(); ++i)
       noisev(i) = normal(rng);
-    noise.insert(key_val.key, noisev);
+    noise.insert(key_dim.first, noisev);
   }
   Values perturbed = initial.retract(noise);
 

gtsam/base/SymmetricBlockMatrix.cpp

@@ -24,6 +24,12 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
+SymmetricBlockMatrix::SymmetricBlockMatrix() : blockStart_(0) {
+  variableColOffsets_.push_back(0);
+  assertInvariants();
+}
+
 /* ************************************************************************* */
 SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
     const SymmetricBlockMatrix& other) {
@@ -61,6 +67,18 @@ Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const {
   }
 }
 
+/* ************************************************************************* */
+void SymmetricBlockMatrix::negate() {
+  full().triangularView<Eigen::Upper>() *= -1.0;
+}
+
+/* ************************************************************************* */
+void SymmetricBlockMatrix::invertInPlace() {
+  const auto identity = Matrix::Identity(rows(), rows());
+  full().triangularView<Eigen::Upper>() =
+      selfadjointView().llt().solve(identity).triangularView<Eigen::Upper>();
+}
+
 /* ************************************************************************* */
 void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) {
   gttic(VerticalBlockMatrix_choleskyPartial);

gtsam/base/SymmetricBlockMatrix.h

@@ -63,12 +63,7 @@ namespace gtsam {
 
   public:
     /// Construct from an empty matrix (asserts that the matrix is empty)
-    SymmetricBlockMatrix() :
-      blockStart_(0)
-    {
-      variableColOffsets_.push_back(0);
-      assertInvariants();
-    }
+    SymmetricBlockMatrix();
 
     /// Construct from a container of the sizes of each block.
     template<typename CONTAINER>
@@ -265,19 +260,10 @@ namespace gtsam {
     }
 
     /// Negate the entire active matrix.
-    void negate() {
-      full().triangularView<Eigen::Upper>() *= -1.0;
-    }
+    void negate();
 
     /// Invert the entire active matrix in place.
-    void invertInPlace() {
-      const auto identity = Matrix::Identity(rows(), rows());
-      full().triangularView<Eigen::Upper>() =
-          selfadjointView()
-              .llt()
-              .solve(identity)
-              .triangularView<Eigen::Upper>();
-    }
+    void invertInPlace();
 
     /// @}
 

gtsam/geometry/Point2.h

@@ -45,7 +45,7 @@ typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
 
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {
-  return p * s;
+  return Point2(s * p.x(), s * p.y());
 }
 
 /*

gtsam/geometry/Pose3.cpp

@@ -158,6 +158,12 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
   return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
 }
 
+/* ************************************************************************* */
+Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
+  return Pose3(interpolate<Rot3>(R_, T.R_, t),
+                interpolate<Point3>(t_, T.t_, t));
+}
+
 /* ************************************************************************* */
 /** Modified from Murray94book version (which assumes w and v normalized?) */
 Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {

gtsam/geometry/Pose3.h

@@ -129,10 +129,7 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
    * @param T End point of interpolation.
    * @param t A value in [0, 1].
    */
-  Pose3 interpolateRt(const Pose3& T, double t) const {
-    return Pose3(interpolate<Rot3>(R_, T.R_, t),
-                 interpolate<Point3>(t_, T.t_, t));
-  }
+  Pose3 interpolateRt(const Pose3& T, double t) const;
 
   /// @}
   /// @name Lie Group

gtsam/geometry/Unit3.cpp

@@ -32,6 +32,14 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+Unit3::Unit3(const Vector3& p) : p_(p.normalized()) {}
+
+Unit3::Unit3(double x, double y, double z) : p_(x, y, z) { p_.normalize(); }
+
+Unit3::Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
+  p_.normalize();
+}
 /* ************************************************************************* */
 Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
   // 3*3 Derivative of representation with respect to point is 3*3:

gtsam/geometry/Unit3.h

@@ -67,21 +67,14 @@ class GTSAM_EXPORT Unit3 {
   }
 
   /// Construct from point
-  explicit Unit3(const Vector3& p) :
-      p_(p.normalized()) {
-  }
+  explicit Unit3(const Vector3& p);
 
   /// Construct from x,y,z
-  Unit3(double x, double y, double z) :
-      p_(x, y, z) {
-    p_.normalize();
-  }
+  Unit3(double x, double y, double z);
 
   /// Construct from 2D point in plane at focal length f
   /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
-  explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
-    p_.normalize();
-  }
+  explicit Unit3(const Point2& p, double f);
 
   /// Copy constructor
   Unit3(const Unit3& u) {

gtsam/hybrid/HybridValues.h

@@ -102,7 +102,7 @@ class GTSAM_EXPORT HybridValues {
 
   /// Check whether a variable with key \c j exists in values.
   bool existsNonlinear(Key j) {
-    return (nonlinear_.find(j) != nonlinear_.end());
+    return nonlinear_.exists(j);
   };
 
   /// Check whether a variable with key \c j exists.

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -129,10 +129,7 @@ TEST(HybridBayesNet, evaluateHybrid) {
 TEST(HybridBayesNet, Choose) {
   Switching s(4);
 
-  Ordering ordering;
-  for (auto&& kvp : s.linearizationPoint) {
-    ordering += kvp.key;
-  }
+  const Ordering ordering(s.linearizationPoint.keys());
 
   HybridBayesNet::shared_ptr hybridBayesNet;
   HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
@@ -163,10 +160,7 @@ TEST(HybridBayesNet, Choose) {
 TEST(HybridBayesNet, OptimizeAssignment) {
   Switching s(4);
 
-  Ordering ordering;
-  for (auto&& kvp : s.linearizationPoint) {
-    ordering += kvp.key;
-  }
+  const Ordering ordering(s.linearizationPoint.keys());
 
   HybridBayesNet::shared_ptr hybridBayesNet;
   HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;

gtsam/linear/LossFunctions.cpp

@@ -40,6 +40,10 @@ Vector Base::weight(const Vector& error) const {
   return w;
 }
 
+Vector Base::sqrtWeight(const Vector &error) const {
+  return weight(error).cwiseSqrt();
+}
+
 // The following three functions re-weight block matrices and a vector
 // according to their weight implementation
 

gtsam/linear/LossFunctions.h

@@ -115,9 +115,7 @@ class GTSAM_EXPORT Base {
   Vector weight(const Vector &error) const;
 
   /** square root version of the weight function */
-  Vector sqrtWeight(const Vector &error) const {
-    return weight(error).cwiseSqrt();
-  }
+  Vector sqrtWeight(const Vector &error) const;
 
   /** reweight block matrices and a vector according to their weight
    * implementation */

gtsam/linear/NoiseModel.cpp

@@ -239,6 +239,8 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
   whitenInPlace(b);
 }
 
+Matrix Gaussian::information() const { return R().transpose() * R(); }
+
 /* ************************************************************************* */
 // Diagonal
 /* ************************************************************************* */
@@ -284,6 +286,11 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
   full: return Diagonal::shared_ptr(new Diagonal(sigmas));
 }
 
+/* ************************************************************************* */
+Diagonal::shared_ptr Diagonal::Precisions(const Vector& precisions,
+                                          bool smart) {
+  return Variances(precisions.array().inverse(), smart);
+}
 /* ************************************************************************* */
 void Diagonal::print(const string& name) const {
   gtsam::print(sigmas_, name + "diagonal sigmas ");
@@ -294,22 +301,18 @@ Vector Diagonal::whiten(const Vector& v) const {
   return v.cwiseProduct(invsigmas_);
 }
 
-/* ************************************************************************* */
 Vector Diagonal::unwhiten(const Vector& v) const {
   return v.cwiseProduct(sigmas_);
 }
 
-/* ************************************************************************* */
 Matrix Diagonal::Whiten(const Matrix& H) const {
   return vector_scale(invsigmas(), H);
 }
 
-/* ************************************************************************* */
 void Diagonal::WhitenInPlace(Matrix& H) const {
   vector_scale_inplace(invsigmas(), H);
 }
 
-/* ************************************************************************* */
 void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
   H = invsigmas().asDiagonal() * H;
 }
@@ -377,6 +380,32 @@ Vector Constrained::whiten(const Vector& v) const {
   return c;
 }
 
+/* ************************************************************************* */
+Constrained::shared_ptr Constrained::MixedSigmas(const Vector& sigmas) {
+  return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
+}
+
+Constrained::shared_ptr Constrained::MixedSigmas(double m,
+                                                 const Vector& sigmas) {
+  return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
+}
+
+Constrained::shared_ptr Constrained::MixedVariances(const Vector& mu,
+                                                    const Vector& variances) {
+  return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
+}
+Constrained::shared_ptr Constrained::MixedVariances(const Vector& variances) {
+  return shared_ptr(new Constrained(variances.cwiseSqrt()));
+}
+
+Constrained::shared_ptr Constrained::MixedPrecisions(const Vector& mu,
+                                                     const Vector& precisions) {
+  return MixedVariances(mu, precisions.array().inverse());
+}
+Constrained::shared_ptr Constrained::MixedPrecisions(const Vector& precisions) {
+  return MixedVariances(precisions.array().inverse());
+}
+
 /* ************************************************************************* */
 double Constrained::squaredMahalanobisDistance(const Vector& v) const {
   Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
@@ -623,6 +652,11 @@ void Unit::print(const std::string& name) const {
   cout << name << "unit (" << dim_ << ") " << endl;
 }
 
+/* ************************************************************************* */
+double Unit::squaredMahalanobisDistance(const Vector& v) const {
+  return v.dot(v);
+}
+
 /* ************************************************************************* */
 // Robust
 /* ************************************************************************* */
@@ -663,6 +697,13 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
   robust_->reweight(A1,A2,A3,b);
 }
 
+Vector Robust::unweightedWhiten(const Vector& v) const {
+  return noise_->unweightedWhiten(v);
+}
+double Robust::weight(const Vector& v) const {
+  return robust_->weight(v.norm());
+}
+
 Robust::shared_ptr Robust::Create(
 const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
   return shared_ptr(new Robust(robust,noise));