FactorGraph: noise handling and priors

wave_optimization/CMakeLists.txt

@@ -19,7 +19,8 @@ WAVE_ADD_TEST(${PROJECT_NAME}_tests
               tests/ceres/ceres_examples_test.cpp
               tests/factor_graph/factor_test.cpp
               tests/factor_graph/graph_test.cpp
-              tests/factor_graph/variable_test.cpp)
+              tests/factor_graph/variable_test.cpp
+              tests/factor_graph/noise_test.cpp)
 TARGET_LINK_LIBRARIES(${PROJECT_NAME}_tests
                       ${PROJECT_NAME}
                       wave_utils

wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp

@@ -17,29 +17,55 @@ namespace wave {
 /** @addtogroup optimization
  *  @{ */
 
-void addFactorToProblem(ceres::Problem &problem, FactorBase &factor) {
+/**
+ * Ceres cost function wrapping `Factor::evaluateRaw`.
+ */
+class FactorCostFunction : public ceres::CostFunction {
+ public:
+    explicit FactorCostFunction(std::shared_ptr<FactorBase> factor)
+        : factor{factor} {
+        this->set_num_residuals(factor->residualSize());
+        for (const auto &var : factor->variables()) {
+            this->mutable_parameter_block_sizes()->push_back(var->size());
+        }
+    }
+
+    bool Evaluate(double const *const *parameters,
+                  double *residuals,
+                  double **jacobians) const override {
+        return factor->evaluateRaw(parameters, residuals, jacobians);
+    }
+
+ private:
+    std::shared_ptr<FactorBase> factor;
+};
+
+void addFactorToProblem(ceres::Problem &problem,
+                        std::shared_ptr<FactorBase> factor) {
     // We make a vector of residual block pointers and pass it to
     // AddResidualBlock because Ceres' implementation forms a vector
     // anyway.
     auto data_ptrs = std::vector<double *>{};
 
-    for (const auto &v : factor.variables()) {
+    for (const auto &v : factor->variables()) {
         data_ptrs.push_back(v->data());
 
         // Explicitly adding parameters "causes additional correctness
         // checking"
         // @todo can add local parametrization in this call
         problem.AddParameterBlock(v->data(), v->size());
 
-        // Set parameter blocks constant if the variable is so marked
-        if (v->isFixed()) {
+        // Set parameter blocks constant if the factor is a zero-noise prior
+        if (factor->isPerfectPrior()) {
             problem.SetParameterBlockConstant(v->data());
         }
     }
 
-    // Give ceres the cost function and its parameter blocks.
-    problem.AddResidualBlock(
-      factor.costFunction().release(), nullptr, data_ptrs);
+    // Finally, give ceres the cost function and its parameter blocks.
+    if (!factor->isPerfectPrior()) {
+        problem.AddResidualBlock(
+          new FactorCostFunction{std::move(factor)}, nullptr, data_ptrs);
+    }
 }
 
 /**
@@ -52,7 +78,7 @@ void evaluateGraph(FactorGraph &graph) {
     ceres::Problem problem;
 
     for (const auto &ptr : graph) {
-        addFactorToProblem(problem, *ptr);
+        addFactorToProblem(problem, ptr);
     }
 
     // Initialize the solver

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

@@ -6,36 +6,16 @@
 #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"
 
+
 namespace wave {
 /** @addtogroup optimization
  *  @{ */
 
-template <int ResidualSize, int... VariableSizes>
-class FactorCostFunction
-  : public ceres::SizedCostFunction<ResidualSize, VariableSizes...> {
- public:
-    explicit FactorCostFunction(
-      std::function<bool(double const *const *, double *, double **)> fn)
-        : f_evaluate{std::move(fn)} {}
-
-    bool Evaluate(double const *const *parameters,
-                  double *residuals,
-                  double **jacobians) const override {
-        return f_evaluate(parameters, residuals, jacobians);
-    }
-
- private:
-    std::function<bool(double const *const *, double *, double **)> f_evaluate;
-};
-
 /**
  * Template for factors of different variable types.
  *
@@ -98,33 +78,17 @@ class Factor : public FactorBase {
     using const_iterator = typename VarArrayType::const_iterator;
 
     /** Construct with the given function, measurement and variables. */
-    explicit Factor(FuncType f,
-                    MeasType meas,
-                    std::shared_ptr<VarTypes>... variable_ptrs)
-        : measurement_function{f},
-          measurement{meas},
-          variable_ptrs{{variable_ptrs...}} {}
-
-    ~Factor() override = default;
+    explicit Factor(FuncType measurement_function,
+                    MeasType measurement,
+                    std::shared_ptr<VarTypes>... variable_ptrs);
 
     int size() const override {
         return NumVars;
     }
-    int numResiduals() const override {
+    int residualSize() const override {
         return ResidualSize;
     }
 
-    std::unique_ptr<ceres::CostFunction> costFunction() override {
-        auto fn = std::bind(&FactorType::evaluateRaw,
-                            this,
-                            std::placeholders::_1,
-                            std::placeholders::_2,
-                            std::placeholders::_3);
-        return std::unique_ptr<ceres::CostFunction>{
-          new FactorCostFunction<ResidualSize, VarTypes::ViewType::Size...>{
-            fn}};
-    }
-
     bool evaluateRaw(double const *const *parameters,
                      double *residuals,
                      double **jacobians) const noexcept override;
@@ -135,6 +99,11 @@ class Factor : public FactorBase {
         return this->variable_ptrs;
     }
 
+    /** Return true if this factor is a zero-noise prior */
+    bool isPerfectPrior() const noexcept override {
+        return false;
+    }
+
     /** Print a representation for debugging. Used by operator<< */
     void print(std::ostream &os) const override;
 
@@ -153,4 +122,4 @@ class Factor : public FactorBase {
 
 #include "impl/Factor.hpp"
 
-#endif
+#endif  // WAVE_OPTIMIZATION_FACTOR_GRAPH_FACTOR_HPP

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

@@ -44,12 +44,17 @@ class FactorBase {
      * Called by `operator<<`. */
     virtual void print(std::ostream &os) const = 0;
 
+    /** The arity (number of variables connected) of the factor */
     virtual int size() const = 0;
-    virtual int numResiduals() const = 0;
-    virtual std::unique_ptr<ceres::CostFunction> costFunction() = 0;
+
+    /** The dimension of the measurement and residual vector */
+    virtual int residualSize() const = 0;
 
     /** Get a reference to the vector of variable pointers */
     virtual const VarVectorType &variables() const noexcept = 0;
+
+    /** Return true if this factor is a zero-noise prior */
+    virtual bool isPerfectPrior() const noexcept = 0;
 };
 
 

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

@@ -53,9 +53,17 @@ class FactorGraph {
      */
     template <typename FuncType, typename MeasType, typename... VarTypes>
     void addFactor(FuncType f,
-                   const MeasType &meas,
+                   const MeasType &measurement,
                    std::shared_ptr<VarTypes>... variables);
 
+    template <typename MeasType>
+    void addPrior(const MeasType &measurement,
+                  std::shared_ptr<typename MeasType::VarType> variable);
+
+    template <typename VarType>
+    void addPerfectPrior(const typename VarType::MappedType &measured_value,
+                         std::shared_ptr<VarType> variable);
+
     // Iterators
 
     /** Return iterator over factors */

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

@@ -7,49 +7,88 @@
 #define WAVE_OPTIMIZATION_FACTOR_GRAPH_MEASUREMENT_HPP
 
 #include "wave/optimization/factor_graph/FactorVariable.hpp"
-
+#include "wave/optimization/factor_graph/Noise.hpp"
+#include "wave/optimization/factor_graph/OutputMap.hpp"
 
 namespace wave {
 /** @addtogroup optimization
  *  @{ */
 
+/** The default NoiseType parameter to FactorMeasurement */
+template <typename V>
+using DefaultNoiseType = DiagonalNoise<FactorVariable<V>::Size>;
+
 /**
- * A measurement associated with a Factor
- *
+ * A measurement, with associated noise, associated with a Factor
  * @tparam V the type of ValueView representing the measurement's value
- * @todo a Noise parameter will be added here
+ * @tparam NoiseTmpl the type of noise
  */
-template <typename V>
+template <typename V, typename N = DefaultNoiseType<V>>
 class FactorMeasurement : public FactorVariable<V> {
     using Base = FactorVariable<V>;
 
  public:
+    using VarType = Base;
     using ViewType = typename Base::ViewType;
+    using NoiseType = N;
     using MappedType = typename Base::MappedType;
     constexpr static int Size = Base::Size;
 
-    /** Construct with initial value */
-    explicit FactorMeasurement(MappedType &&initial)
-        : Base{std::move(initial)} {}
+    /** Construct with initial value and initial noise value*/
+    explicit FactorMeasurement(MappedType initial,
+                               typename NoiseType::InitType noise_value)
+        : Base{std::move(initial)}, noise{std::move(noise_value)} {}
+
 
-    /** Construct copying initial value  */
-    explicit FactorMeasurement(const MappedType &initial) : Base{initial} {}
+    /** Construct with initial value and initial noise object*/
+    explicit FactorMeasurement(MappedType initial, NoiseType noise)
+        : Base{std::move(initial)}, noise{std::move(noise)} {}
+
+    /** Construct with initial value and no noise
+     * Only allowed when NoiseType is void*/
+    explicit FactorMeasurement(MappedType initial) : Base{std::move(initial)} {
+        static_assert(std::is_void<NoiseType>::value,
+                      "A noise value must be provided as the second argument");
+    }
+
+    NoiseType noise;
 };
 
-/** Subtract a measurement from the result of a measurement function
- * @return the difference: the non-normalized residual
+
+/**
+ * Partially specialized FactorMeasurement with zero noise
+ *
+ * This specialization can be constructed with a measured value only, without
+ * needing to specify noise.
  */
 template <typename V>
-inline Eigen::Matrix<double, 1, FactorMeasurement<V>::Size> operator-(
-  const ResultOut<FactorMeasurement<V>::Size> &lhs,
-  const FactorMeasurement<V> &rhs) {
-    // We need to accept the specific types and manually convert them, as Eigen
-    // 3.2 is not as great at automatically converting things. See #151
-    return lhs - Eigen::Map<
-                   const Eigen::Matrix<double, 1, FactorMeasurement<V>::Size>>{
-                   rhs.data()};
+class FactorMeasurement<V, void> : public FactorVariable<V> {
+    using Base = FactorVariable<V>;
+
+ public:
+    using VarType = Base;
+    using ViewType = typename Base::ViewType;
+    using NoiseType = void;
+    using MappedType = typename Base::MappedType;
+    constexpr static int Size = Base::Size;
+
+    /** Construct with initial value and no noise */
+    explicit FactorMeasurement(MappedType initial) : Base{std::move(initial)} {}
+};
+
+template <typename V, typename N>
+inline Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1> operator-(
+  const ResultOut<FactorMeasurement<V, N>::Size> &lhs,
+  const FactorMeasurement<V, N> &rhs) {
+    // We need to accept the specific ResultOut type (instead of a generic Eigen
+    // map) as for some reason they are not converted with Eigen 3.2. See #151
+    return lhs -
+           Eigen::Map<
+             const Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1>>{
+             rhs.data()};
 }
 
+
 /** @} group optimization */
 }  // namespace wave
 

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

@@ -48,19 +48,50 @@ class FactorVariable : public FactorVariableBase {
     using MappedType = typename ViewType::MappedType;
     constexpr static int Size = ViewType::Size;
 
+    // Constructors
+
     /** Default construct with uninitialized estimate
      * Actually initializes to zero to avoid problems with garbage floats
      * @todo move setting of initial value elsewhere
      */
-    FactorVariable() : storage{MappedType::Zero()}, value{storage} {}
+    FactorVariable() noexcept : storage{MappedType::Zero()}, value{storage} {}
 
     /** Construct with initial value */
-    explicit FactorVariable(MappedType &&initial)
+    explicit FactorVariable(MappedType initial)
         : storage{std::move(initial)}, value{storage} {}
 
-    /** Construct copying initial value */
-    explicit FactorVariable(const MappedType &initial)
-        : storage{initial}, value{storage} {}
+    // Because `value` is a map holding a pointer to another member, we must
+    // define a custom copy constructor (and the rest of the rule of five)
+    /** Copy constructor */
+    FactorVariable(const FactorVariable &other) noexcept
+      : FactorVariable{other.storage} {}
+
+    /** Move constructor */
+    FactorVariable(FactorVariable &&other) noexcept
+      : FactorVariable{std::move(other.storage)} {}
+
+    /** Copy assignment operator */
+    FactorVariable &operator=(const FactorVariable &other) noexcept {
+        auto temp = FactorVariable{other};
+        swap(*this, temp);
+        return *this;
+    }
+
+    /** Move assignment operator */
+    FactorVariable &operator=(FactorVariable &&other) noexcept {
+        auto temp = std::move(other);
+        swap(*this, temp);
+        return *this;
+    }
+
+    friend void swap(FactorVariable &lhs, FactorVariable &rhs) noexcept {
+        std::swap(lhs.storage, rhs.storage);
+        // Note: don't swap value
+    }
+
+    ~FactorVariable() override = default;
+
+    // Access
 
     /** Return the number of scalar values in the variable. */
     int size() const noexcept override {