From 8dde1c2d05a7a42e05c90cd0c9c89d6e310dc8db Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Tue, 20 Jun 2017 03:43:32 -0400
Subject: [PATCH 1/9] Add copy constructor (+ big five) to FactorVariable

Because FactorVariable handles pointers (to its own member!) in its constructor, it needs a custom copy constructor and copy assignment operator.

To round out the rule of five (http://en.cppreference.com/w/cpp/language/rule_of_three) also define move constructor, move assignment, and destructor.

Add tests which would fail previously.
---
 .../factor_graph/FactorVariable.hpp           | 49 +++++++++++++++++--
 .../tests/factor_graph/variable_test.cpp      | 38 ++++++++++++++
 2 files changed, 82 insertions(+), 5 deletions(-)

diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp
index 8c73c570..119e21ed 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp
@@ -48,19 +48,58 @@ 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)
-        : storage{std::move(initial)}, value{storage} {}
+    explicit FactorVariable(MappedType &&initial) noexcept
+      : storage{std::move(initial)},
+        value{storage} {}
 
     /** Construct copying initial value */
-    explicit FactorVariable(const MappedType &initial)
-        : storage{initial}, value{storage} {}
+    explicit FactorVariable(const MappedType &initial) noexcept
+      : 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 {
diff --git a/wave_optimization/tests/factor_graph/variable_test.cpp b/wave_optimization/tests/factor_graph/variable_test.cpp
index 47d1dfce..bfd5a3de 100644
--- a/wave_optimization/tests/factor_graph/variable_test.cpp
+++ b/wave_optimization/tests/factor_graph/variable_test.cpp
@@ -49,4 +49,42 @@ TEST(VariableTest, constructInitialRvalue) {
     EXPECT_PRED2(VectorsNear, expected, Eigen::Map<Vec2>{var.value.data()});
 }
 
+TEST(VariableTest, copyConstruct) {
+    // Because FactorVariable works with pointers to its storage, it must have
+    // a custom copy constructor
+    FactorVariable<ValueView<2>> a{Vec2{1.1, 2.2}};
+    FactorVariable<ValueView<2>> b{a};
+
+    ASSERT_NE(&a, &b);
+    EXPECT_NE(a.data(), b.data());
+    EXPECT_DOUBLE_EQ(1.1, *a.data());
+    EXPECT_DOUBLE_EQ(1.1, *b.data());
+}
+
+TEST(VariableTest, moveConstruct) {
+    FactorVariable<ValueView<2>> a;
+    auto a_ptr = a.data();
+    FactorVariable<ValueView<2>> b{std::move(a)};
+
+    ASSERT_NE(&a, &b);
+    EXPECT_NE(a_ptr, b.data());
+}
+
+TEST(VariableTest, copyAssign) {
+    FactorVariable<ValueView<2>> a, b;
+    b = a;
+
+    ASSERT_NE(&a, &b);
+    EXPECT_NE(a.data(), b.data());
+}
+
+
+TEST(VariableTest, moveAssign) {
+    FactorVariable<ValueView<2>> a, b;
+    b = std::move(a);
+
+    ASSERT_NE(&a, &b);
+    EXPECT_NE(a.data(), b.data());
+}
+
 }  // namespace wave

From c59d81a6a65c0456133628a00877530bd37e300f Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Sat, 17 Jun 2017 14:48:43 -0400
Subject: [PATCH 2/9] Add Noise and priors

- Handle special case of size-1 variables

Variables of size 1 are a special case, allowing values to be given as doubles instead of Eigen::Matrix<double, 1, 1>.

- Add `inline` to FactorGraph method definitions

It is needed for ODR since the class is not a template

- Add tests
---
 wave_optimization/CMakeLists.txt              |   3 +-
 .../wave/optimization/factor_graph/Factor.hpp |  20 ++--
 .../optimization/factor_graph/FactorGraph.hpp |   6 +-
 .../factor_graph/FactorMeasurement.hpp        |  79 ++++++++++----
 .../wave/optimization/factor_graph/Noise.hpp  | 102 ++++++++++++++++++
 .../optimization/factor_graph/OutputMap.hpp   |  26 +++++
 .../optimization/factor_graph/impl/Factor.hpp |  42 +++++++-
 .../factor_graph/impl/FactorGraph.hpp         |  57 +++++++---
 .../tests/factor_graph/factor_test.cpp        |  35 +++++-
 .../tests/factor_graph/graph_test.cpp         |  64 +++++++++--
 .../tests/factor_graph/noise_test.cpp         |  51 +++++++++
 .../tests/factor_graph/variable_test.cpp      |  23 +++-
 12 files changed, 455 insertions(+), 53 deletions(-)
 create mode 100644 wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
 create mode 100644 wave_optimization/tests/factor_graph/noise_test.cpp

diff --git a/wave_optimization/CMakeLists.txt b/wave_optimization/CMakeLists.txt
index 84bf8096..8b92f597 100644
--- a/wave_optimization/CMakeLists.txt
+++ b/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
diff --git a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
index ee549df6..48a0a29e 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
@@ -98,12 +98,9 @@ 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...}} {}
+    explicit Factor(FuncType measurement_function,
+                    MeasType measurement,
+                    std::shared_ptr<VarTypes>... variable_ptrs);
 
     ~Factor() override = default;
 
@@ -121,8 +118,7 @@ class Factor : public FactorBase {
                             std::placeholders::_2,
                             std::placeholders::_3);
         return std::unique_ptr<ceres::CostFunction>{
-          new FactorCostFunction<ResidualSize, VarTypes::ViewType::Size...>{
-            fn}};
+          new FactorCostFunction<ResidualSize, VarTypes::Size...>{fn}};
     }
 
     bool evaluateRaw(double const *const *parameters,
@@ -139,6 +135,12 @@ class Factor : public FactorBase {
     void print(std::ostream &os) const override;
 
  private:
+    /** Set each variable fixed
+     * @throw std::runtime_error if one is already fixed
+     */
+    void setVariablesFixed();
+
+
     FuncType *measurement_function;
 
     /** Storage of the measurement */
@@ -153,4 +155,4 @@ class Factor : public FactorBase {
 
 #include "impl/Factor.hpp"
 
-#endif
+#endif  // WAVE_OPTIMIZATION_FACTOR_GRAPH_PERFECT_PRIOR_HPP
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp
index 708037b5..5f779676 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp
@@ -53,9 +53,13 @@ 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);
+
     // Iterators
 
     /** Return iterator over factors */
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
index c0802bb8..a934ca9c 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
@@ -7,49 +7,92 @@
 #define WAVE_OPTIMIZATION_FACTOR_GRAPH_MEASUREMENT_HPP
 
 #include "wave/optimization/factor_graph/FactorVariable.hpp"
-
+#include "wave/optimization/factor_graph/Noise.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 and initial noise value*/
+    explicit FactorMeasurement(MappedType &&initial,
+                               typename NoiseType::InitType &&noise)
+        : Base{std::move(initial)}, noise{std::move(noise)} {}
+
+    /** Construct copying initial value and initial noise value*/
+    explicit FactorMeasurement(const MappedType &initial,
+                               const typename NoiseType::InitType &noise)
+        : Base{initial}, noise{noise} {}
+
+    /** Construct with initial value and initial noise value*/
+    explicit FactorMeasurement(MappedType &&initial, NoiseType &&noise)
+        : Base{std::move(initial)}, noise{std::move(noise)} {}
+
+    /** Construct copying initial value and initial noise value*/
+    explicit FactorMeasurement(const MappedType &initial,
+                               const NoiseType &noise)
+        : Base{initial}, noise{noise} {}
+
+    NoiseType noise;
+};
+
+/**
+ * Partially specialized FactorMeasurement with zero noise
+ */
+template <typename V>
+class FactorMeasurement<V, ZeroNoise> : public FactorVariable<V> {
+    using Base = FactorVariable<V>;
+
+ public:
+    using ViewType = typename Base::ViewType;
+    using NoiseType = ZeroNoise;
     using MappedType = typename Base::MappedType;
     constexpr static int Size = Base::Size;
 
     /** Construct with initial value */
     explicit FactorMeasurement(MappedType &&initial)
-        : Base{std::move(initial)} {}
+        : Base{std::move(initial)} {
+        this->setFixed(true);
+    }
 
     /** Construct copying initial value  */
     explicit FactorMeasurement(const MappedType &initial) : Base{initial} {}
+
+    ZeroNoise noise;
 };
 
-/** Subtract a measurement from the result of a measurement function
- * @return the difference: the non-normalized residual
- */
-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()};
+
+template <typename V, typename N>
+inline Eigen::Matrix<double, 1, FactorMeasurement<V, N>::Size> operator-(
+  const Eigen::Ref<Eigen::Matrix<double, 1, FactorMeasurement<V, N>::Size>>
+    &lhs,
+  const FactorMeasurement<V, N> &rhs) {
+    return lhs -
+           Eigen::Map<
+             const Eigen::Matrix<double, 1, FactorMeasurement<V, N>::Size>>{
+             rhs.data()};
 }
 
+
 /** @} group optimization */
 }  // namespace wave
 
diff --git a/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
new file mode 100644
index 00000000..f9c22548
--- /dev/null
+++ b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
@@ -0,0 +1,102 @@
+/**
+ * @file
+ * @ingroup optimization
+ */
+
+#ifndef WAVE_OPTIMIZATION_FACTOR_GRAPH_NOISE_HPP
+#define WAVE_OPTIMIZATION_FACTOR_GRAPH_NOISE_HPP
+
+#include <iostream>
+
+#include "wave/utils/math.hpp"
+#include "wave/optimization/factor_graph/FactorVariableBase.hpp"
+
+namespace wave {
+/** @addtogroup optimization
+ *  @{ */
+
+template <typename T>
+struct traits {};
+
+/**
+ * Gaussian noise with full covariance matrix
+ * @tparam S dimension of the value
+ */
+template <int S>
+class FullNoise {
+ public:
+    explicit FullNoise(Eigen::Matrix<double, S, S> cov) : covariance_mat{cov} {}
+
+    Eigen::Matrix<double, S, S> inverseSqrtCov() const {
+        const auto L =
+          Eigen::Matrix<double, S, S>{this->covariance_mat.llt().matrixL()};
+        return L.inverse();
+    };
+
+    Eigen::Matrix<double, S, S> covariance() const {
+        return this->covariance_mat;
+    };
+
+ private:
+    Eigen::Matrix<double, S, S> covariance_mat;
+};
+
+/**
+ * Gaussian noise with diagonal covariance matrix
+ * @tparam S dimension of the value
+ */
+template <int S>
+class DiagonalNoise {
+ public:
+    // Decide which type must be passed in the constructor
+    // Normally it's an Eigen vector, but for size-one values accept a double.
+    using InitType = Eigen::Matrix<double, S, 1>;
+
+    explicit DiagonalNoise(const InitType &sigma)
+        : covariance_mat{sigma.cwiseProduct(sigma)} {}
+
+    Eigen::DiagonalMatrix<double, S> inverseSqrtCov() const {
+        return Eigen::DiagonalMatrix<double, S>{
+          this->covariance_mat.diagonal().cwiseSqrt().cwiseInverse()};
+    };
+
+    Eigen::DiagonalMatrix<double, S> covariance() const {
+        return this->covariance_mat;
+    };
+
+ private:
+    Eigen::DiagonalMatrix<double, S> covariance_mat;
+};
+
+/**
+ * Special case of noise for a single value
+ */
+template <>
+class DiagonalNoise<1> {
+ public:
+    using InitType = double;
+
+    explicit DiagonalNoise<1>(double stddev) : stddev{stddev} {}
+
+    double inverseSqrtCov() const {
+        return 1. / this->stddev;
+    }
+
+    double covariance() const {
+        return this->stddev * this->stddev;
+    };
+
+ private:
+    double stddev;
+};
+
+/**
+ * Struct representing the special case of zero noise.
+ */
+class ZeroNoise {};
+
+
+/** @} group optimization */
+}  // namespace wave
+
+#endif  // WAVE_OPTIMIZATION_FACTOR_GRAPH_NOISE_HPP
diff --git a/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp b/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp
index b332728c..8ca69293 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp
@@ -29,11 +29,37 @@ class OutputMap : public Eigen::Map<T> {
         return this->data() != nullptr;
     }
 
+    /* Assign from an Eigen matrix */
     template <typename OtherDerived>
     OutputMap &operator=(const Eigen::MatrixBase<OtherDerived> &other) {
         this->Eigen::Map<T>::operator=(other);
         return *this;
     }
+
+    /**
+     * Assign from a ValueView
+     *
+     * @tparam ViewType the type of ValueView
+     * @tparam MappedType specified only so this template is used only for
+     * ValueView types (see SFINAE)
+     */
+    template <typename ViewType,
+              typename MappedType = typename ViewType::MappedType>
+    OutputMap &operator=(const ViewType &v) {
+        this->Eigen::Map<T>::operator=(Eigen::Map<const MappedType>{v.data()});
+        return *this;
+    }
+
+    /**
+     * Assign from a double
+     * Allowed for values of size 1 only
+     */
+    OutputMap &operator=(double v) {
+        static_assert(OutputMap::SizeAtCompileTime == 1,
+                      "Only OutputMap of size 1 may be assigned doubles");
+        *this->data() = v;
+        return *this;
+    };
 };
 
 /** Type of jacobian output parameters for `Factor::evaluate()`. */
diff --git a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
index 88ba7015..53422269 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
@@ -48,7 +48,9 @@ struct make_index_sequence : make_index_sequence<N - 1, N - 1, Indices...> {};
 template <int... Indices>
 struct make_index_sequence<0, Indices...> : index_sequence<Indices...> {};
 
-/** Constructs a FactorValue mapping the given raw array.
+/** Constructs a ValueView mapping the given raw array.
+ *
+ * Applies for values of size > 1
  *
  * @tparam V the type of FactorVariable
  * @tparam I the index of the variable in this factor
@@ -56,7 +58,8 @@ struct make_index_sequence<0, Indices...> : index_sequence<Indices...> {};
  * @return a FactorValue mapping the Ith array
  */
 template <typename V, int I>
-const typename V::ViewType make_value(double const *const *parameters) {
+inline typename std::enable_if<(V::Size > 1), const typename V::ViewType>::type
+make_value(double const *const *parameters) {
     /* The array is const, but ValueView maps non-const arrays. It would be
      * complicated to have two variants of ValueView, one for const and one
      * for non-const arrays. In this case, we know that the ValueView itself
@@ -67,6 +70,16 @@ const typename V::ViewType make_value(double const *const *parameters) {
     return typename V::ViewType{ptr};
 }
 
+/** Constructs a ValueView mapping the given raw array.
+ *
+ * Specialization for values of size 1
+ */
+template <typename V, int I>
+inline typename std::enable_if<(V::Size == 1), const double &>::type make_value(
+  double const *const *parameters) {
+    return *parameters[I];
+}
+
 /** Constructs a jacobian output object mapping the given raw array.
  *
  * @tparam R size of residuals (height of the jacobian matrix)
@@ -100,6 +113,18 @@ bool callEvaluate(F measurement_function,
 
 }  // namespace internal
 
+template <typename M, typename... V>
+Factor<M, V...>::Factor(Factor::FuncType measurement_function,
+                        M measurement,
+                        std::shared_ptr<V>... variable_ptrs)
+    : measurement_function{measurement_function},
+      measurement{measurement},
+      variable_ptrs{{variable_ptrs...}} {
+    if (measurement.isFixed()) {
+        this->setVariablesFixed();
+    }
+}
+
 template <typename M, typename... V>
 bool Factor<M, V...>::evaluateRaw(double const *const *parameters,
                                   double *residuals,
@@ -136,4 +161,17 @@ void Factor<M, V...>::print(std::ostream &os) const {
     os << "]";
 }
 
+template <typename M, typename... V>
+void Factor<M, V...>::setVariablesFixed() {
+    for (const auto ptr : this->variable_ptrs) {
+        if (ptr->isFixed()) {
+            throw std::runtime_error(
+              "Factor::setVariablesFixed(): Variable was already fixed."
+              " This means there are conflicting ideal measurements.");
+        }
+        ptr->setFixed(true);
+    }
+}
+
+
 }  // namespace wave
diff --git a/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp b/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
index baedb1bb..594e5a70 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
@@ -1,23 +1,45 @@
 namespace wave {
 
+namespace internal {
+
+/**
+ * Trivial measurement function for a prior, f(X) = X
+ */
+template <typename V>
+inline bool identityMeasurementFunction(
+  const V &variable,
+  ResultOut<FactorVariable<V>::Size> result,
+  JacobianOut<FactorVariable<V>::Size, FactorVariable<V>::Size> jacobian) {
+    result = variable;
+
+    if (jacobian) {
+        jacobian.setIdentity();
+    }
+
+    return true;
+}
+
+}  // namespace internal
+
 // Constructors
-FactorGraph::FactorGraph() {}
+inline FactorGraph::FactorGraph() {}
 
 // Capacity
 
-FactorGraph::size_type FactorGraph::countFactors() const noexcept {
+inline FactorGraph::size_type FactorGraph::countFactors() const noexcept {
     return this->factors.size();
 }
 
-bool FactorGraph::empty() const noexcept {
+inline bool FactorGraph::empty() const noexcept {
     return this->factors.empty();
 }
 
 // Modifiers
+
 template <typename FuncType, typename MeasType, typename... VarTypes>
-void FactorGraph::addFactor(FuncType f,
-                            const MeasType &meas,
-                            std::shared_ptr<VarTypes>... variables) {
+inline void FactorGraph::addFactor(FuncType f,
+                                   const MeasType &measurement,
+                                   std::shared_ptr<VarTypes>... variables) {
     using FactorType = Factor<MeasType, VarTypes...>;
 
     // Give a nice error message if the function type is wrong
@@ -26,28 +48,39 @@ void FactorGraph::addFactor(FuncType f,
       "The given measurement function is of incorrect type");
 
     this->factors.emplace_back(
-      std::make_shared<FactorType>(f, meas, std::move(variables)...));
+      std::make_shared<FactorType>(f, measurement, std::move(variables)...));
 };
 
+template <typename MeasType>
+inline void FactorGraph::addPrior(
+  const MeasType &measurement,
+  std::shared_ptr<typename MeasType::VarType> variable) {
+    return this->addFactor(
+      internal::identityMeasurementFunction<typename MeasType::ViewType>,
+      measurement,
+      std::move(variable));
+}
+
 // Iterators
 
-typename FactorGraph::iterator FactorGraph::begin() noexcept {
+inline typename FactorGraph::iterator FactorGraph::begin() noexcept {
     return this->factors.begin();
 }
 
-typename FactorGraph::iterator FactorGraph::end() noexcept {
+inline typename FactorGraph::iterator FactorGraph::end() noexcept {
     return this->factors.end();
 }
 
-typename FactorGraph::const_iterator FactorGraph::begin() const noexcept {
+inline typename FactorGraph::const_iterator FactorGraph::begin() const
+  noexcept {
     return this->factors.begin();
 }
 
-typename FactorGraph::const_iterator FactorGraph::end() const noexcept {
+inline typename FactorGraph::const_iterator FactorGraph::end() const noexcept {
     return this->factors.end();
 }
 
-std::ostream &operator<<(std::ostream &os, const FactorGraph &graph) {
+inline std::ostream &operator<<(std::ostream &os, const FactorGraph &graph) {
     os << "FactorGraph " << graph.countFactors() << " factors [";
     const auto N = graph.countFactors();
     for (auto i = 0 * N; i < N; ++i) {
diff --git a/wave_optimization/tests/factor_graph/factor_test.cpp b/wave_optimization/tests/factor_graph/factor_test.cpp
index e30f2ca1..085b4914 100644
--- a/wave_optimization/tests/factor_graph/factor_test.cpp
+++ b/wave_optimization/tests/factor_graph/factor_test.cpp
@@ -1,5 +1,7 @@
 #include "wave/wave_test.hpp"
 #include "wave/optimization/factor_graph/Factor.hpp"
+#include "wave/optimization/factor_graph/FactorVariable.hpp"
+#include "wave/optimization/factor_graph/FactorGraph.hpp"
 #include "example_instances.hpp"
 
 namespace wave {
@@ -11,7 +13,7 @@ TEST(FactorTest, evaluate) {
     using Landmark2DVar = FactorVariable<Landmark2D>;
 
 
-    DistanceToLandmarkFactor f{DistanceMeasurement{meas},
+    DistanceToLandmarkFactor f{DistanceMeasurement{meas, 0.0},
                                std::make_shared<Pose2DVar>(),
                                std::make_shared<Landmark2DVar>()};
 
@@ -44,7 +46,7 @@ TEST(FactorTest, evaluate) {
 TEST(FactorTest, print) {
     auto v1 = std::make_shared<Pose2DVar>();
     auto v2 = std::make_shared<Landmark2DVar>();
-    auto meas = DistanceMeasurement{0.0};
+    auto meas = DistanceMeasurement{0.0, 0.0};
 
     auto factor = DistanceToLandmarkFactor{meas, v1, v2};
 
@@ -58,7 +60,7 @@ TEST(FactorTest, print) {
 }
 
 TEST(FactorTest, evaluateCostFunction) {
-    auto meas = DistanceMeasurement{1.23};
+    auto meas = DistanceMeasurement{1.23, 0.0};
     DistanceToLandmarkFactor f{
       meas, std::make_shared<Pose2DVar>(), std::make_shared<Landmark2DVar>()};
 
@@ -89,4 +91,31 @@ TEST(FactorTest, evaluateCostFunction) {
       VectorsNear, expected_jac_landmark, Eigen::Map<Vec2>{out_jac_landmark});
 }
 
+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);
+}
+
 }  // namespace wave
diff --git a/wave_optimization/tests/factor_graph/graph_test.cpp b/wave_optimization/tests/factor_graph/graph_test.cpp
index 20decb56..0da53d39 100644
--- a/wave_optimization/tests/factor_graph/graph_test.cpp
+++ b/wave_optimization/tests/factor_graph/graph_test.cpp
@@ -10,7 +10,7 @@ TEST(FactorGraph, add) {
     FactorGraph graph;
     auto p = std::make_shared<Pose2DVar>();
     auto l = std::make_shared<Landmark2DVar>();
-    auto m = DistanceMeasurement{2.3};
+    auto m = DistanceMeasurement{2.3, 0.0};
 
     graph.addFactor(distanceMeasurementFunction, m, p, l);
 
@@ -26,13 +26,65 @@ TEST(FactorGraph, capacity) {
 
     auto p = std::make_shared<Pose2DVar>();
     auto l = std::make_shared<Landmark2DVar>();
-    auto m = DistanceMeasurement{2.3};
+    auto m = DistanceMeasurement{2.3, 0.0};
     graph.addFactor(distanceMeasurementFunction, m, p, l);
 
     EXPECT_EQ(1u, graph.countFactors());
     EXPECT_FALSE(graph.empty());
 }
 
+TEST(FactorGraph, addPrior) {
+    const auto test_meas = Vec2{1.2, 3.4};
+    const auto test_stddev = Vec2{0.01, 0.01};
+    const auto test_val = Vec2{1.23, 3.38};
+
+    // Prepare arguments to add unary factor
+    FactorGraph graph;
+    auto p = std::make_shared<Landmark2DVar>();
+    auto m = FactorMeasurement<Landmark2D>{test_meas, test_stddev};
+
+    // Prepare arguments in a form matching ceres calls
+    Vec2 test_residual;
+    Mat2 test_jac;
+    const double *const params[] = {test_val.data()};
+    double *jacs[] = {test_jac.data()};
+
+    // Add the factor, retrieve the pointer to it, and verity
+    graph.addPrior(m, p);
+    auto factor = *graph.begin();
+    ASSERT_NE(nullptr, factor);
+
+    EXPECT_TRUE(factor->evaluateRaw(params, test_residual.data(), jacs));
+    EXPECT_PRED2(VectorsNear, test_val - test_meas, test_residual);
+    EXPECT_PRED2(MatricesNear, Mat2::Identity(), test_jac);
+}
+
+TEST(FactorGraph, addPriorOfSize1) {
+    const auto test_meas = 1.2;
+    const auto test_stddev = 0.01;
+    const auto test_val = 1.23;
+
+    // Prepare arguments to add unary factor
+    FactorGraph graph;
+    auto p = std::make_shared<FactorVariable<double>>();
+    auto m = FactorMeasurement<double>{test_meas, test_stddev};
+
+    // 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, retrieve the pointer to it, and verity
+    graph.addPrior(m, p);
+    auto factor = *graph.begin();
+    ASSERT_NE(nullptr, factor);
+
+    EXPECT_TRUE(factor->evaluateRaw(params, &test_residual, jacs));
+    EXPECT_DOUBLE_EQ(test_val - test_meas, test_residual);
+    EXPECT_DOUBLE_EQ(1.0, test_jac);
+}
+
 TEST(FactorGraph, triangulationSim) {
     // Simulate a robot passing by a landmark
     // First generate "true" data
@@ -58,7 +110,7 @@ TEST(FactorGraph, triangulationSim) {
         pose_vars.push_back(p);
         for (auto i = 0u; i < landmark_vars.size(); ++i) {
             auto distance = double{(true_l_pos[i] - pose.head<2>()).norm()};
-            auto meas = DistanceMeasurement{distance};
+            auto meas = DistanceMeasurement{distance, 0.0};
             graph.addFactor(
               distanceMeasurementFunction, meas, p, landmark_vars[i]);
         }
@@ -85,9 +137,9 @@ TEST(GraphTest, print) {
 
     auto graph = FactorGraph{};
 
-    auto m1 = DistanceMeasurement{0.0};
-    auto m2 = DistanceMeasurement{1.1};
-    auto m3 = DistanceMeasurement{2.2};
+    auto m1 = DistanceMeasurement{0.0, 0.0};
+    auto m2 = DistanceMeasurement{1.1, 0.0};
+    auto m3 = DistanceMeasurement{2.2, 0.0};
 
     graph.addFactor(distanceMeasurementFunction, m1, v1, l);
     graph.addFactor(distanceMeasurementFunction, m2, v2, l);
diff --git a/wave_optimization/tests/factor_graph/noise_test.cpp b/wave_optimization/tests/factor_graph/noise_test.cpp
new file mode 100644
index 00000000..2cfffbde
--- /dev/null
+++ b/wave_optimization/tests/factor_graph/noise_test.cpp
@@ -0,0 +1,51 @@
+#include "wave/wave_test.hpp"
+#include "wave/optimization/factor_graph/Noise.hpp"
+
+namespace wave {
+
+TEST(NoiseTest, diagonalNoise) {
+    const auto 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;
+
+    auto n = DiagonalNoise<2>{stddev};
+
+    auto res = Eigen::MatrixXd{n.covariance()};
+    EXPECT_PRED2(MatricesNear, expected_cov, res);
+
+    res = Eigen::MatrixXd{n.inverseSqrtCov()};
+    EXPECT_PRED2(MatricesNear, expected_inv, res);
+}
+
+TEST(NoiseTest, singleNoise) {
+    const auto 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{};
+    cov << 3.3, 1.1, 1.1, 4.4;
+
+    auto n = FullNoise<2>{cov};
+
+    // inverse of cholesky of cov calculated separately
+    Mat2 expected_inv;
+    expected_inv << 0.550481882563180, 0, -0.165976532577323, 0.497929597731969;
+
+    auto res = Eigen::MatrixXd{n.covariance()};
+    EXPECT_PRED2(MatricesNear, cov, res);
+
+    res = Eigen::MatrixXd{n.inverseSqrtCov()};
+    EXPECT_PRED2(MatricesNear, expected_inv, res);
+}
+
+TEST(NoiseTest, zeroNoise) {
+    static_assert(std::is_empty<ZeroNoise>::value,
+                  "ZeroNoise expected to be have no data members.");
+}
+
+}  // namespace wave
diff --git a/wave_optimization/tests/factor_graph/variable_test.cpp b/wave_optimization/tests/factor_graph/variable_test.cpp
index bfd5a3de..267c5b14 100644
--- a/wave_optimization/tests/factor_graph/variable_test.cpp
+++ b/wave_optimization/tests/factor_graph/variable_test.cpp
@@ -1,6 +1,6 @@
 #include "wave/wave_test.hpp"
 #include "wave/optimization/factor_graph/FactorVariable.hpp"
-
+#include "wave/optimization/factor_graph/FactorMeasurement.hpp"
 
 namespace wave {
 
@@ -87,4 +87,25 @@ TEST(VariableTest, moveAssign) {
     EXPECT_NE(a.data(), b.data());
 }
 
+TEST(VariableTest, setFixed) {
+    // Construct and initialize a variable
+    auto var = FactorVariable<ValueView<2>>{Vec2{1.1, 2.2}};
+
+    EXPECT_FALSE(var.isFixed());
+
+    var.setFixed(true);
+    EXPECT_TRUE(var.isFixed());
+
+    var.setFixed(false);
+    EXPECT_FALSE(var.isFixed());
+}
+
+TEST(MeasurementTest, constructFromRvalue) {
+    auto meas = FactorMeasurement<ValueView<2>>{Vec2{1.1, 2.2}, Vec2{0.1, 0.2}};
+    const auto expected_val = Vec2{1.1, 2.2};
+    EXPECT_EQ(2, meas.size());
+    EXPECT_PRED2(
+      VectorsNear, expected_val, Eigen::Map<Vec2>{meas.value.data()});
+}
+
 }  // namespace wave

From 318d5ba263792273b5b851d582968975f3c9d847 Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Sat, 17 Jun 2017 19:46:34 -0400
Subject: [PATCH 3/9] Add normalization of residuals

---
 .../wave/optimization/factor_graph/Factor.hpp |  1 -
 .../factor_graph/FactorMeasurement.hpp        |  6 +++---
 .../wave/optimization/factor_graph/Noise.hpp  | 17 ++++++++++++++++-
 .../optimization/factor_graph/impl/Factor.hpp |  4 +++-
 .../tests/factor_graph/factor_test.cpp        |  6 +++---
 .../tests/factor_graph/graph_test.cpp         | 19 +++++++++++--------
 .../tests/factor_graph/noise_test.cpp         |  5 +++++
 7 files changed, 41 insertions(+), 17 deletions(-)

diff --git a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
index 48a0a29e..5e6e12cb 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
@@ -140,7 +140,6 @@ class Factor : public FactorBase {
      */
     void setVariablesFixed();
 
-
     FuncType *measurement_function;
 
     /** Storage of the measurement */
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
index a934ca9c..ed7c9ab7 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
@@ -82,13 +82,13 @@ class FactorMeasurement<V, ZeroNoise> : public FactorVariable<V> {
 
 
 template <typename V, typename N>
-inline Eigen::Matrix<double, 1, FactorMeasurement<V, N>::Size> operator-(
-  const Eigen::Ref<Eigen::Matrix<double, 1, FactorMeasurement<V, N>::Size>>
+inline Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1> operator-(
+  const Eigen::Ref<Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1>>
     &lhs,
   const FactorMeasurement<V, N> &rhs) {
     return lhs -
            Eigen::Map<
-             const Eigen::Matrix<double, 1, FactorMeasurement<V, N>::Size>>{
+             const Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1>>{
              rhs.data()};
 }
 
diff --git a/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
index f9c22548..aeaa6966 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
@@ -92,8 +92,23 @@ class DiagonalNoise<1> {
 
 /**
  * Struct representing the special case of zero noise.
+ *
+ * Its member functions always throw, and are not expected to be called. Any
+ * measurements attached to factors with a ZeroNoise measurement are marked
+ * constant, and should never be evaluated by the optimizer.
+ *
+ * @todo make sure this assumption holds up
  */
-class ZeroNoise {};
+class ZeroNoise {
+ public:
+    double inverseSqrtCov() const {
+        throw std::logic_error("Special ZeroNoise type has no covariance");
+    }
+
+    double covariance() const {
+        throw std::logic_error("Special ZeroNoise type has no covariance");
+    };
+};
 
 
 /** @} group optimization */
diff --git a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
index 53422269..b35988be 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
@@ -141,7 +141,9 @@ bool Factor<M, V...>::evaluateRaw(double const *const *parameters,
       internal::make_index_sequence<sizeof...(V)>());
 
     if (ok) {
-        results = results - this->measurement;
+        // Calculate the normalized residual
+        const auto &L = this->measurement.noise.inverseSqrtCov();
+        results = L * (results - this->measurement);
     }
     return ok;
 }
diff --git a/wave_optimization/tests/factor_graph/factor_test.cpp b/wave_optimization/tests/factor_graph/factor_test.cpp
index 085b4914..c7388753 100644
--- a/wave_optimization/tests/factor_graph/factor_test.cpp
+++ b/wave_optimization/tests/factor_graph/factor_test.cpp
@@ -13,7 +13,7 @@ TEST(FactorTest, evaluate) {
     using Landmark2DVar = FactorVariable<Landmark2D>;
 
 
-    DistanceToLandmarkFactor f{DistanceMeasurement{meas, 0.0},
+    DistanceToLandmarkFactor f{DistanceMeasurement{meas, 1.0},
                                std::make_shared<Pose2DVar>(),
                                std::make_shared<Landmark2DVar>()};
 
@@ -46,7 +46,7 @@ TEST(FactorTest, evaluate) {
 TEST(FactorTest, print) {
     auto v1 = std::make_shared<Pose2DVar>();
     auto v2 = std::make_shared<Landmark2DVar>();
-    auto meas = DistanceMeasurement{0.0, 0.0};
+    auto meas = DistanceMeasurement{0.0, 1.0};
 
     auto factor = DistanceToLandmarkFactor{meas, v1, v2};
 
@@ -60,7 +60,7 @@ TEST(FactorTest, print) {
 }
 
 TEST(FactorTest, evaluateCostFunction) {
-    auto meas = DistanceMeasurement{1.23, 0.0};
+    auto meas = DistanceMeasurement{1.23, 1.0};
     DistanceToLandmarkFactor f{
       meas, std::make_shared<Pose2DVar>(), std::make_shared<Landmark2DVar>()};
 
diff --git a/wave_optimization/tests/factor_graph/graph_test.cpp b/wave_optimization/tests/factor_graph/graph_test.cpp
index 0da53d39..caa8cbdd 100644
--- a/wave_optimization/tests/factor_graph/graph_test.cpp
+++ b/wave_optimization/tests/factor_graph/graph_test.cpp
@@ -10,7 +10,7 @@ TEST(FactorGraph, add) {
     FactorGraph graph;
     auto p = std::make_shared<Pose2DVar>();
     auto l = std::make_shared<Landmark2DVar>();
-    auto m = DistanceMeasurement{2.3, 0.0};
+    auto m = DistanceMeasurement{2.3, 1.0};
 
     graph.addFactor(distanceMeasurementFunction, m, p, l);
 
@@ -26,7 +26,7 @@ TEST(FactorGraph, capacity) {
 
     auto p = std::make_shared<Pose2DVar>();
     auto l = std::make_shared<Landmark2DVar>();
-    auto m = DistanceMeasurement{2.3, 0.0};
+    auto m = DistanceMeasurement{2.3, 1.0};
     graph.addFactor(distanceMeasurementFunction, m, p, l);
 
     EXPECT_EQ(1u, graph.countFactors());
@@ -37,6 +37,8 @@ TEST(FactorGraph, addPrior) {
     const auto test_meas = Vec2{1.2, 3.4};
     const auto test_stddev = Vec2{0.01, 0.01};
     const auto test_val = Vec2{1.23, 3.38};
+    // The expected residuals are normalized
+    const Vec2 expected_res = (test_val - test_meas).cwiseQuotient(test_stddev);
 
     // Prepare arguments to add unary factor
     FactorGraph graph;
@@ -55,7 +57,7 @@ TEST(FactorGraph, addPrior) {
     ASSERT_NE(nullptr, factor);
 
     EXPECT_TRUE(factor->evaluateRaw(params, test_residual.data(), jacs));
-    EXPECT_PRED2(VectorsNear, test_val - test_meas, test_residual);
+    EXPECT_PRED2(VectorsNear, expected_res, test_residual);
     EXPECT_PRED2(MatricesNear, Mat2::Identity(), test_jac);
 }
 
@@ -63,6 +65,7 @@ TEST(FactorGraph, addPriorOfSize1) {
     const auto test_meas = 1.2;
     const auto test_stddev = 0.01;
     const auto test_val = 1.23;
+    const auto expected_res = (test_val - test_meas) / test_stddev;
 
     // Prepare arguments to add unary factor
     FactorGraph graph;
@@ -81,7 +84,7 @@ TEST(FactorGraph, addPriorOfSize1) {
     ASSERT_NE(nullptr, factor);
 
     EXPECT_TRUE(factor->evaluateRaw(params, &test_residual, jacs));
-    EXPECT_DOUBLE_EQ(test_val - test_meas, test_residual);
+    EXPECT_DOUBLE_EQ(expected_res, test_residual);
     EXPECT_DOUBLE_EQ(1.0, test_jac);
 }
 
@@ -110,7 +113,7 @@ TEST(FactorGraph, triangulationSim) {
         pose_vars.push_back(p);
         for (auto i = 0u; i < landmark_vars.size(); ++i) {
             auto distance = double{(true_l_pos[i] - pose.head<2>()).norm()};
-            auto meas = DistanceMeasurement{distance, 0.0};
+            auto meas = DistanceMeasurement{distance, 1.0};
             graph.addFactor(
               distanceMeasurementFunction, meas, p, landmark_vars[i]);
         }
@@ -137,9 +140,9 @@ TEST(GraphTest, print) {
 
     auto graph = FactorGraph{};
 
-    auto m1 = DistanceMeasurement{0.0, 0.0};
-    auto m2 = DistanceMeasurement{1.1, 0.0};
-    auto m3 = DistanceMeasurement{2.2, 0.0};
+    auto m1 = DistanceMeasurement{0.0, 1.0};
+    auto m2 = DistanceMeasurement{1.1, 1.0};
+    auto m3 = DistanceMeasurement{2.2, 1.0};
 
     graph.addFactor(distanceMeasurementFunction, m1, v1, l);
     graph.addFactor(distanceMeasurementFunction, m2, v2, l);
diff --git a/wave_optimization/tests/factor_graph/noise_test.cpp b/wave_optimization/tests/factor_graph/noise_test.cpp
index 2cfffbde..45555509 100644
--- a/wave_optimization/tests/factor_graph/noise_test.cpp
+++ b/wave_optimization/tests/factor_graph/noise_test.cpp
@@ -46,6 +46,11 @@ TEST(NoiseTest, fullNoise) {
 TEST(NoiseTest, zeroNoise) {
     static_assert(std::is_empty<ZeroNoise>::value,
                   "ZeroNoise expected to be have no data members.");
+
+    auto n = ZeroNoise{};
+
+    EXPECT_THROW(n.covariance(), std::logic_error);
+    EXPECT_THROW(n.inverseSqrtCov(), std::logic_error);
 }
 
 }  // namespace wave

From 6e8e127e6e1aed95187fcd94338812de041a0ebd Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Sat, 17 Jun 2017 20:28:22 -0400
Subject: [PATCH 4/9] Add PerfectPrior class

(Squashed commit)

- Add addPerfectPrior

- Simplify constructors

When we have to copy a value, accept a value.

The alternative is to have two overloads: (T&&) and (T const&) but this adds repeated code.

See https://stackoverflow.com/questions/7592630/is-pass-by-value-a-reasonable-default-in-c11

- Refactor perfect priors

- Setting variables constant is handled in graph_wrapper, not by factors
- Remove ZeroNoise type, instead, use "void" NoiseType
- evaluateRaw calls a different templated helper function if the Factor has zero noise

- Add PerfectPrior class

Remove the evaluateSpecialized helper, too complicated.

Have addPerfectPrior add a special PerfectPrior class. This class does nothing on evaluateRaw.

Set the variable value, as well setting it constant. This is the key!
---
 .../wave/optimization/ceres/graph_wrapper.hpp | 10 ++-
 .../wave/optimization/factor_graph/Factor.hpp | 73 +++++++++++++++++--
 .../optimization/factor_graph/FactorBase.hpp  |  3 +
 .../optimization/factor_graph/FactorGraph.hpp |  4 +
 .../factor_graph/FactorMeasurement.hpp        | 46 +++++-------
 .../factor_graph/FactorVariable.hpp           | 12 +--
 .../factor_graph/FactorVariableBase.hpp       | 17 -----
 .../wave/optimization/factor_graph/Noise.hpp  | 21 ------
 .../optimization/factor_graph/ValueView.hpp   | 10 +++
 .../optimization/factor_graph/impl/Factor.hpp | 54 ++++++--------
 .../factor_graph/impl/FactorGraph.hpp         | 12 +++
 .../tests/factor_graph/factor_test.cpp        | 53 +++++++-------
 .../tests/factor_graph/graph_test.cpp         | 47 +++++++++++-
 .../tests/factor_graph/noise_test.cpp         | 10 ---
 .../tests/factor_graph/variable_test.cpp      | 13 ----
 15 files changed, 217 insertions(+), 168 deletions(-)

diff --git a/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp b/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp
index 57527f9e..dae847e5 100644
--- a/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp
+++ b/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp
@@ -31,15 +31,17 @@ void addFactorToProblem(ceres::Problem &problem, FactorBase &factor) {
         // @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);
+    if (!factor.isPerfectPrior()) {
+        problem.AddResidualBlock(
+          factor.costFunction().release(), nullptr, data_ptrs);
+    }
 }
 
 /**
diff --git a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
index 5e6e12cb..63aa8708 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
@@ -13,6 +13,8 @@
 #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 {
 /** @addtogroup optimization
@@ -131,15 +133,15 @@ 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;
 
  private:
-    /** Set each variable fixed
-     * @throw std::runtime_error if one is already fixed
-     */
-    void setVariablesFixed();
-
     FuncType *measurement_function;
 
     /** Storage of the measurement */
@@ -149,6 +151,67 @@ 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 numResiduals() const override {
+        return ResidualSize;
+    }
+
+    std::unique_ptr<ceres::CostFunction> costFunction() override {
+        return nullptr;
+    }
+
+    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
 
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp
index 6bee8a1e..1d379027 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp
@@ -50,6 +50,9 @@ class FactorBase {
 
     /** 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;
 };
 
 
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp
index 5f779676..94e8b70d 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorGraph.hpp
@@ -60,6 +60,10 @@ class FactorGraph {
     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 */
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
index ed7c9ab7..ea9294c7 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
@@ -34,53 +34,47 @@ class FactorMeasurement : public FactorVariable<V> {
     constexpr static int Size = Base::Size;
 
     /** Construct with initial value and initial noise value*/
-    explicit FactorMeasurement(MappedType &&initial,
-                               typename NoiseType::InitType &&noise)
-        : Base{std::move(initial)}, noise{std::move(noise)} {}
+    explicit FactorMeasurement(MappedType initial,
+                               typename NoiseType::InitType noise_value)
+        : Base{std::move(initial)}, noise{std::move(noise_value)} {}
 
-    /** Construct copying initial value and initial noise value*/
-    explicit FactorMeasurement(const MappedType &initial,
-                               const typename NoiseType::InitType &noise)
-        : Base{initial}, noise{noise} {}
 
-    /** Construct with initial value and initial noise value*/
-    explicit FactorMeasurement(MappedType &&initial, NoiseType &&noise)
+    /** Construct with initial value and initial noise object*/
+    explicit FactorMeasurement(MappedType initial, NoiseType noise)
         : Base{std::move(initial)}, noise{std::move(noise)} {}
 
-    /** Construct copying initial value and initial noise value*/
-    explicit FactorMeasurement(const MappedType &initial,
-                               const NoiseType &noise)
-        : Base{initial}, noise{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;
 };
 
+
 /**
  * Partially specialized FactorMeasurement with zero noise
+ *
+ * This specialization can be constructed with a measured value only, without
+ * needing to specify noise.
  */
 template <typename V>
-class FactorMeasurement<V, ZeroNoise> : public FactorVariable<V> {
+class FactorMeasurement<V, void> : public FactorVariable<V> {
     using Base = FactorVariable<V>;
 
  public:
+    using VarType = Base;
     using ViewType = typename Base::ViewType;
-    using NoiseType = ZeroNoise;
+    using NoiseType = void;
     using MappedType = typename Base::MappedType;
     constexpr static int Size = Base::Size;
 
-    /** Construct with initial value */
-    explicit FactorMeasurement(MappedType &&initial)
-        : Base{std::move(initial)} {
-        this->setFixed(true);
-    }
-
-    /** Construct copying initial value  */
-    explicit FactorMeasurement(const MappedType &initial) : Base{initial} {}
-
-    ZeroNoise noise;
+    /** 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 Eigen::Ref<Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1>>
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp
index 119e21ed..cf7efc66 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorVariable.hpp
@@ -57,18 +57,11 @@ class FactorVariable : public FactorVariableBase {
     FactorVariable() noexcept : storage{MappedType::Zero()}, value{storage} {}
 
     /** Construct with initial value */
-    explicit FactorVariable(MappedType &&initial) noexcept
-      : storage{std::move(initial)},
-        value{storage} {}
-
-    /** Construct copying initial value */
-    explicit FactorVariable(const MappedType &initial) noexcept
-      : storage{initial},
-        value{storage} {}
+    explicit FactorVariable(MappedType initial)
+        : storage{std::move(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} {}
@@ -100,7 +93,6 @@ class FactorVariable : public FactorVariableBase {
 
     // Access
 
-
     /** Return the number of scalar values in the variable. */
     int size() const noexcept override {
         return Size;
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorVariableBase.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorVariableBase.hpp
index c71726ed..8c59fdd2 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorVariableBase.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorVariableBase.hpp
@@ -22,26 +22,9 @@ class FactorVariableBase {
     virtual const double *data() const noexcept = 0;
     virtual double *data() noexcept = 0;
 
-    /** Marks as constant during optimization
-     * @todo replace with unary factors for priors
-     */
-    void setFixed(bool c) noexcept {
-        this->fixed = c;
-    }
-
-    /** Whether this has been marked constant during optimization
-     * @todo replace with unary factors for priors
-     */
-    bool isFixed() const noexcept {
-        return this->fixed;
-    }
-
     /** Print representation of the object for debugging.
      * Called by `operator<<`. */
     virtual void print(std::ostream &os) const = 0;
-
- private:
-    bool fixed = false;
 };
 
 inline std::ostream &operator<<(std::ostream &os, const FactorVariableBase &v) {
diff --git a/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
index aeaa6966..eac20111 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
@@ -90,27 +90,6 @@ class DiagonalNoise<1> {
     double stddev;
 };
 
-/**
- * Struct representing the special case of zero noise.
- *
- * Its member functions always throw, and are not expected to be called. Any
- * measurements attached to factors with a ZeroNoise measurement are marked
- * constant, and should never be evaluated by the optimizer.
- *
- * @todo make sure this assumption holds up
- */
-class ZeroNoise {
- public:
-    double inverseSqrtCov() const {
-        throw std::logic_error("Special ZeroNoise type has no covariance");
-    }
-
-    double covariance() const {
-        throw std::logic_error("Special ZeroNoise type has no covariance");
-    };
-};
-
-
 /** @} group optimization */
 }  // namespace wave
 
diff --git a/wave_optimization/include/wave/optimization/factor_graph/ValueView.hpp b/wave_optimization/include/wave/optimization/factor_graph/ValueView.hpp
index 2f65f15c..255a901e 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/ValueView.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/ValueView.hpp
@@ -55,6 +55,16 @@ class ValueView {
         return Size;
     }
 
+    /** Return an Eigen Map to the data */
+    Eigen::Map<const MappedType> asVector() const {
+        return Eigen::Map<const MappedType>{this->dataptr};
+    }
+
+    /** Return an Eigen Map to the data */
+    Eigen::Map<MappedType> asVector() {
+        return Eigen::Map<MappedType>{this->dataptr};
+    }
+
     /** Return a raw pointer to the start of the internal vector */
     const double *data() const {
         return this->dataptr;
diff --git a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
index b35988be..63916ce3 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
@@ -97,18 +97,22 @@ JacobianOut<R, C> make_jacobian(double **jacobians) {
 /** Calls Factor::evaluate() with the necessary arguments generated from the
  * Factor type (known at compile time) and the provided array pointers (known
  * at run time).
+ *
+ * Calling a separate function expands the given index sequence - this approach
+ * is explained above in the documentation for `index_sequence`.
  */
-template <class F, int R, typename... V, int... Is>
-bool callEvaluate(F measurement_function,
+template <typename M, typename... V, int... Is>
+bool callEvaluate(typename Factor<M, V...>::FuncType measurement_function,
                   double const *const *parameters,
-                  const ResultOut<R> &results,
+                  ResultOut<M::Size> &residuals,
                   double **jacobians,
                   index_sequence<Is...>) noexcept {
     // Call evaluate, generating the correct type and number of the three kinds
     // of arguments: input values, output residuals, and output jacobians.
-    return measurement_function(make_value<V, Is>(parameters)...,
-                                results,
-                                make_jacobian<R, V::Size, Is>(jacobians)...);
+    return measurement_function(
+      make_value<V, Is>(parameters)...,
+      residuals,
+      make_jacobian<M::Size, V::Size, Is>(jacobians)...);
 }
 
 }  // namespace internal
@@ -119,32 +123,31 @@ Factor<M, V...>::Factor(Factor::FuncType measurement_function,
                         std::shared_ptr<V>... variable_ptrs)
     : measurement_function{measurement_function},
       measurement{measurement},
-      variable_ptrs{{variable_ptrs...}} {
-    if (measurement.isFixed()) {
-        this->setVariablesFixed();
-    }
-}
+      variable_ptrs{{variable_ptrs...}} {}
 
+// Definition of evaluateRaw for regular factor (not perfect prior)
 template <typename M, typename... V>
 bool Factor<M, V...>::evaluateRaw(double const *const *parameters,
-                                  double *residuals,
+                                  double *raw_residuals,
                                   double **jacobians) const noexcept {
-    auto results = ResultOut<M::Size>{residuals};
+    // Map the raw residuals array to an OutputMap, allowing easy assignment
+    auto residuals = ResultOut<M::Size>{raw_residuals};
 
-    // Call a helper function. We need to do this to expand the index sequence
-    // which we generate here. This approach is explained above.
-    auto ok = internal::callEvaluate<Factor<M, V...>::FuncType, M::Size, V...>(
+    // Call a helper function, specialized for this factor's type. This helps
+    // by expanding the index sequence we create here (see `index_sequence`).
+    bool ok = internal::callEvaluate<M, V...>(
       this->measurement_function,
       parameters,
-      results,
+      residuals,
       jacobians,
       internal::make_index_sequence<sizeof...(V)>());
 
     if (ok) {
         // Calculate the normalized residual
-        const auto &L = this->measurement.noise.inverseSqrtCov();
-        results = L * (results - this->measurement);
+        const auto &L = measurement.noise.inverseSqrtCov();
+        residuals = L * (residuals - measurement);
     }
+
     return ok;
 }
 
@@ -163,17 +166,4 @@ void Factor<M, V...>::print(std::ostream &os) const {
     os << "]";
 }
 
-template <typename M, typename... V>
-void Factor<M, V...>::setVariablesFixed() {
-    for (const auto ptr : this->variable_ptrs) {
-        if (ptr->isFixed()) {
-            throw std::runtime_error(
-              "Factor::setVariablesFixed(): Variable was already fixed."
-              " This means there are conflicting ideal measurements.");
-        }
-        ptr->setFixed(true);
-    }
-}
-
-
 }  // namespace wave
diff --git a/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp b/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
index 594e5a70..7136c8c5 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
@@ -1,3 +1,5 @@
+#include "wave/optimization/factor_graph/FactorMeasurement.hpp"
+
 namespace wave {
 
 namespace internal {
@@ -61,6 +63,16 @@ inline void FactorGraph::addPrior(
       std::move(variable));
 }
 
+template <typename VarType>
+inline void FactorGraph::addPerfectPrior(
+  const typename VarType::MappedType &measured_value,
+  std::shared_ptr<VarType> variable) {
+    using MeasType = FactorMeasurement<typename VarType::ViewType, void>;
+
+    this->factors.emplace_back(std::make_shared<PerfectPrior<VarType>>(
+      MeasType{measured_value}, std::move(variable)));
+}
+
 // Iterators
 
 inline typename FactorGraph::iterator FactorGraph::begin() noexcept {
diff --git a/wave_optimization/tests/factor_graph/factor_test.cpp b/wave_optimization/tests/factor_graph/factor_test.cpp
index c7388753..99ffe945 100644
--- a/wave_optimization/tests/factor_graph/factor_test.cpp
+++ b/wave_optimization/tests/factor_graph/factor_test.cpp
@@ -91,31 +91,32 @@ TEST(FactorTest, evaluateCostFunction) {
       VectorsNear, expected_jac_landmark, Eigen::Map<Vec2>{out_jac_landmark});
 }
 
-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, 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);
+//}
 
 }  // namespace wave
diff --git a/wave_optimization/tests/factor_graph/graph_test.cpp b/wave_optimization/tests/factor_graph/graph_test.cpp
index caa8cbdd..204441e6 100644
--- a/wave_optimization/tests/factor_graph/graph_test.cpp
+++ b/wave_optimization/tests/factor_graph/graph_test.cpp
@@ -53,6 +53,8 @@ TEST(FactorGraph, addPrior) {
 
     // Add the factor, retrieve the pointer to it, and verity
     graph.addPrior(m, p);
+    EXPECT_EQ(1u, graph.countFactors());
+
     auto factor = *graph.begin();
     ASSERT_NE(nullptr, factor);
 
@@ -80,6 +82,8 @@ TEST(FactorGraph, addPriorOfSize1) {
 
     // Add the factor, retrieve the pointer to it, and verity
     graph.addPrior(m, p);
+    EXPECT_EQ(1u, graph.countFactors());
+
     auto factor = *graph.begin();
     ASSERT_NE(nullptr, factor);
 
@@ -88,6 +92,36 @@ TEST(FactorGraph, addPriorOfSize1) {
     EXPECT_DOUBLE_EQ(1.0, test_jac);
 }
 
+TEST(FactorGraph, addPerfectPrior) {
+    const auto test_meas = Vec2{1.2, 3.4};
+    const auto test_val = Vec2{1.23, 3.38};
+
+    // Prepare arguments to add unary factor
+    FactorGraph graph;
+    auto p = std::make_shared<Landmark2DVar>();
+
+    // Prepare arguments in a form matching ceres calls
+    Vec2 test_residual;
+    Mat2 test_jac;
+    const double *const params[] = {test_val.data()};
+    double *jacs[] = {test_jac.data()};
+
+    // Add the factor
+    graph.addPerfectPrior(test_meas, p);
+    EXPECT_EQ(1u, graph.countFactors());
+
+    // The variable should now be marked constant
+    //    EXPECT_TRUE(p->isFixed());
+
+    // Retrieve a pointer to the factor we just (indirectly) added
+    auto factor = *graph.begin();
+    ASSERT_NE(nullptr, factor);
+
+    // 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, triangulationSim) {
     // Simulate a robot passing by a landmark
     // First generate "true" data
@@ -101,12 +135,16 @@ TEST(FactorGraph, triangulationSim) {
     // Construct variables and add them to the graph
     auto landmark_vars = std::vector<std::shared_ptr<Landmark2DVar>>{};
     auto pose_vars = std::vector<std::shared_ptr<Pose2DVar>>{};
+
     FactorGraph graph;
-    for (const auto &l : true_l_pos) {
-        landmark_vars.push_back(std::make_shared<Landmark2DVar>(l));
+
+    for (const auto &l_pos : true_l_pos) {
+        auto l = std::make_shared<Landmark2DVar>();
+        landmark_vars.push_back(l);
         // For now, landmark positions are exactly known.
-        landmark_vars.back()->setFixed(true);
+        graph.addPerfectPrior(l_pos, l);
     }
+
     for (const auto &pose : true_poses) {
         // Make uninitialized Variable
         auto p = std::make_shared<Pose2DVar>();
@@ -119,7 +157,8 @@ TEST(FactorGraph, triangulationSim) {
         }
     }
 
-    EXPECT_EQ(true_poses.size() * true_l_pos.size(), graph.countFactors());
+    // Expect one factor for each measurement, plus the three priors
+    EXPECT_EQ(3 + true_poses.size() * true_l_pos.size(), graph.countFactors());
 
     // Fill the variables with values
     evaluateGraph(graph);
diff --git a/wave_optimization/tests/factor_graph/noise_test.cpp b/wave_optimization/tests/factor_graph/noise_test.cpp
index 45555509..28c30619 100644
--- a/wave_optimization/tests/factor_graph/noise_test.cpp
+++ b/wave_optimization/tests/factor_graph/noise_test.cpp
@@ -43,14 +43,4 @@ TEST(NoiseTest, fullNoise) {
     EXPECT_PRED2(MatricesNear, expected_inv, res);
 }
 
-TEST(NoiseTest, zeroNoise) {
-    static_assert(std::is_empty<ZeroNoise>::value,
-                  "ZeroNoise expected to be have no data members.");
-
-    auto n = ZeroNoise{};
-
-    EXPECT_THROW(n.covariance(), std::logic_error);
-    EXPECT_THROW(n.inverseSqrtCov(), std::logic_error);
-}
-
 }  // namespace wave
diff --git a/wave_optimization/tests/factor_graph/variable_test.cpp b/wave_optimization/tests/factor_graph/variable_test.cpp
index 267c5b14..d68881c6 100644
--- a/wave_optimization/tests/factor_graph/variable_test.cpp
+++ b/wave_optimization/tests/factor_graph/variable_test.cpp
@@ -87,19 +87,6 @@ TEST(VariableTest, moveAssign) {
     EXPECT_NE(a.data(), b.data());
 }
 
-TEST(VariableTest, setFixed) {
-    // Construct and initialize a variable
-    auto var = FactorVariable<ValueView<2>>{Vec2{1.1, 2.2}};
-
-    EXPECT_FALSE(var.isFixed());
-
-    var.setFixed(true);
-    EXPECT_TRUE(var.isFixed());
-
-    var.setFixed(false);
-    EXPECT_FALSE(var.isFixed());
-}
-
 TEST(MeasurementTest, constructFromRvalue) {
     auto meas = FactorMeasurement<ValueView<2>>{Vec2{1.1, 2.2}, Vec2{0.1, 0.2}};
     const auto expected_val = Vec2{1.1, 2.2};

From 268bb260c1dfcb04b838496af233d4c8a69a212e Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Sun, 18 Jun 2017 00:41:39 -0400
Subject: [PATCH 5/9] Move FactorCostFunction to graph_wrapper, not Factor

---
 .../wave/optimization/ceres/graph_wrapper.hpp | 38 +++++++++++++++----
 .../wave/optimization/factor_graph/Factor.hpp | 36 +-----------------
 .../optimization/factor_graph/FactorBase.hpp  |  6 ++-
 .../tests/factor_graph/factor_test.cpp        | 32 ----------------
 4 files changed, 37 insertions(+), 75 deletions(-)

diff --git a/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp b/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp
index dae847e5..55e38708 100644
--- a/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp
+++ b/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp
@@ -17,13 +17,37 @@ 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
@@ -32,15 +56,15 @@ void addFactorToProblem(ceres::Problem &problem, FactorBase &factor) {
         problem.AddParameterBlock(v->data(), v->size());
 
         // Set parameter blocks constant if the factor is a zero-noise prior
-        if (factor.isPerfectPrior()) {
+        if (factor->isPerfectPrior()) {
             problem.SetParameterBlockConstant(v->data());
         }
     }
 
-    // Give ceres the cost function and its parameter blocks.
-    if (!factor.isPerfectPrior()) {
+    // Finally, give ceres the cost function and its parameter blocks.
+    if (!factor->isPerfectPrior()) {
         problem.AddResidualBlock(
-          factor.costFunction().release(), nullptr, data_ptrs);
+          new FactorCostFunction{std::move(factor)}, nullptr, data_ptrs);
     }
 }
 
@@ -54,7 +78,7 @@ void evaluateGraph(FactorGraph &graph) {
     ceres::Problem problem;
 
     for (const auto &ptr : graph) {
-        addFactorToProblem(problem, *ptr);
+        addFactorToProblem(problem, ptr);
     }
 
     // Initialize the solver
diff --git a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
index 63aa8708..91edddc0 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
@@ -20,24 +20,6 @@ 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.
  *
@@ -109,20 +91,10 @@ class Factor : public FactorBase {
     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::Size...>{fn}};
-    }
-
     bool evaluateRaw(double const *const *parameters,
                      double *residuals,
                      double **jacobians) const noexcept override;
@@ -178,14 +150,10 @@ class PerfectPrior : public FactorBase {
         return NumVars;
     }
 
-    int numResiduals() const override {
+    int residualSize() const override {
         return ResidualSize;
     }
 
-    std::unique_ptr<ceres::CostFunction> costFunction() override {
-        return nullptr;
-    }
-
     bool evaluateRaw(double const *const *, double *, double **) const
       noexcept override {
         return false;
diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp
index 1d379027..a5ae3c86 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorBase.hpp
@@ -44,9 +44,11 @@ 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;
diff --git a/wave_optimization/tests/factor_graph/factor_test.cpp b/wave_optimization/tests/factor_graph/factor_test.cpp
index 99ffe945..e6f5ce82 100644
--- a/wave_optimization/tests/factor_graph/factor_test.cpp
+++ b/wave_optimization/tests/factor_graph/factor_test.cpp
@@ -59,38 +59,6 @@ TEST(FactorTest, print) {
     EXPECT_EQ(expected.str(), ss.str());
 }
 
-TEST(FactorTest, evaluateCostFunction) {
-    auto meas = DistanceMeasurement{1.23, 1.0};
-    DistanceToLandmarkFactor f{
-      meas, std::make_shared<Pose2DVar>(), std::make_shared<Landmark2DVar>()};
-
-    // Prepare sample C-style arrays as used by Ceres
-    const double param_pose[3] = {1.1, 2.2, 3.3};
-    const double param_landmark[2] = {4.4, 5.5};
-    const double *const parameters[2] = {param_pose, param_landmark};
-    double out_residuals[1];
-    double out_jac_pose[3];
-    double out_jac_landmark[2];
-    double *out_jacobians[2] = {out_jac_pose, out_jac_landmark};
-
-    // Calculate expected values (use analytic jacobians)
-    auto dist =
-      std::sqrt((1.1 - 4.4) * (1.1 - 4.4) + (2.2 - 5.5) * (2.2 - 5.5));
-    auto expected_residual = dist - 1.23;
-    Vec3 expected_jac_pose{(1.1 - 4.4) / dist, (2.2 - 5.5) / dist, 0};
-    Vec2 expected_jac_landmark{(1.1 - 4.4) / dist, (2.2 - 5.5) / dist};
-
-    // Call and compare
-    auto p_costfn = f.costFunction();
-    auto res = p_costfn->Evaluate(parameters, out_residuals, out_jacobians);
-    EXPECT_TRUE(res);
-    EXPECT_DOUBLE_EQ(expected_residual, out_residuals[0]);
-    EXPECT_PRED2(
-      VectorsNear, expected_jac_pose, Eigen::Map<Vec3>{out_jac_pose});
-    EXPECT_PRED2(
-      VectorsNear, expected_jac_landmark, Eigen::Map<Vec2>{out_jac_landmark});
-}
-
 // TEST(FactorTest, idealMeasurement) {
 //    using MeasType = FactorMeasurement<double, ZeroNoise>;
 //    using VarType = FactorVariable<double>;

From 137c76e20b140975f8bf232a2acadb5ee768b6bb Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Sun, 18 Jun 2017 01:32:43 -0400
Subject: [PATCH 6/9] Separate out PerfectPrior.hpp

Also:
- Fix immediate assignment of variable values
---
 .../wave/optimization/factor_graph/Factor.hpp | 65 +------------
 .../optimization/factor_graph/OutputMap.hpp   |  2 +-
 .../factor_graph/PerfectPrior.hpp             | 95 +++++++++++++++++++
 .../optimization/factor_graph/ValueView.hpp   |  6 ++
 .../factor_graph/impl/FactorGraph.hpp         |  1 +
 .../tests/factor_graph/example_instances.hpp  |  9 +-
 .../tests/factor_graph/factor_test.cpp        | 41 +++-----
 7 files changed, 126 insertions(+), 93 deletions(-)
 create mode 100644 wave_optimization/include/wave/optimization/factor_graph/PerfectPrior.hpp

diff --git a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
index 91edddc0..8d69f621 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Factor.hpp
+++ b/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
diff --git a/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp b/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp
index 8ca69293..df7b31e5 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp
+++ b/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;
     }
 
diff --git a/wave_optimization/include/wave/optimization/factor_graph/PerfectPrior.hpp b/wave_optimization/include/wave/optimization/factor_graph/PerfectPrior.hpp
new file mode 100644
index 00000000..8e689e1b
--- /dev/null
+++ b/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
diff --git a/wave_optimization/include/wave/optimization/factor_graph/ValueView.hpp b/wave_optimization/include/wave/optimization/factor_graph/ValueView.hpp
index 255a901e..95524c7a 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/ValueView.hpp
+++ b/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;
diff --git a/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp b/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
index 7136c8c5..4b3d1ba5 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/impl/FactorGraph.hpp
+++ b/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 {
 
diff --git a/wave_optimization/tests/factor_graph/example_instances.hpp b/wave_optimization/tests/factor_graph/example_instances.hpp
index 870228e2..1842e2c0 100644
--- a/wave_optimization/tests/factor_graph/example_instances.hpp
+++ b/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};
 };
diff --git a/wave_optimization/tests/factor_graph/factor_test.cpp b/wave_optimization/tests/factor_graph/factor_test.cpp
index e6f5ce82..69929120 100644
--- a/wave_optimization/tests/factor_graph/factor_test.cpp
+++ b/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

From f7bb840c7938a18f5b8785c5bd131cd5f11b0e72 Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Sun, 18 Jun 2017 14:55:51 -0400
Subject: [PATCH 7/9] Update tests for factors and noise

Also:
- Noise: pre-calculate inverse covariance, add docs
---
 .../wave/optimization/factor_graph/Noise.hpp  | 52 ++++++++-----
 .../factor_graph/PerfectPrior.hpp             |  6 +-
 .../tests/factor_graph/factor_test.cpp        | 75 ++++++++++++++++---
 .../tests/factor_graph/graph_test.cpp         | 39 +++++++++-
 .../tests/factor_graph/noise_test.cpp         |  6 +-
 5 files changed, 143 insertions(+), 35 deletions(-)

diff --git a/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
index eac20111..5b3d7ded 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/Noise.hpp
@@ -24,13 +24,20 @@ 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 {
@@ -38,7 +45,8 @@ class FullNoise {
     };
 
  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;
 };
diff --git a/wave_optimization/include/wave/optimization/factor_graph/PerfectPrior.hpp b/wave_optimization/include/wave/optimization/factor_graph/PerfectPrior.hpp
index 8e689e1b..3c20e2ad 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/PerfectPrior.hpp
+++ b/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 */
diff --git a/wave_optimization/tests/factor_graph/factor_test.cpp b/wave_optimization/tests/factor_graph/factor_test.cpp
index 69929120..9575aa40 100644
--- a/wave_optimization/tests/factor_graph/factor_test.cpp
+++ b/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
diff --git a/wave_optimization/tests/factor_graph/graph_test.cpp b/wave_optimization/tests/factor_graph/graph_test.cpp
index 204441e6..225fa77b 100644
--- a/wave_optimization/tests/factor_graph/graph_test.cpp
+++ b/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
diff --git a/wave_optimization/tests/factor_graph/noise_test.cpp b/wave_optimization/tests/factor_graph/noise_test.cpp
index 28c30619..49425164 100644
--- a/wave_optimization/tests/factor_graph/noise_test.cpp
+++ b/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,7 +19,7 @@ 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());
@@ -27,7 +27,7 @@ TEST(NoiseTest, singleNoise) {
 }
 
 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};

From 72d672a0cbfb8192d35162d1e5fd26355f70f931 Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Sun, 18 Jun 2017 15:31:18 -0400
Subject: [PATCH 8/9] Fix build error with Eigen 3.2

---
 .../wave/optimization/factor_graph/FactorMeasurement.hpp    | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
index ea9294c7..7aa7615f 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/FactorMeasurement.hpp
@@ -8,6 +8,7 @@
 
 #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
@@ -77,9 +78,10 @@ class FactorMeasurement<V, void> : public FactorVariable<V> {
 
 template <typename V, typename N>
 inline Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1> operator-(
-  const Eigen::Ref<Eigen::Matrix<double, FactorMeasurement<V, N>::Size, 1>>
-    &lhs,
+  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>>{

From ec6ca64e82d3a196a185a2f1ed88bf4ab68b0d9e Mon Sep 17 00:00:00 2001
From: Leo Koppel <lkoppel@uwaterloo.ca>
Date: Thu, 22 Jun 2017 16:56:56 -0400
Subject: [PATCH 9/9] Normalize Jacobian as well

The solver relies on the reported jacobian being the actual Jacobian of what
it's given, not something scaled, so we have to multiply it as well.
---
 .../optimization/factor_graph/impl/Factor.hpp | 37 +++++++++++++++++--
 .../tests/factor_graph/factor_test.cpp        |  3 +-
 .../tests/factor_graph/graph_test.cpp         |  9 +++--
 3 files changed, 42 insertions(+), 7 deletions(-)

diff --git a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
index 63916ce3..d8a81007 100644
--- a/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
+++ b/wave_optimization/include/wave/optimization/factor_graph/impl/Factor.hpp
@@ -115,6 +115,34 @@ bool callEvaluate(typename Factor<M, V...>::FuncType measurement_function,
       make_jacobian<M::Size, V::Size, Is>(jacobians)...);
 }
 
+template <typename T, int R, int C>
+void multiplyJacobian(const T &L, JacobianOut<R, C> jac) {
+    if (jac) {
+        jac = L * jac;
+    }
+};
+
+/** Calls Factor::evaluate() with the necessary arguments generated from the
+ * Factor type (known at compile time) and the provided array pointers (known
+ * at run time).
+ *
+ * Calling a separate function expands the given index sequence - this approach
+ * is explained in the documentation for `index_sequence`.
+ */
+template <typename M, typename... V, int... Is>
+void normalize(const M &measurement,
+               ResultOut<M::Size> &residuals,
+               double **jacobians,
+               index_sequence<Is...>) noexcept {
+    // Calculate the normalized residual
+    const auto &L = measurement.noise.inverseSqrtCov();
+    residuals = L * (residuals - measurement);
+    // Call multiplyJacobian for each jacobian pointer
+    auto loop = {
+      (multiplyJacobian(L, make_jacobian<M::Size, V::Size, Is>(jacobians)),
+       0)...};
+    (void) loop;  // ignore unused variable warning
+}
 }  // namespace internal
 
 template <typename M, typename... V>
@@ -142,10 +170,13 @@ bool Factor<M, V...>::evaluateRaw(double const *const *parameters,
       jacobians,
       internal::make_index_sequence<sizeof...(V)>());
 
+
     if (ok) {
-        // Calculate the normalized residual
-        const auto &L = measurement.noise.inverseSqrtCov();
-        residuals = L * (residuals - measurement);
+        internal::normalize<M, V...>(
+          this->measurement,
+          residuals,
+          jacobians,
+          internal::make_index_sequence<sizeof...(V)>());
     }
 
     return ok;
diff --git a/wave_optimization/tests/factor_graph/factor_test.cpp b/wave_optimization/tests/factor_graph/factor_test.cpp
index 9575aa40..c5fa913e 100644
--- a/wave_optimization/tests/factor_graph/factor_test.cpp
+++ b/wave_optimization/tests/factor_graph/factor_test.cpp
@@ -79,7 +79,8 @@ TEST(FactorTest, evaluateSize1) {
     // 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);
+    // We expect the jacobian to be normalized as well
+    EXPECT_DOUBLE_EQ(-1.1 / meas_stddev, test_jac);
 }
 
 
diff --git a/wave_optimization/tests/factor_graph/graph_test.cpp b/wave_optimization/tests/factor_graph/graph_test.cpp
index 225fa77b..6eed5e86 100644
--- a/wave_optimization/tests/factor_graph/graph_test.cpp
+++ b/wave_optimization/tests/factor_graph/graph_test.cpp
@@ -37,8 +37,9 @@ TEST(FactorGraph, addPrior) {
     const auto test_meas = Vec2{1.2, 3.4};
     const auto test_stddev = Vec2{0.01, 0.01};
     const auto test_val = Vec2{1.23, 3.38};
-    // The expected residuals are normalized
+    // The expected results are normalized
     const Vec2 expected_res = (test_val - test_meas).cwiseQuotient(test_stddev);
+    const Mat2 expected_jac = Mat2::Identity() / 0.01;
 
     // Prepare arguments to add unary factor
     FactorGraph graph;
@@ -60,14 +61,16 @@ TEST(FactorGraph, addPrior) {
 
     EXPECT_TRUE(factor->evaluateRaw(params, test_residual.data(), jacs));
     EXPECT_PRED2(VectorsNear, expected_res, test_residual);
-    EXPECT_PRED2(MatricesNear, Mat2::Identity(), test_jac);
+    EXPECT_PRED2(MatricesNear, expected_jac, test_jac);
 }
 
 TEST(FactorGraph, addPriorOfSize1) {
     const auto test_meas = 1.2;
     const auto test_stddev = 0.01;
     const auto test_val = 1.23;
+    // The expected results are normalized
     const auto expected_res = (test_val - test_meas) / test_stddev;
+    const auto expected_jac = 1.0 / test_stddev;
 
     // Prepare arguments to add unary factor
     FactorGraph graph;
@@ -89,7 +92,7 @@ TEST(FactorGraph, addPriorOfSize1) {
 
     EXPECT_TRUE(factor->evaluateRaw(params, &test_residual, jacs));
     EXPECT_DOUBLE_EQ(expected_res, test_residual);
-    EXPECT_DOUBLE_EQ(1.0, test_jac);
+    EXPECT_DOUBLE_EQ(expected_jac, test_jac);
 }
 
 TEST(FactorGraph, addPerfectPrior) {