Update tests for factors and noise

Also:
- Noise: pre-calculate inverse covariance, add docs

wave_optimization/include/wave/optimization/factor_graph/Noise.hpp

@@ -24,21 +24,29 @@ struct traits {};
  */
 template <int S>
 class FullNoise {
+    using SquareMat = Eigen::Matrix<double, S, S>;
+
  public:
-    explicit FullNoise(Eigen::Matrix<double, S, S> cov) : covariance_mat{cov} {}
+    /** The type accepted by the constructor */
+    using InitType = SquareMat;
+
+    /** Construct with the given covariance matrix */
+    explicit FullNoise(InitType cov)
+        : covariance_mat{cov},
+          // Pre-calculate inverse sqrt covariance, used in normalization
+          inverse_sqrt_cov{SquareMat{cov.llt().matrixL()}.inverse()} {}
 
     Eigen::Matrix<double, S, S> inverseSqrtCov() const {
-        const auto L =
-          Eigen::Matrix<double, S, S>{this->covariance_mat.llt().matrixL()};
-        return L.inverse();
+        return this->inverse_sqrt_cov;
     };
 
     Eigen::Matrix<double, S, S> covariance() const {
         return this->covariance_mat;
     };
 
  private:
-    Eigen::Matrix<double, S, S> covariance_mat;
+    const SquareMat covariance_mat;
+    const SquareMat inverse_sqrt_cov;
 };
 
 /**
@@ -47,25 +55,29 @@ class FullNoise {
  */
 template <int S>
 class DiagonalNoise {
+    using DiagonalMat = Eigen::DiagonalMatrix<double, S>;
+
  public:
-    // Decide which type must be passed in the constructor
-    // Normally it's an Eigen vector, but for size-one values accept a double.
+    /** The type accepted by the constructor */
     using InitType = Eigen::Matrix<double, S, 1>;
 
-    explicit DiagonalNoise(const InitType &sigma)
-        : covariance_mat{sigma.cwiseProduct(sigma)} {}
+    /** Construct with the given vector of standard devations (sigmas) */
+    explicit DiagonalNoise(const InitType &stddev)
+        : covariance_mat{stddev.cwiseProduct(stddev)},
+          // Pre-calculate inverse sqrt covariance, used in normalization
+          inverse_sqrt_cov{stddev.cwiseInverse()} {}
 
-    Eigen::DiagonalMatrix<double, S> inverseSqrtCov() const {
-        return Eigen::DiagonalMatrix<double, S>{
-          this->covariance_mat.diagonal().cwiseSqrt().cwiseInverse()};
+    DiagonalMat covariance() const {
+        return this->covariance_mat;
     };
 
-    Eigen::DiagonalMatrix<double, S> covariance() const {
-        return this->covariance_mat;
+    DiagonalMat inverseSqrtCov() const {
+        return this->inverse_sqrt_cov;
     };
 
  private:
-    Eigen::DiagonalMatrix<double, S> covariance_mat;
+    const DiagonalMat covariance_mat;
+    const DiagonalMat inverse_sqrt_cov;
 };
 
 /**
@@ -74,18 +86,20 @@ class DiagonalNoise {
 template <>
 class DiagonalNoise<1> {
  public:
+    /** The type accepted by the constructor */
     using InitType = double;
 
+    /** Construct with the given standard devations (sigma) */
     explicit DiagonalNoise<1>(double stddev) : stddev{stddev} {}
 
-    double inverseSqrtCov() const {
-        return 1. / this->stddev;
-    }
-
     double covariance() const {
         return this->stddev * this->stddev;
     };
 
+    double inverseSqrtCov() const {
+        return 1. / this->stddev;
+    }
+
  private:
     double stddev;
 };

wave_optimization/include/wave/optimization/factor_graph/PerfectPrior.hpp

@@ -79,7 +79,11 @@ class PerfectPrior : public FactorBase {
     }
 
     /** Print a representation for debugging. Used by operator<< */
-    void print(std::ostream &os) const override {}
+    void print(std::ostream &os) const override {
+        os << "[Perfect prior for variable ";
+        const auto &v = this->variable_ptrs.front();
+        os << *v << "(" << v << ")]";
+    }
 
  private:
     /** Storage of the measurement */

wave_optimization/tests/factor_graph/factor_test.cpp

@@ -43,6 +43,63 @@ TEST(FactorTest, evaluate) {
       VectorsNear, expected_jac_landmark, Eigen::Map<Vec2>{out_jac_landmark});
 }
 
+
+TEST(FactorTest, evaluateSize1) {
+    // Demonstrate construction of a simple unary factor with size-1 measurement
+    // This also shows that the measurement function can be a lambda
+    auto func = [](
+      const double &v, ResultOut<1> result, JacobianOut<1, 1> jac) -> bool {
+        result = v * 2;
+        jac(0, 0) = -1.1;
+        return true;
+    };
+    const auto meas_val = 1.2;
+    const auto meas_stddev = 0.1;
+    auto meas = FactorMeasurement<double>{meas_val, meas_stddev};
+    auto var = std::make_shared<FactorVariable<double>>();
+    auto factor = Factor<FactorMeasurement<double>, FactorVariable<double>>{
+      func, meas, var};
+    EXPECT_EQ(1, factor.size());
+    EXPECT_EQ(1, factor.residualSize());
+    ASSERT_EQ(1u, factor.variables().size());
+    EXPECT_EQ(var, factor.variables().front());
+    EXPECT_FALSE(factor.isPerfectPrior());
+
+
+    // Prepare sample C-style arrays as used by Ceres
+    double test_residual;
+    double test_jac;
+    double test_val = 1.09;
+    const double *const params[] = {&test_val};
+    double *jacs[] = {&test_jac};
+
+    // evaluate the factor
+    EXPECT_TRUE(factor.evaluateRaw(params, &test_residual, jacs));
+
+    // Compare the result. We expect the residual to be L(f(X) - Z)
+    // In this case that is (2x - Z)/stddev
+    EXPECT_DOUBLE_EQ((test_val * 2 - meas_val) / meas_stddev, test_residual);
+    EXPECT_DOUBLE_EQ(-1.1, test_jac);
+}
+
+
+TEST(FactorTest, constructPerfectPrior) {
+    // Test that using a perfect prior immediately sets the variable's value
+    // @todo this may change
+
+    // Note explicitly constructing PerfectPrior is not intended for users -
+    // They should use FactorGraph::addPerfectPrior. That is why this test does
+    // some non-intuitive preparation (e.g. constructing a FactorMeasurement)
+    using MeasType = FactorMeasurement<double, void>;
+    using VarType = FactorVariable<double>;
+    auto v = std::make_shared<VarType>();
+
+    auto factor = PerfectPrior<VarType>{MeasType{1.2}, v};
+    EXPECT_DOUBLE_EQ(1.2, v->value);
+
+    EXPECT_TRUE(factor.isPerfectPrior());
+}
+
 TEST(FactorTest, print) {
     auto v1 = std::make_shared<Pose2DVar>();
     auto v2 = std::make_shared<Landmark2DVar>();
@@ -59,19 +116,19 @@ TEST(FactorTest, print) {
     EXPECT_EQ(expected.str(), ss.str());
 }
 
-TEST(FactorTest, perfectPrior) {
-    // Test that using a perfect prior immediately sets the variable's value
-    // Note explicitly constructing PerfectPrior is not intended for users -
-    // They should use FactorGraph::addPerfectPrior. That is why this test does
-    // some non-intuitive preparation (e.g. constructing a FactorMeasurement)
+TEST(FactorTest, printPerfectPrior) {
     using MeasType = FactorMeasurement<double, void>;
     using VarType = FactorVariable<double>;
-    const auto &func = internal::identityMeasurementFunction<double>;
-    using FactorType = PerfectPrior<VarType>;
     auto v = std::make_shared<VarType>();
+    auto factor = PerfectPrior<VarType>{MeasType{1.2}, v};
 
-    PerfectPrior<VarType>{MeasType{1.2}, v};
-    EXPECT_DOUBLE_EQ(1.2, v->value);
+    std::stringstream expected;
+    expected << "[Perfect prior for variable ";
+    expected << *v << "(" << v << ")]";
+
+    std::stringstream ss;
+    ss << factor;
+    EXPECT_EQ(expected.str(), ss.str());
 }
 
 }  // namespace wave

wave_optimization/tests/factor_graph/graph_test.cpp

@@ -110,18 +110,51 @@ TEST(FactorGraph, addPerfectPrior) {
     graph.addPerfectPrior(test_meas, p);
     EXPECT_EQ(1u, graph.countFactors());
 
-    // The variable should now be marked constant
-    //    EXPECT_TRUE(p->isFixed());
+    // The variable should have the measured value immediately
+    // @todo this may change
+    EXPECT_PRED2(VectorsNear, test_meas, Eigen::Map<Vec2>(p->data()));
 
     // Retrieve a pointer to the factor we just (indirectly) added
     auto factor = *graph.begin();
     ASSERT_NE(nullptr, factor);
 
+    EXPECT_TRUE(factor->isPerfectPrior());
+
     // We cannot evaluate a factor that is a perfect prior
-    // Since evaluateRaw is nothrow, the program will die
     EXPECT_FALSE(factor->evaluateRaw(params, test_residual.data(), jacs));
 }
 
+TEST(FactorGraph, addPerfectPriorOfSize1) {
+    const auto test_meas = 1.2;
+    const auto test_val = 1.23;
+
+    // Prepare arguments to add unary factor
+    FactorGraph graph;
+    auto p = std::make_shared<FactorVariable<double>>();
+
+    // Prepare arguments in a form matching ceres calls
+    double test_residual;
+    double test_jac;
+    const double *const params[] = {&test_val};
+    double *jacs[] = {&test_jac};
+
+    // Add the factor
+    graph.addPerfectPrior(test_meas, p);
+    EXPECT_EQ(1u, graph.countFactors());
+
+    // The variable should have the measured value immediately
+    // @todo this may change
+    EXPECT_DOUBLE_EQ(test_meas, *p->data());
+
+    auto factor = *graph.begin();
+    ASSERT_NE(nullptr, factor);
+
+    EXPECT_TRUE(factor->isPerfectPrior());
+
+    // We cannot evaluate a factor that is a perfect prior
+    EXPECT_FALSE(factor->evaluateRaw(params, &test_residual, jacs));
+}
+
 TEST(FactorGraph, triangulationSim) {
     // Simulate a robot passing by a landmark
     // First generate "true" data

wave_optimization/tests/factor_graph/noise_test.cpp

@@ -4,7 +4,7 @@
 namespace wave {
 
 TEST(NoiseTest, diagonalNoise) {
-    const auto stddev = Vec2{1.1, 2.2};
+    const DiagonalNoise<2>::InitType stddev = Vec2{1.1, 2.2};
     Eigen::Matrix2d expected_cov, expected_inv;
     expected_cov << 1.1 * 1.1, 0, 0, 2.2 * 2.2;
     expected_inv << 1 / 1.1, 0, 0, 1 / 2.2;
@@ -19,15 +19,15 @@ TEST(NoiseTest, diagonalNoise) {
 }
 
 TEST(NoiseTest, singleNoise) {
-    const auto stddev = 1.1;
+    const DiagonalNoise<1>::InitType stddev = 1.1;
 
     auto n = DiagonalNoise<1>{stddev};
     EXPECT_DOUBLE_EQ(stddev * stddev, n.covariance());
     EXPECT_DOUBLE_EQ(1. / stddev, n.inverseSqrtCov());
 }
 
 TEST(NoiseTest, fullNoise) {
-    auto cov = Mat2{};
+    FullNoise<2>::InitType cov = Mat2{};
     cov << 3.3, 1.1, 1.1, 4.4;
 
     auto n = FullNoise<2>{cov};