From aa82c8b10353daf3460e62867aedf9226a89bb89 Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Thu, 22 Jun 2017 12:45:30 -0400 Subject: [PATCH 1/3] Use row-major order for output map Ceres matrices are in row-major order. --- .../wave/optimization/factor_graph/OutputMap.hpp | 3 ++- .../tests/factor_graph/variable_test.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp b/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp index df7b31e5..0e0d6124 100644 --- a/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp +++ b/wave_optimization/include/wave/optimization/factor_graph/OutputMap.hpp @@ -64,7 +64,8 @@ class OutputMap : public Eigen::Map { /** Type of jacobian output parameters for `Factor::evaluate()`. */ template -using JacobianOut = OutputMap>; +using JacobianOut = + OutputMap>; /** Type of measurement output parameter for `Factor::evaluate()`. */ template diff --git a/wave_optimization/tests/factor_graph/variable_test.cpp b/wave_optimization/tests/factor_graph/variable_test.cpp index d68881c6..d661712f 100644 --- a/wave_optimization/tests/factor_graph/variable_test.cpp +++ b/wave_optimization/tests/factor_graph/variable_test.cpp @@ -95,4 +95,16 @@ TEST(MeasurementTest, constructFromRvalue) { VectorsNear, expected_val, Eigen::Map{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 From d96bf80e2289ff43c3c84dcd651a5e10fb625f1a Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Fri, 23 Jun 2017 00:48:38 -0400 Subject: [PATCH 2/3] Add 2D graph slam example This example supposes a robot has 2D distance and bearing measurements to a set of landmarks. --- wave_optimization/CMakeLists.txt | 8 ++ .../tests/factor_graph/example_instances.hpp | 47 +++++++ .../tests/factor_graph/graph_slam_2d.cpp | 132 ++++++++++++++++++ wave_utils/include/wave/wave_test.hpp | 25 ++++ 4 files changed, 212 insertions(+) create mode 100644 wave_optimization/tests/factor_graph/graph_slam_2d.cpp diff --git a/wave_optimization/CMakeLists.txt b/wave_optimization/CMakeLists.txt index 8b92f597..355a48b4 100644 --- a/wave_optimization/CMakeLists.txt +++ b/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}) diff --git a/wave_optimization/tests/factor_graph/example_instances.hpp b/wave_optimization/tests/factor_graph/example_instances.hpp index 1842e2c0..d0cc7f88 100644 --- a/wave_optimization/tests/factor_graph/example_instances.hpp +++ b/wave_optimization/tests/factor_graph/example_instances.hpp @@ -62,6 +62,22 @@ struct Landmark2D : public ValueView<2> { Eigen::Map 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; using Landmark2DVar = FactorVariable; @@ -116,6 +132,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.value(); + + 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 diff --git a/wave_optimization/tests/factor_graph/graph_slam_2d.cpp b/wave_optimization/tests/factor_graph/graph_slam_2d.cpp new file mode 100644 index 00000000..7cab1600 --- /dev/null +++ b/wave_optimization/tests/factor_graph/graph_slam_2d.cpp @@ -0,0 +1,132 @@ +#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{noisy, stddev}; + this->measurements.back().emplace(k, meas); + } + } + + // Ground truth landmark positions, held in zero-noise "measurements" + std::vector> true_landmarks; + + // Ground truth poses at each time step + std::vector> true_poses; + + // Landmark measurements + // At each time step, there is a map from landmark index to measurements + std::vector>> measurements; +}; + +TEST_F(GraphSlam2d, example) { + auto graph = FactorGraph{}; + + // Make variables for each pose and landmark + auto poses = std::vector>{}; + auto landmarks = std::vector>{}; + for (auto i = 0u; i < this->true_poses.size(); ++i) { + poses.push_back(std::make_shared()); + } + for (auto i = 0u; i < this->true_landmarks.size(); ++i) { + // @todo: the variables are still initialized to zero, and the distance + // measurement has undefined jacobian singular at 0... manually + // initialize them non-zero for now. + landmarks.push_back(std::make_shared(Vec2{1., 1.})); + } + + // 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[0], estimate.orientation[0], 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 diff --git a/wave_utils/include/wave/wave_test.hpp b/wave_utils/include/wave/wave_test.hpp index 8569a415..d7901775 100644 --- a/wave_utils/include/wave/wave_test.hpp +++ b/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 From 4df65d809bfec64f5ee0b7df8adefac938f6a2e0 Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Fri, 23 Jun 2017 11:59:57 -0400 Subject: [PATCH 3/3] Use random landmark positions and initial estimates --- .../wave/optimization/ceres/graph_wrapper.hpp | 1 + .../tests/factor_graph/example_instances.hpp | 5 ++-- .../tests/factor_graph/graph_slam_2d.cpp | 29 ++++++++++++++----- 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp b/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp index 55e38708..cd7aab95 100644 --- a/wave_optimization/include/wave/optimization/ceres/graph_wrapper.hpp +++ b/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 */ diff --git a/wave_optimization/tests/factor_graph/example_instances.hpp b/wave_optimization/tests/factor_graph/example_instances.hpp index d0cc7f88..e0b19ec3 100644 --- a/wave_optimization/tests/factor_graph/example_instances.hpp +++ b/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; Eigen::Map position{dataptr}; - Eigen::Map orientation{dataptr + 2}; + double &orientation{*(dataptr + 2)}; }; /** @@ -142,7 +141,7 @@ inline bool measureRangeBearing(const Pose2D &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.value(); + double bearing = atan2(diff.y(), diff.x()) - pose.orientation; result[0] = distance; result[1] = bearing; diff --git a/wave_optimization/tests/factor_graph/graph_slam_2d.cpp b/wave_optimization/tests/factor_graph/graph_slam_2d.cpp index 7cab1600..f2401a18 100644 --- a/wave_optimization/tests/factor_graph/graph_slam_2d.cpp +++ b/wave_optimization/tests/factor_graph/graph_slam_2d.cpp @@ -65,6 +65,13 @@ class GraphSlam2d : public ::testing::Test { } } + // 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> true_landmarks; @@ -83,13 +90,21 @@ TEST_F(GraphSlam2d, example) { auto poses = std::vector>{}; auto landmarks = std::vector>{}; for (auto i = 0u; i < this->true_poses.size(); ++i) { - poses.push_back(std::make_shared()); + // 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(initial_estimate)); } for (auto i = 0u; i < this->true_landmarks.size(); ++i) { - // @todo: the variables are still initialized to zero, and the distance - // measurement has undefined jacobian singular at 0... manually - // initialize them non-zero for now. - landmarks.push_back(std::make_shared(Vec2{1., 1.})); + // 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(estimate)); } // Add factors for each measurement @@ -117,8 +132,8 @@ TEST_F(GraphSlam2d, example) { EXPECT_PRED3( VectorsNearWithPrec, truth.position, estimate.position, 0.1) << "pose #" << i; - EXPECT_NEAR(truth.orientation[0], estimate.orientation[0], 0.04) - << "pose #" << i; + EXPECT_NEAR(truth.orientation, estimate.orientation, 0.04) << "pose #" + << i; } for (auto i = 0u; i < this->true_landmarks.size(); ++i) {