| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2015 Google Inc. All rights reserved. |
| // http://ceres-solver.org/ |
| // |
| // 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 estimates 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 exploited 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 odometry |
| // 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 |
| // odometry observations will only be known at run time. |
| |
| #include <cmath> |
| #include <cstdio> |
| #include <random> |
| #include <vector> |
| |
| #include "ceres/ceres.h" |
| #include "ceres/dynamic_autodiff_cost_function.h" |
| #include "gflags/gflags.h" |
| #include "glog/logging.h" |
| |
| using ceres::AutoDiffCostFunction; |
| using ceres::CauchyLoss; |
| using ceres::CostFunction; |
| using ceres::DynamicAutoDiffCostFunction; |
| using ceres::LossFunction; |
| using ceres::Problem; |
| using ceres::Solve; |
| using ceres::Solver; |
| 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 constexpr int kStride = 10; |
| |
| struct OdometryConstraint { |
| using OdometryCostFunction = AutoDiffCostFunction<OdometryConstraint, 1, 1>; |
| |
| 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 - odometry_mean) / odometry_stddev; |
| return true; |
| } |
| |
| static OdometryCostFunction* Create(const double odometry_value) { |
| return new OdometryCostFunction(new OdometryConstraint( |
| odometry_value, CERES_GET_FLAG(FLAGS_odometry_stddev))); |
| } |
| |
| const double odometry_mean; |
| const double odometry_stddev; |
| }; |
| |
| struct RangeConstraint { |
| using RangeCostFunction = |
| DynamicAutoDiffCostFunction<RangeConstraint, kStride>; |
| |
| 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 + range_reading - corridor_length) / 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) { |
| auto* constraint = |
| new RangeConstraint(pose_index, |
| range_reading, |
| CERES_GET_FLAG(FLAGS_range_stddev), |
| CERES_GET_FLAG(FLAGS_corridor_length)); |
| auto* 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; |
| }; |
| |
| namespace { |
| |
| void SimulateRobot(vector<double>* odometry_values, |
| vector<double>* range_readings) { |
| const int num_steps = |
| static_cast<int>(ceil(CERES_GET_FLAG(FLAGS_corridor_length) / |
| CERES_GET_FLAG(FLAGS_pose_separation))); |
| std::mt19937 prng; |
| std::normal_distribution<double> odometry_noise( |
| 0.0, CERES_GET_FLAG(FLAGS_odometry_stddev)); |
| std::normal_distribution<double> range_noise( |
| 0.0, CERES_GET_FLAG(FLAGS_range_stddev)); |
| |
| // 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(CERES_GET_FLAG(FLAGS_pose_separation), |
| CERES_GET_FLAG(FLAGS_corridor_length) - robot_location); |
| robot_location += actual_odometry_value; |
| const double actual_range = |
| CERES_GET_FLAG(FLAGS_corridor_length) - robot_location; |
| const double observed_odometry = |
| actual_odometry_value + odometry_noise(prng); |
| const double observed_range = actual_range + range_noise(prng); |
| 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] - |
| CERES_GET_FLAG(FLAGS_corridor_length); |
| const double odometry_error = |
| CERES_GET_FLAG(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); |
| } |
| } |
| |
| } // namespace |
| |
| int main(int argc, char** argv) { |
| google::InitGoogleLogging(argv[0]); |
| GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); |
| // Make sure that the arguments parsed are all positive. |
| CHECK_GT(CERES_GET_FLAG(FLAGS_corridor_length), 0.0); |
| CHECK_GT(CERES_GET_FLAG(FLAGS_pose_separation), 0.0); |
| CHECK_GT(CERES_GET_FLAG(FLAGS_odometry_stddev), 0.0); |
| CHECK_GT(CERES_GET_FLAG(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, nullptr, parameter_blocks); |
| |
| // Create and add an AutoDiffCostFunction for the OdometryConstraint for |
| // pose i. |
| problem.AddResidualBlock(OdometryConstraint::Create(odometry_values[i]), |
| nullptr, |
| &(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; |
| } |