|
|
@@ -0,0 +1,316 @@
|
|
|
+// Ceres Solver - A fast non-linear least squares minimizer
|
|
|
+// Copyright 2014 Google Inc. All rights reserved.
|
|
|
+// http://code.google.com/p/ceres-solver/
|
|
|
+//
|
|
|
+// Redistribution and use in source and binary forms, with or without
|
|
|
+// modification, are permitted provided that the following conditions are met:
|
|
|
+//
|
|
|
+// * Redistributions of source code must retain the above copyright notice,
|
|
|
+// this list of conditions and the following disclaimer.
|
|
|
+// * Redistributions in binary form must reproduce the above copyright notice,
|
|
|
+// this list of conditions and the following disclaimer in the documentation
|
|
|
+// and/or other materials provided with the distribution.
|
|
|
+// * Neither the name of Google Inc. nor the names of its contributors may be
|
|
|
+// used to endorse or promote products derived from this software without
|
|
|
+// specific prior written permission.
|
|
|
+//
|
|
|
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
|
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
|
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
|
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
|
|
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
|
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
|
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
|
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
|
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
|
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
|
+// POSSIBILITY OF SUCH DAMAGE.
|
|
|
+//
|
|
|
+// Author: joydeepb@ri.cmu.edu (Joydeep Biswas)
|
|
|
+//
|
|
|
+// This example demonstrates how to use the DynamicAutoDiffCostFunction
|
|
|
+// variant of CostFunction. The DynamicAutoDiffCostFunction is meant to
|
|
|
+// be used in cases where the number of parameter blocks or the sizes are not
|
|
|
+// known at compile time.
|
|
|
+//
|
|
|
+// This example simulates a robot traversing down a 1-dimension hallway with
|
|
|
+// noise odometry readings and noisy range readings of the end of the hallway.
|
|
|
+// By fusing the noisy odometry and sensor readings this example demonstrates
|
|
|
+// how to compute the maximum likelihood estimate (MLE) of the robot's pose at
|
|
|
+// each timestep.
|
|
|
+//
|
|
|
+// The robot starts at the origin, and it is travels to the end of a corridor of
|
|
|
+// fixed length specified by the "--corridor_length" flag. It executes a series
|
|
|
+// of motion commands to move forward a fixed length, specified by the
|
|
|
+// "--pose_separation" flag, at which pose it receives relative odometry
|
|
|
+// measurements as well as a range reading of the distance to the end of the
|
|
|
+// hallway. The odometry readings are drawn with Gaussian noise and standard
|
|
|
+// deviation specified by the "--odometry_stddev" flag, and the range readings
|
|
|
+// similarly with standard deviation specified by the "--range-stddev" flag.
|
|
|
+//
|
|
|
+// There are two types of residuals in this problem:
|
|
|
+// 1) The OdometryConstraint residual, that accounts for the odometry readings
|
|
|
+// between successive pose estimatess of the robot.
|
|
|
+// 2) The RangeConstraint residual, that accounts for the errors in the observed
|
|
|
+// range readings from each pose.
|
|
|
+//
|
|
|
+// The OdometryConstraint residual is modeled as an AutoDiffCostFunction with
|
|
|
+// a fixed parameter block size of 1, which is the relative odometry being
|
|
|
+// solved for, between a pair of successive poses of the robot. Differences
|
|
|
+// between observed and computed relative odometry values are penalized weighted
|
|
|
+// by the known standard deviation of the odometry readings.
|
|
|
+//
|
|
|
+// The RangeConstraint residual is modeled as a DynamicAutoDiffCostFunction
|
|
|
+// which sums up the relative odometry estimates to compute the estimated
|
|
|
+// global pose of the robot, and then computes the expected range reading.
|
|
|
+// Differences between the observed and expected range readings are then
|
|
|
+// penalized weighted by the standard deviation of readings of the sensor.
|
|
|
+// Since the number of poses of the robot is not known at compile time, this
|
|
|
+// cost function is implemented as a DynamicAutoDiffCostFunction.
|
|
|
+//
|
|
|
+// The outputs of the example are the initial values of the odometry and range
|
|
|
+// readings, and the range and odometry errors for every pose of the robot.
|
|
|
+// After computing the MLE, the computed poses and corrected odometry values
|
|
|
+// are printed out, along with the corresponding range and odometry errors. Note
|
|
|
+// that as an MLE of a noisy system the errors will not be reduced to zero, but
|
|
|
+// the odometry estimates will be updated to maximize the joint likelihood of
|
|
|
+// all odometry and range readings of the robot.
|
|
|
+//
|
|
|
+// Mathematical Formulation
|
|
|
+// ======================================================
|
|
|
+//
|
|
|
+// Let p_0, .., p_N be (N+1) robot poses, where the robot moves down the
|
|
|
+// corridor starting from p_0 and ending at p_N. We assume that p_0 is the
|
|
|
+// origin of the coordinate system.
|
|
|
+// Odometry u_i is the observed relative odometry between pose p_(i-1) and p_i,
|
|
|
+// and range reading y_i is the range reading of the end of the corridor from
|
|
|
+// pose p_i. Both odometry as well as range readings are noisy, but we wish to
|
|
|
+// compute the maximum likelihood estimate (MLE) of corrected odometry values
|
|
|
+// u*_0 to u*_(N-1), such that the Belief is optimized:
|
|
|
+//
|
|
|
+// Belief(u*_(0:N-1) | u_(0:N-1), y_(0:N-1)) 1.
|
|
|
+// = P(u*_(0:N-1) | u_(0:N-1), y_(0:N-1)) 2.
|
|
|
+// \propto P(y_(0:N-1) | u*_(0:N-1), u_(0:N-1)) P(u*_(0:N-1) | u_(0:N-1)) 3.
|
|
|
+// = \prod_i{ P(y_i | u*_(0:i)) P(u*_i | u_i) } 4.
|
|
|
+//
|
|
|
+// Here, the subscript "(0:i)" is used as shorthand to indicate entries from all
|
|
|
+// timesteps 0 to i for that variable, both inclusive.
|
|
|
+//
|
|
|
+// Bayes' rule is used to derive eq. 3 from 2, and the independence of
|
|
|
+// odometry observations and range readings is expolited to derive 4 from 3.
|
|
|
+//
|
|
|
+// Thus, the Belief, up to scale, is factored as a product of a number of
|
|
|
+// terms, two for each pose, where for each pose term there is one term for the
|
|
|
+// range reading, P(y_i | u*_(0:i) and one term for the odometry reading,
|
|
|
+// P(u*_i | u_i) . Note that the term for the range reading is dependent on all
|
|
|
+// odometry values u*_(0:i), while the odometry term, P(u*_i | u_i) depends only
|
|
|
+// on a single value, u_i. Both the range reading as well as odoemtry
|
|
|
+// probability terms are modeled as the Normal distribution, and have the form:
|
|
|
+//
|
|
|
+// p(x) \propto \exp{-((x - x_mean) / x_stddev)^2}
|
|
|
+//
|
|
|
+// where x refers to either the MLE odometry u* or range reading y, and x_mean
|
|
|
+// is the corresponding mean value, u for the odometry terms, and y_expected,
|
|
|
+// the expected range reading based on all the previous odometry terms.
|
|
|
+// The MLE is thus found by finding those values x* which minimize:
|
|
|
+//
|
|
|
+// x* = \arg\min{((x - x_mean) / x_stddev)^2}
|
|
|
+//
|
|
|
+// which is in the nonlinear least-square form, suited to being solved by Ceres.
|
|
|
+// The non-linear component arise from the computation of x_mean. The residuals
|
|
|
+// ((x - x_mean) / x_stddev) for the residuals that Ceres will optimize. As
|
|
|
+// mentioned earlier, the odometry term for each pose depends only on one
|
|
|
+// variable, and will be computed by an AutoDiffCostFunction, while the term
|
|
|
+// for the range reading will depend on all previous odometry observations, and
|
|
|
+// will be computed by a DynamicAutoDiffCostFunction since the number of
|
|
|
+// odoemtry observations will only be known at run time.
|
|
|
+
|
|
|
+#include <cstdio>
|
|
|
+#include <math.h>
|
|
|
+#include <vector>
|
|
|
+
|
|
|
+#include "ceres/ceres.h"
|
|
|
+#include "ceres/dynamic_autodiff_cost_function.h"
|
|
|
+#include "gflags/gflags.h"
|
|
|
+#include "glog/logging.h"
|
|
|
+#include "random.h"
|
|
|
+
|
|
|
+using ceres::AutoDiffCostFunction;
|
|
|
+using ceres::DynamicAutoDiffCostFunction;
|
|
|
+using ceres::CauchyLoss;
|
|
|
+using ceres::CostFunction;
|
|
|
+using ceres::LossFunction;
|
|
|
+using ceres::Problem;
|
|
|
+using ceres::Solve;
|
|
|
+using ceres::Solver;
|
|
|
+using ceres::examples::RandNormal;
|
|
|
+using std::min;
|
|
|
+using std::vector;
|
|
|
+
|
|
|
+DEFINE_double(corridor_length, 30.0, "Length of the corridor that the robot is "
|
|
|
+ "travelling down.");
|
|
|
+
|
|
|
+DEFINE_double(pose_separation, 0.5, "The distance that the robot traverses "
|
|
|
+ "between successive odometry updates.");
|
|
|
+
|
|
|
+DEFINE_double(odometry_stddev, 0.1, "The standard deviation of "
|
|
|
+ "odometry error of the robot.");
|
|
|
+
|
|
|
+DEFINE_double(range_stddev, 0.01, "The standard deviation of range readings of "
|
|
|
+ "the robot.");
|
|
|
+
|
|
|
+// The stride length of the dynamic_autodiff_cost_function evaluator.
|
|
|
+static const int kStride = 10;
|
|
|
+
|
|
|
+struct OdometryConstraint {
|
|
|
+ typedef AutoDiffCostFunction<OdometryConstraint, 1, 1> OdometryCostFunction;
|
|
|
+
|
|
|
+ OdometryConstraint(double odometry_mean, double odometry_stddev) :
|
|
|
+ odometry_mean(odometry_mean), odometry_stddev(odometry_stddev) {}
|
|
|
+
|
|
|
+ template <typename T>
|
|
|
+ bool operator()(const T* const odometry, T* residual) const {
|
|
|
+ *residual = (*odometry - T(odometry_mean)) / T(odometry_stddev);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ static OdometryCostFunction* Create(const double odometry_value) {
|
|
|
+ return new OdometryCostFunction(
|
|
|
+ new OdometryConstraint(odometry_value, FLAGS_odometry_stddev));
|
|
|
+ }
|
|
|
+
|
|
|
+ const double odometry_mean;
|
|
|
+ const double odometry_stddev;
|
|
|
+};
|
|
|
+
|
|
|
+struct RangeConstraint {
|
|
|
+ typedef DynamicAutoDiffCostFunction<RangeConstraint, kStride>
|
|
|
+ RangeCostFunction;
|
|
|
+
|
|
|
+ RangeConstraint(
|
|
|
+ int pose_index,
|
|
|
+ double range_reading,
|
|
|
+ double range_stddev,
|
|
|
+ double corridor_length) :
|
|
|
+ pose_index(pose_index), range_reading(range_reading),
|
|
|
+ range_stddev(range_stddev), corridor_length(corridor_length) {}
|
|
|
+
|
|
|
+ template <typename T>
|
|
|
+ bool operator()(T const* const* relative_poses, T* residuals) const {
|
|
|
+ T global_pose(0);
|
|
|
+ for (int i = 0; i <= pose_index; ++i) {
|
|
|
+ global_pose += relative_poses[i][0];
|
|
|
+ }
|
|
|
+ residuals[0] = (global_pose + T(range_reading) - T(corridor_length)) /
|
|
|
+ T(range_stddev);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Factory method to create a CostFunction from a RangeConstraint to
|
|
|
+ // conveniently add to a ceres problem.
|
|
|
+ static RangeCostFunction* Create(const int pose_index,
|
|
|
+ const double range_reading,
|
|
|
+ vector<double>* odometry_values,
|
|
|
+ vector<double*>* parameter_blocks) {
|
|
|
+ RangeConstraint* constraint = new RangeConstraint(
|
|
|
+ pose_index, range_reading, FLAGS_range_stddev, FLAGS_corridor_length);
|
|
|
+ RangeCostFunction* cost_function = new RangeCostFunction(constraint);
|
|
|
+ // Add all the parameter blocks that affect this constraint.
|
|
|
+ parameter_blocks->clear();
|
|
|
+ for (int i = 0; i <= pose_index; ++i) {
|
|
|
+ parameter_blocks->push_back(&((*odometry_values)[i]));
|
|
|
+ cost_function->AddParameterBlock(1);
|
|
|
+ }
|
|
|
+ cost_function->SetNumResiduals(1);
|
|
|
+ return (cost_function);
|
|
|
+ }
|
|
|
+
|
|
|
+ const int pose_index;
|
|
|
+ const double range_reading;
|
|
|
+ const double range_stddev;
|
|
|
+ const double corridor_length;
|
|
|
+};
|
|
|
+
|
|
|
+void SimulateRobot(vector<double>* odometry_values,
|
|
|
+ vector<double>* range_readings) {
|
|
|
+ const int num_steps = static_cast<int>(
|
|
|
+ ceil(FLAGS_corridor_length / FLAGS_pose_separation));
|
|
|
+
|
|
|
+ // The robot starts out at the origin.
|
|
|
+ double robot_location = 0.0;
|
|
|
+ for (int i = 0; i < num_steps; ++i) {
|
|
|
+ const double actual_odometry_value = min(
|
|
|
+ FLAGS_pose_separation, FLAGS_corridor_length - robot_location);
|
|
|
+ robot_location += actual_odometry_value;
|
|
|
+ const double actual_range = FLAGS_corridor_length - robot_location;
|
|
|
+ const double observed_odometry =
|
|
|
+ RandNormal() * FLAGS_odometry_stddev + actual_odometry_value;
|
|
|
+ const double observed_range =
|
|
|
+ RandNormal() * FLAGS_range_stddev + actual_range;
|
|
|
+ odometry_values->push_back(observed_odometry);
|
|
|
+ range_readings->push_back(observed_range);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void PrintState(const vector<double>& odometry_readings,
|
|
|
+ const vector<double>& range_readings) {
|
|
|
+ CHECK_EQ(odometry_readings.size(), range_readings.size());
|
|
|
+ double robot_location = 0.0;
|
|
|
+ printf("pose: location odom range r.error o.error\n");
|
|
|
+ for (int i = 0; i < odometry_readings.size(); ++i) {
|
|
|
+ robot_location += odometry_readings[i];
|
|
|
+ const double range_error =
|
|
|
+ robot_location + range_readings[i] - FLAGS_corridor_length;
|
|
|
+ const double odometry_error =
|
|
|
+ FLAGS_pose_separation - odometry_readings[i];
|
|
|
+ printf("%4d: %8.3f %8.3f %8.3f %8.3f %8.3f\n",
|
|
|
+ static_cast<int>(i), robot_location, odometry_readings[i],
|
|
|
+ range_readings[i], range_error, odometry_error);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+int main(int argc, char** argv) {
|
|
|
+ google::InitGoogleLogging(argv[0]);
|
|
|
+ google::ParseCommandLineFlags(&argc, &argv, true);
|
|
|
+ // Make sure that the arguments parsed are all positive.
|
|
|
+ CHECK_GT(FLAGS_corridor_length, 0.0);
|
|
|
+ CHECK_GT(FLAGS_pose_separation, 0.0);
|
|
|
+ CHECK_GT(FLAGS_odometry_stddev, 0.0);
|
|
|
+ CHECK_GT(FLAGS_range_stddev, 0.0);
|
|
|
+
|
|
|
+ vector<double> odometry_values;
|
|
|
+ vector<double> range_readings;
|
|
|
+ SimulateRobot(&odometry_values, &range_readings);
|
|
|
+
|
|
|
+ printf("Initial values:\n");
|
|
|
+ PrintState(odometry_values, range_readings);
|
|
|
+ ceres::Problem problem;
|
|
|
+
|
|
|
+ for (int i = 0; i < odometry_values.size(); ++i) {
|
|
|
+ // Create and add a DynamicAutoDiffCostFunction for the RangeConstraint from
|
|
|
+ // pose i.
|
|
|
+ vector<double*> parameter_blocks;
|
|
|
+ RangeConstraint::RangeCostFunction* range_cost_function =
|
|
|
+ RangeConstraint::Create(
|
|
|
+ i, range_readings[i], &odometry_values, ¶meter_blocks);
|
|
|
+ problem.AddResidualBlock(range_cost_function, NULL, parameter_blocks);
|
|
|
+
|
|
|
+ // Create and add an AutoDiffCostFunction for the OdometryConstraint for
|
|
|
+ // pose i.
|
|
|
+ problem.AddResidualBlock(OdometryConstraint::Create(odometry_values[i]),
|
|
|
+ NULL,
|
|
|
+ &(odometry_values[i]));
|
|
|
+ }
|
|
|
+
|
|
|
+ ceres::Solver::Options solver_options;
|
|
|
+ solver_options.minimizer_progress_to_stdout = true;
|
|
|
+
|
|
|
+ Solver::Summary summary;
|
|
|
+ printf("Solving...\n");
|
|
|
+ Solve(solver_options, &problem, &summary);
|
|
|
+ printf("Done.\n");
|
|
|
+ std::cout << summary.FullReport() << "\n";
|
|
|
+ printf("Final values:\n");
|
|
|
+ PrintState(odometry_values, range_readings);
|
|
|
+ return 0;
|
|
|
+}
|
Joydeep Biswas