Add 2D graph slam example

wave_optimization/CMakeLists.txt

@@ -25,3 +25,11 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME}_tests
                       ${PROJECT_NAME}
                       wave_utils
                       ${CERES_LIBRARIES})
+
+WAVE_ADD_TEST(${PROJECT_NAME}_examples
+    tests/factor_graph/graph_slam_2d.cpp)
+
+TARGET_LINK_LIBRARIES(${PROJECT_NAME}_examples
+    ${PROJECT_NAME}
+    wave_kinematics
+    ${CERES_LIBRARIES})

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

@@ -89,6 +89,7 @@ void evaluateGraph(FactorGraph &graph) {
     // Solve the problem, and write the estimated values to the variables
     // in the graph
     ceres::Solve(options, &problem, &summary);
+    std::cout << summary.FullReport() << std::endl;
 }
 
 /** @} group optimization */

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

@@ -64,7 +64,8 @@ class OutputMap : public Eigen::Map<T> {
 
 /** Type of jacobian output parameters for `Factor::evaluate()`. */
 template <int Rows, int Cols>
-using JacobianOut = OutputMap<Eigen::Matrix<double, Rows, Cols>>;
+using JacobianOut =
+  OutputMap<Eigen::Matrix<double, Rows, Cols, Eigen::RowMajor>>;
 
 /** Type of measurement output parameter for `Factor::evaluate()`. */
 template <int Size>

wave_optimization/tests/factor_graph/example_instances.hpp

@@ -36,10 +36,9 @@ struct Pose2D : public ValueView<3> {
         this->ValueView<3>::operator=(other);
         return *this;
     }
-    using Vec1 = Eigen::Matrix<double, 1, 1>;
 
     Eigen::Map<const Vec2> position{dataptr};
-    Eigen::Map<const Vec1> orientation{dataptr + 2};
+    double &orientation{*(dataptr + 2)};
 };
 
 /**
@@ -62,6 +61,22 @@ struct Landmark2D : public ValueView<2> {
     Eigen::Map<const Vec2> position{dataptr};
 };
 
+/*
+ * Combined distance and angle observation
+ */
+struct RangeBearing : public ValueView<2> {
+    explicit RangeBearing(double *d) : ValueView<2>{d} {}
+    explicit RangeBearing(MappedType &m) : ValueView<2>{m} {}
+    RangeBearing &operator=(const RangeBearing &other) {
+        this->ValueView<2>::operator=(other);
+        return *this;
+    }
+
+    double &range{*dataptr};
+    double &bearing{*(dataptr + 1)};
+};
+
+
 /** Define variable types for each value type */
 using Pose2DVar = FactorVariable<Pose2D>;
 using Landmark2DVar = FactorVariable<Landmark2D>;
@@ -116,6 +131,37 @@ class DistanceToLandmarkFactor
             distanceMeasurementFunction, meas, std::move(p), std::move(l)} {}
 };
 
+
+/** Measurement function giving a distance and orientation
+ */
+inline bool measureRangeBearing(const Pose2D &pose,
+                                const Landmark2D &landmark,
+                                ResultOut<2> result,
+                                JacobianOut<2, 3> j_pose,
+                                JacobianOut<2, 2> j_landmark) noexcept {
+    Vec2 diff = landmark.position - pose.position;
+    double distance = diff.norm();
+    double bearing = atan2(diff.y(), diff.x()) - pose.orientation;
+
+    result[0] = distance;
+    result[1] = bearing;
+
+    // For each jacobian, check that optimizer requested it
+    // (just as you would check pointers from ceres)
+    if (j_pose) {
+        const auto d2 = distance * distance;
+        j_pose.row(0) << -diff.transpose() / distance, 0;
+        j_pose.row(1) << diff.y() / d2, -diff.x() / d2, -1;
+    }
+    if (j_landmark) {
+        const auto d2 = distance * distance;
+        j_landmark.row(0) << diff.transpose() / distance;
+        j_landmark.row(1) << -diff.y() / d2, diff.x() / d2;
+    }
+
+    return true;
+}
+
 /** @} group optimization */
 }  // namespace wave
 

wave_optimization/tests/factor_graph/graph_slam_2d.cpp

@@ -0,0 +1,147 @@
+#include "wave/wave_test.hpp"
+#include "wave/optimization/factor_graph/FactorGraph.hpp"
+#include "wave/optimization/ceres/graph_wrapper.hpp"
+#include "example_instances.hpp"
+
+namespace wave {
+
+/** Draw a single number from zero-mean Gaussian distribution */
+inline double gaussianNoise(double stddev) {
+    static std::default_random_engine generator{std::random_device{}()};
+    static std::normal_distribution<> normal{0, 1};
+
+    return stddev * normal(generator);
+}
+
+class GraphSlam2d : public ::testing::Test {
+    const int num_landmarks = 10;
+    const double circle_radius = 5;
+    const int num_steps = 5;
+    const double time_per_step = 0.05;
+
+ protected:
+    GraphSlam2d() {
+        // Generate landmark positions
+        for (auto i = 0; i < this->num_landmarks; ++i) {
+            auto pos = Vec2{7.5, i};
+            this->true_landmarks.emplace_back(pos);
+        }
+
+        // Simulate robot motion
+        for (auto i = 0; i < this->num_steps; ++i) {
+            const auto t = this->time_per_step * i;
+
+            // Generate ground truth circular path
+            const auto x = this->circle_radius * cos(t);
+            const auto y = this->circle_radius * sin(t);
+            const auto angle = M_PI_2 + atan2(y, x);
+
+            // Store ground truth
+            this->true_poses.emplace_back(Vec3{x, y, angle});
+
+            // Take noisy measurements of landmarks
+            this->takeMeasurements(this->true_poses.back().value);
+        }
+    }
+
+    // Store measurements of landmarks from the given pose
+    void takeMeasurements(const Pose2D &pose) {
+        this->measurements.emplace_back();
+
+        for (auto k = 0u; k < this->true_landmarks.size(); ++k) {
+            Vec2 result;
+            measureRangeBearing(pose,
+                                this->true_landmarks[k].value,
+                                ResultOut<2>{result.data()},
+                                JacobianOut<2, 3>{nullptr},
+                                JacobianOut<2, 2>{nullptr});
+
+            // Store a measurement for all landmarks for now
+            // Add noise
+            Vec2 stddev = Vec2{0.05, 0.01};
+            Vec2 noisy = result + stddev.unaryExpr(&gaussianNoise);
+            auto meas = FactorMeasurement<RangeBearing>{noisy, stddev};
+            this->measurements.back().emplace(k, meas);
+        }
+    }
+
+    // Make an estimate for a landmark position using the first pose it was
+    // measured from
+    Vec2 getLandmarkEstimate(const Pose2D &pose, const RangeBearing &meas) {
+        auto angle = pose.orientation + meas.bearing;
+        return pose.position + Vec2{cos(angle), sin(angle)};
+    }
+
+    // Ground truth landmark positions, held in zero-noise "measurements"
+    std::vector<FactorMeasurement<Landmark2D, void>> true_landmarks;
+
+    // Ground truth poses at each time step
+    std::vector<FactorMeasurement<Pose2D, void>> true_poses;
+
+    // Landmark measurements
+    // At each time step, there is a map from landmark index to measurements
+    std::vector<std::map<int, FactorMeasurement<RangeBearing>>> measurements;
+};
+
+TEST_F(GraphSlam2d, example) {
+    auto graph = FactorGraph{};
+
+    // Make variables for each pose and landmark
+    auto poses = std::vector<std::shared_ptr<Pose2DVar>>{};
+    auto landmarks = std::vector<std::shared_ptr<Landmark2DVar>>{};
+    for (auto i = 0u; i < this->true_poses.size(); ++i) {
+        // Initialize the variable with an estimated pose
+        // We need reasonable estimates for the algorithm to succeed, because
+        // the problem is not convex. Typically this might come from a motion
+        // model, but for now just use the noisy truth.
+        const auto initial_estimate =
+          this->true_poses[i].value.asVector() + 5 * Vec3::Random();
+        poses.push_back(std::make_shared<Pose2DVar>(initial_estimate));
+    }
+    for (auto i = 0u; i < this->true_landmarks.size(); ++i) {
+        // Initialize the variable with an estimated pose
+        // For now, get the estimate from the first measurement and the first
+        // pose estimate
+        const auto &meas = this->measurements[0].at(i).value;
+        auto estimate = this->getLandmarkEstimate(poses[0]->value, meas);
+        landmarks.push_back(std::make_shared<Landmark2DVar>(estimate));
+    }
+
+    // Add factors for each measurement
+    for (auto i = 0u; i < this->true_poses.size(); ++i) {
+        for (const auto &meas : this->measurements[i]) {
+            graph.addFactor(measureRangeBearing,
+                            meas.second,
+                            poses[i],
+                            landmarks[meas.first]);
+        }
+    }
+
+    // Add one more factor to constrain the first pose at the origin
+    graph.addPerfectPrior(this->true_poses[0].value.asVector(), poses[0]);
+
+    // Finally, evaluate the whole graph
+    evaluateGraph(graph);
+
+    // Check the results
+    // Note the tolerances are loose and arbitrary. This is not a strict test of
+    // the algorithm's accuracy, just a check that it's working at all.
+    for (auto i = 0u; i < this->true_poses.size(); ++i) {
+        const auto &truth = this->true_poses[i].value;
+        const auto &estimate = poses[i]->value;
+        EXPECT_PRED3(
+          VectorsNearWithPrec, truth.position, estimate.position, 0.1)
+          << "pose #" << i;
+        EXPECT_NEAR(truth.orientation, estimate.orientation, 0.04) << "pose #"
+                                                                   << i;
+    }
+
+    for (auto i = 0u; i < this->true_landmarks.size(); ++i) {
+        const auto &truth = this->true_landmarks[i].value.position;
+        const auto &estimate = landmarks[i]->value.position;
+        EXPECT_PRED3(VectorsNearWithPrec, truth, estimate, 0.1) << "landmark #"
+                                                                << i;
+    }
+}
+
+}  // namespace wave

wave_optimization/tests/factor_graph/variable_test.cpp

@@ -95,4 +95,16 @@ TEST(MeasurementTest, constructFromRvalue) {
       VectorsNear, expected_val, Eigen::Map<Vec2>{meas.value.data()});
 }
 
+TEST(OutputMapTest, jacobianMap) {
+    // A raw matrix stored in row-major order (as used by ceres)
+    double buf[] = {1, 2, 3, 4, 5, 6, 7, 8};
+
+    auto map = JacobianOut<2, 4>{buf};
+
+    EXPECT_DOUBLE_EQ(5, map(1, 0));
+
+    map(1, 0) = 9.9;
+    EXPECT_DOUBLE_EQ(9.9, buf[4]);
+}
+
 }  // namespace wave

wave_utils/include/wave/wave_test.hpp

@@ -17,12 +17,37 @@ inline bool VectorsNear(const VecX &v1, const VecX &v2) {
     return v1.isApprox(v2);
 }
 
+
+/** Predicate to check if vectors are equal within the given precision.
+ * Use with EXPECT_PRED3
+ * @note The comparison is multiplicative, and `prec` is not a linear tolerance.
+ * See the documentation for Eigen's `isApprox()`. Specifically, `isApprox(a,b)`
+ * checks for:
+ * @f[ \| a - b \| \leq p \min(\| a\|, \| b\|) @f]
+ * where @f$ p @f$ is the given precision.
+ */
+inline bool VectorsNearWithPrec(const VecX &v1, const VecX &v2, double prec) {
+    return v1.isApprox(v2, prec);
+}
+
 /** Predicate to check if matrices are approximately equal.
  * Use with EXPECT_PRED2 */
 inline bool MatricesNear(const MatX &m1, const MatX &m2) {
     return m1.isApprox(m2);
 }
 
+/** Predicate to check if matrices are equal within the given precision.
+ * Use with EXPECT_PRED3
+ * @note The comparison is multiplicative, and `prec` is not a linear tolerance.
+ * See the documentation for Eigen's `isApprox()`. Specifically, `isApprox(a,b)`
+ * checks for:
+ * @f[ \| a - b \| \leq p \min(\| a\|, \| b\|) @f]
+ * where @f$ p @f$ is the given precision.
+ */
+inline bool MatricesNearWithPrec(const MatX &m1, const MatX &m2, double prec) {
+    return m1.isApprox(m2, prec);
+}
+
 }  // namespace wave
 
 #endif  // WAVE_TEST_HPP