Separate out PerfectPrior.hpp

Also:
- Fix immediate assignment of variable values

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

@@ -6,14 +6,10 @@
 #ifndef WAVE_OPTIMIZATION_FACTOR_GRAPH_FACTOR_HPP
 #define WAVE_OPTIMIZATION_FACTOR_GRAPH_FACTOR_HPP
 
-#include <ceres/sized_cost_function.h>
-#include <memory>
-
 #include "wave/utils/math.hpp"
 #include "wave/optimization/factor_graph/FactorVariableBase.hpp"
 #include "wave/optimization/factor_graph/FactorBase.hpp"
 #include "wave/optimization/factor_graph/OutputMap.hpp"
-#include "wave/optimization/factor_graph/FactorMeasurement.hpp"
 
 
 namespace wave {
@@ -86,8 +82,6 @@ class Factor : public FactorBase {
                     MeasType measurement,
                     std::shared_ptr<VarTypes>... variable_ptrs);
 
-    ~Factor() override = default;
-
     int size() const override {
         return NumVars;
     }
@@ -123,66 +117,9 @@ class Factor : public FactorBase {
     VarArrayType variable_ptrs;
 };
 
-template <typename VarType>
-class PerfectPrior : public FactorBase {
-    using FactorType = PerfectPrior<VarType>;
-    using ViewType = typename VarType::ViewType;
-    using MeasType = FactorMeasurement<ViewType, void>;
-
- public:
-    constexpr static int NumVars = 1;
-    constexpr static int ResidualSize = MeasType::Size;
-    using ResidualType = Eigen::Matrix<double, ResidualSize, 1>;
-    using VarArrayType = FactorBase::VarVectorType;
-    using const_iterator = typename VarArrayType::const_iterator;
-
-    /** Construct with the given measurement and variable. */
-    explicit PerfectPrior(MeasType measurement,
-                          std::shared_ptr<VarType> variable_ptr)
-        : measurement{measurement}, variable_ptrs{variable_ptr} {
-        // Assign to the variable
-        variable_ptr->value.asVector() = measurement.value.asVector();
-    }
-
-    ~PerfectPrior() override = default;
-
-    int size() const override {
-        return NumVars;
-    }
-
-    int residualSize() const override {
-        return ResidualSize;
-    }
-
-    bool evaluateRaw(double const *const *, double *, double **) const
-      noexcept override {
-        return false;
-    }
-
-    /** Get a reference to the vector of variable pointers */
-    const VarVectorType &variables() const noexcept override {
-        return this->variable_ptrs;
-    }
-
-    /** Return true if this factor is a zero-noise prior */
-    bool isPerfectPrior() const noexcept override {
-        return true;
-    }
-
-    /** Print a representation for debugging. Used by operator<< */
-    void print(std::ostream &os) const override {}
-
- private:
-    /** Storage of the measurement */
-    MeasType measurement;
-
-    /** Pointers to the variables this factor is linked to */
-    VarArrayType variable_ptrs;
-};
-
 /** @} group optimization */
 }  // namespace wave
 
 #include "impl/Factor.hpp"
 
-#endif  // WAVE_OPTIMIZATION_FACTOR_GRAPH_PERFECT_PRIOR_HPP
+#endif  // WAVE_OPTIMIZATION_FACTOR_GRAPH_FACTOR_HPP

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

@@ -46,7 +46,7 @@ class OutputMap : public Eigen::Map<T> {
     template <typename ViewType,
               typename MappedType = typename ViewType::MappedType>
     OutputMap &operator=(const ViewType &v) {
-        this->Eigen::Map<T>::operator=(Eigen::Map<const MappedType>{v.data()});
+        this->Eigen::Map<T>::operator=(v.asVector());
         return *this;
     }
 

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

@@ -0,0 +1,95 @@
+/**
+ * @file
+ * @ingroup optimization
+ */
+
+#ifndef WAVE_OPTIMIZATION_FACTOR_GRAPH_PERFECT_PRIOR_HPP
+#define WAVE_OPTIMIZATION_FACTOR_GRAPH_PERFECT_PRIOR_HPP
+
+#include "wave/optimization/factor_graph/FactorVariableBase.hpp"
+#include "wave/optimization/factor_graph/FactorBase.hpp"
+#include "wave/optimization/factor_graph/FactorMeasurement.hpp"
+
+
+namespace wave {
+/** @addtogroup optimization
+ *  @{ */
+
+/**
+ * A unary factor representing a noiseless prior
+ *
+ * A prior in a factor graph can be described as
+ *
+ * @f[
+ * Z = f(\theta) + \epsilon
+ * @f]
+ *
+ * where @f$ f @$f is the measurement function,  @f$ \theta @$f is a random
+ * variable and @f$ \epsilon @$f is random noise. @f$ \epsilon @f cannot be
+ * zero in general, as there may not be a solution for @f$ \Theta @$f.
+ *
+ * In libwave, noiseless priors are allowed in the case of a direct
+ * measurement (i.e., @f$ f(\theta) = \theta @$f). These are called perfect
+ * priors. Each FactorVariable may be connected to at most one perfect prior.
+ *
+ * @tparam VarType The type of factor variable we have prior information on
+ */
+template <typename VarType>
+class PerfectPrior : public FactorBase {
+    using FactorType = PerfectPrior<VarType>;
+    using ViewType = typename VarType::ViewType;
+    using MeasType = FactorMeasurement<ViewType, void>;
+
+ public:
+    constexpr static int NumVars = 1;
+    constexpr static int ResidualSize = MeasType::Size;
+    using ResidualType = Eigen::Matrix<double, ResidualSize, 1>;
+    using VarArrayType = FactorBase::VarVectorType;
+    using const_iterator = typename VarArrayType::const_iterator;
+
+    /** Construct with the given measurement and variable. */
+    explicit PerfectPrior(MeasType measurement,
+                          std::shared_ptr<VarType> variable_ptr)
+        : measurement{measurement}, variable_ptrs{variable_ptr} {
+        // Assign to the variable
+        variable_ptr->value = measurement.value;
+    }
+
+    int size() const override {
+        return NumVars;
+    }
+
+    int residualSize() const override {
+        return ResidualSize;
+    }
+
+    bool evaluateRaw(double const *const *, double *, double **) const
+      noexcept override {
+        return false;
+    }
+
+    /** Get a reference to the vector of variable pointers */
+    const VarVectorType &variables() const noexcept override {
+        return this->variable_ptrs;
+    }
+
+    /** Return true if this factor is a zero-noise prior */
+    bool isPerfectPrior() const noexcept override {
+        return true;
+    }
+
+    /** Print a representation for debugging. Used by operator<< */
+    void print(std::ostream &os) const override {}
+
+ private:
+    /** Storage of the measurement */
+    MeasType measurement;
+
+    /** Pointers to the variables this factor is linked to */
+    VarArrayType variable_ptrs;
+};
+
+/** @} group optimization */
+}  // namespace wave
+
+#endif  // WAVE_OPTIMIZATION_FACTOR_GRAPH_PERFECT_PRIOR_HPP

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

@@ -50,6 +50,12 @@ class ValueView {
     /** Construct to map the given mapped Eigen matrix */
     explicit ValueView(MappedType &m) : dataptr{m.data()} {}
 
+    /** Copy the value of another ValueView */
+    ValueView &operator=(const ValueView<S> &other) {
+        this->asVector() = other.asVector();
+        return *this;
+    }
+
     /** Return the size of the underlying value */
     constexpr int size() const noexcept {
         return Size;

wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp

@@ -1,4 +1,5 @@
 #include "wave/optimization/factor_graph/FactorMeasurement.hpp"
+#include "wave/optimization/factor_graph/PerfectPrior.hpp"
 
 namespace wave {
 

wave_optimization/tests/factor_graph/example_instances.hpp

@@ -32,7 +32,10 @@ struct Pose2D : public ValueView<3> {
     // (https://gcc.gnu.org/bugzilla/show_bug.cgi?id=67054)
     explicit Pose2D(double *d) : ValueView<3>{d} {}
     explicit Pose2D(MappedType &m) : ValueView<3>{m} {}
-
+    Pose2D &operator=(const Pose2D &other) {
+        this->ValueView<3>::operator=(other);
+        return *this;
+    }
     using Vec1 = Eigen::Matrix<double, 1, 1>;
 
     Eigen::Map<const Vec2> position{dataptr};
@@ -51,6 +54,10 @@ struct Landmark2D : public ValueView<2> {
     // (https://gcc.gnu.org/bugzilla/show_bug.cgi?id=67054)
     explicit Landmark2D(double *d) : ValueView<2>{d} {}
     explicit Landmark2D(MappedType &m) : ValueView<2>{m} {}
+    Landmark2D &operator=(const Landmark2D &other) {
+        this->ValueView<2>::operator=(other);
+        return *this;
+    }
 
     Eigen::Map<const Vec2> position{dataptr};
 };

wave_optimization/tests/factor_graph/factor_test.cpp

@@ -59,32 +59,19 @@ TEST(FactorTest, print) {
     EXPECT_EQ(expected.str(), ss.str());
 }
 
-// TEST(FactorTest, idealMeasurement) {
-//    using MeasType = FactorMeasurement<double, ZeroNoise>;
-//    using VarType = FactorVariable<double>;
-//    const auto &func = internal::identityMeasurementFunction<double>;
-//    using FactorType = Factor<MeasType, VarType>;
-//    auto v = std::make_shared<VarType>();
-//
-//    // Linking a variable to a factor with ZeroNoise measurement marks it
-//    fixed
-//    FactorType{func, MeasType{1.2}, v};
-//
-//    EXPECT_TRUE(v->isFixed());
-//}
-//
-// TEST(FactorTest, idealMeasurementConflict) {
-//    using MeasType = FactorMeasurement<double, ZeroNoise>;
-//    using VarType = FactorVariable<double>;
-//    const auto &func = internal::identityMeasurementFunction<double>;
-//    using FactorType = Factor<MeasType, VarType>;
-//    auto v = std::make_shared<VarType>();
-//
-//    // The first factor marks the variable fixed
-//    FactorType{func, MeasType{1.2}, v};
-//
-//    // The second one finds it is already fixed; there must be a conflict
-//    EXPECT_THROW(FactorType(func, MeasType{1.2}, v), std::runtime_error);
-//}
+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)
+    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>();
+
+    PerfectPrior<VarType>{MeasType{1.2}, v};
+    EXPECT_DOUBLE_EQ(1.2, v->value);
+}
 
 }  // namespace wave