Add test for unscented transform

g2o/examples/g2o_hierarchical/CMakeLists.txt

@@ -5,7 +5,6 @@ add_library(g2o_hierarchical_library ${G2O_LIB_TYPE}
   edge_types_cost_function.cpp
   backbone_tree_action.cpp
   simple_star_ops.cpp
-  g2o_hierarchical_test_functions.cpp
   edge_labeler.h
   edge_creator.h
   star.h

g2o/stuff/unscented.h

@@ -51,10 +51,10 @@ bool sampleUnscented(std::vector<SigmaPoint<SampleType>>& sigmaPoints,
   const int numPoints = 2 * dim + 1;
   assert(covariance.rows() == covariance.cols() &&
          covariance.cols() == mean.size() && "Dimension Mismatch");
-  const double alpha = cst(1e-3);
-  const double beta = 2;
+  constexpr double alpha = 1e-3;
+  constexpr double beta = 2.;
   const double lambda = alpha * alpha * dim;
-  const double wi = cst(1) / (2 * (dim + lambda));
+  const double wi = 1. / (2. * (dim + lambda));
 
   sigmaPoints.resize(numPoints);
   sigmaPoints[0] = SigmaPoint<SampleType>(

unit_test/stuff/CMakeLists.txt

@@ -5,6 +5,7 @@ add_executable(unittest_stuff
   property_tests.cpp
   string_tools_tests.cpp
   tuple_tools_tests.cpp
+  unscented_tests.cpp
 )
 target_link_libraries(unittest_stuff stuff)
 create_test(unittest_stuff)

g2o/examples/g2o_hierarchical/g2o_hierarchical_test_functions.cpp → unit_test/stuff/unscented_tests.cpp

@@ -1,5 +1,5 @@
 // g2o - General Graph Optimization
-// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
 // All rights reserved.
 //
 // Redistribution and use in source and binary forms, with or without
@@ -24,43 +24,43 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
-#include <signal.h>
+#include <gmock/gmock.h>
 
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include <algorithm>
-#include <cassert>
-#include <fstream>
-#include <iomanip>
-#include <iostream>
-#include <sstream>
-#include <string>
-
-#include "edge_creator.h"
-#include "edge_labeler.h"
-#include "g2o/apps/g2o_cli/dl_wrapper.h"
-#include "g2o/apps/g2o_cli/g2o_common.h"
-#include "g2o/apps/g2o_cli/output_helper.h"
-#include "g2o/core/estimate_propagator.h"
-#include "g2o/core/factory.h"
-#include "g2o/core/hyper_dijkstra.h"
-#include "g2o/core/optimization_algorithm_factory.h"
-#include "g2o/core/sparse_optimizer.h"
-#include "g2o/stuff/color_macros.h"
-#include "g2o/stuff/command_args.h"
-#include "g2o/stuff/filesys_tools.h"
-#include "g2o/stuff/macros.h"
-#include "g2o/stuff/string_tools.h"
-#include "g2o/stuff/timeutil.h"
+#include "g2o/core/eigen_types.h"
 #include "g2o/stuff/unscented.h"
-#include "star.h"
+#include "gmock/gmock.h"
+#include "unit_test/test_helper/eigen_matcher.h"
+
+namespace {
+using MySigmaPoint = g2o::SigmaPoint<g2o::VectorX>;
+}
 
-using namespace std;
-using namespace g2o;
-using namespace Eigen;
+TEST(Unscented, SampleUnscented) {
+  constexpr int kDim = 6;
 
-typedef SigmaPoint<VectorXd> MySigmaPoint;
+  g2o::MatrixX covariance = g2o::MatrixX::Zero(kDim, kDim);
+  for (int i = 0; i < kDim; i++) {
+    for (int j = i; j < kDim; j++) {
+      covariance(i, j) = covariance(j, i) = i * j + 1;
+    }
+  }
+  covariance += g2o::MatrixX::Identity(kDim, kDim);
+  g2o::VectorX mean = g2o::VectorX::Ones(kDim);
 
+  std::vector<MySigmaPoint> spts;
+  sampleUnscented(spts, mean, covariance);
+  EXPECT_THAT(spts, testing::SizeIs(2 * kDim + 1));
+
+  g2o::VectorX rec_mean(kDim);
+  g2o::MatrixX rec_covariance(kDim, kDim);
+  reconstructGaussian(rec_mean, rec_covariance, spts);
+
+  EXPECT_THAT(print_wrap(mean), EigenApproxEqual(print_wrap(rec_mean), 1e-6));
+  EXPECT_THAT(print_wrap(covariance),
+              EigenApproxEqual(print_wrap(rec_covariance), 1e-6));
+}
+
+#if 0
 void testMarginals(SparseOptimizer& optimizer) {
   cerr << "Projecting marginals" << endl;
   std::vector<std::pair<int, int> > blockIndices;
@@ -130,37 +130,4 @@ void testMarginals(SparseOptimizer& optimizer) {
     }
   }
 }
-
-int unscentedTest() {
-  MatrixXd m = MatrixXd(6, 6);
-  for (int i = 0; i < 6; i++) {
-    for (int j = i; j < 6; j++) {
-      m(i, j) = m(j, i) = i * j + 1;
-    }
-  }
-  m += MatrixXd::Identity(6, 6);
-  cerr << m;
-  VectorXd mean(6);
-  mean.fill(1);
-
-  std::vector<MySigmaPoint> spts;
-  sampleUnscented(spts, mean, m);
-  for (size_t i = 0; i < spts.size(); i++) {
-    cerr << "Point " << i << " " << endl
-         << "wi=" << spts[i]._wi << " wp=" << spts[i]._wp << " " << endl;
-    cerr << spts[i]._sample << endl;
-  }
-
-  VectorXd recMean(6);
-  MatrixXd recCov(6, 6);
-
-  reconstructGaussian(recMean, recCov, spts);
-
-  cerr << "recMean" << endl;
-  cerr << recMean << endl;
-
-  cerr << "recCov" << endl;
-  cerr << recCov << endl;
-
-  return 0;
-}
+#endif

unit_test/test_helper/eigen_matcher.h

@@ -0,0 +1,59 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <gmock/gmock.h>
+
+#include <Eigen/Core>
+
+MATCHER_P(EigenEqual, expect,
+          std::string(negation ? "isn't" : "is") + " equal to" +
+              ::testing::PrintToString(expect)) {
+  return arg == expect;
+}
+
+MATCHER_P2(EigenApproxEqual, expect, prec,
+           std::string(negation ? "isn't" : "is") + " approx equal to" +
+               ::testing::PrintToString(expect) + "\nwith precision " +
+               ::testing::PrintToString(prec)) {
+  return arg.isApprox(expect, prec);
+}
+
+MATCHER_P2(EigenMaxDiff, expect, max_diff,
+           std::string(negation ? "doesn't" : "does") + " differ from " +
+               ::testing::PrintToString(expect) + "\nmore than " +
+               ::testing::PrintToString(max_diff)) {
+  return (arg - expect).array().abs().maxCoeff() <= max_diff;
+}
+
+template <class Base>
+class EigenPrintWrap : public Base {
+  void PrintTo(const EigenPrintWrap& m, ::std::ostream* o) { *o << "\n" << m; }
+};
+
+template <class Base>
+const EigenPrintWrap<Base>& print_wrap(const Base& base) {
+  return static_cast<const EigenPrintWrap<Base>&>(base);
+}