Migrate examples to use Manifolds Also change NULL to nullptr. Change-Id: I80a2328185d7891f61e07e64d5c1b59e74588ac7
diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc index ceac89a..8500b06 100644 --- a/examples/bal_problem.cc +++ b/examples/bal_problem.cc
@@ -73,7 +73,7 @@ BALProblem::BALProblem(const std::string& filename, bool use_quaternions) { FILE* fptr = fopen(filename.c_str(), "r"); - if (fptr == NULL) { + if (fptr == nullptr) { LOG(FATAL) << "Error: unable to open file " << filename; return; }; @@ -137,7 +137,7 @@ void BALProblem::WriteToFile(const std::string& filename) const { FILE* fptr = fopen(filename.c_str(), "w"); - if (fptr == NULL) { + if (fptr == nullptr) { LOG(FATAL) << "Error: unable to open file " << filename; return; };
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc index b5b0c55..ef2dbc4 100644 --- a/examples/bundle_adjuster.cc +++ b/examples/bundle_adjuster.cc
@@ -98,8 +98,7 @@ DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent " "rotations. If false, angle axis is used."); -DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local " - "parameterization."); +DEFINE_bool(use_manifolds, false, "For quaternions, use a manifold."); DEFINE_bool(robustify, false, "Use a robust loss function."); DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the " @@ -293,7 +292,7 @@ // If enabled use Huber's loss function. LossFunction* loss_function = - CERES_GET_FLAG(FLAGS_robustify) ? new HuberLoss(1.0) : NULL; + CERES_GET_FLAG(FLAGS_robustify) ? new HuberLoss(1.0) : nullptr; // Each observation correponds to a pair of a camera and a point // which are identified by camera_index()[i] and point_index()[i] @@ -305,13 +304,11 @@ } if (CERES_GET_FLAG(FLAGS_use_quaternions) && - CERES_GET_FLAG(FLAGS_use_local_parameterization)) { - LocalParameterization* camera_parameterization = - new ProductParameterization(new QuaternionParameterization(), - new IdentityParameterization(6)); + CERES_GET_FLAG(FLAGS_use_manifolds)) { + Manifold* camera_manifold = + new ProductManifold(new QuaternionManifold(), new EuclideanManifold(6)); for (int i = 0; i < bal_problem->num_cameras(); ++i) { - problem->SetParameterization(cameras + camera_block_size * i, - camera_parameterization); + problem->SetManifold(cameras + camera_block_size * i, camera_manifold); } } } @@ -358,9 +355,8 @@ } CHECK(CERES_GET_FLAG(FLAGS_use_quaternions) || - !CERES_GET_FLAG(FLAGS_use_local_parameterization)) - << "--use_local_parameterization can only be used with " - << "--use_quaternions."; + !CERES_GET_FLAG(FLAGS_use_manifolds)) + << "--use_manifolds can only be used with --use_quaternions."; ceres::examples::SolveProblem(CERES_GET_FLAG(FLAGS_input).c_str()); return 0; }
diff --git a/examples/circle_fit.cc b/examples/circle_fit.cc index 13b4077..e051b6a 100644 --- a/examples/circle_fit.cc +++ b/examples/circle_fit.cc
@@ -131,7 +131,7 @@ Problem problem; // Configure the loss function. - LossFunction* loss = NULL; + LossFunction* loss = nullptr; if (CERES_GET_FLAG(FLAGS_robust_threshold)) { loss = new CauchyLoss(CERES_GET_FLAG(FLAGS_robust_threshold)); }
diff --git a/examples/curve_fitting.cc b/examples/curve_fitting.cc index fc7ff94..c89f3d9 100644 --- a/examples/curve_fitting.cc +++ b/examples/curve_fitting.cc
@@ -145,7 +145,7 @@ problem.AddResidualBlock( new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>( new ExponentialResidual(data[2 * i], data[2 * i + 1])), - NULL, + nullptr, &m, &c); }
diff --git a/examples/denoising.cc b/examples/denoising.cc index eab05be..b885aed 100644 --- a/examples/denoising.cc +++ b/examples/denoising.cc
@@ -118,7 +118,7 @@ double** jacobians) const { const double x = parameters[0][0]; residuals[0] = sqrta_ * (x - b_); - if (jacobians != NULL && jacobians[0] != NULL) { + if (jacobians != nullptr && jacobians[0] != nullptr) { jacobians[0][0] = sqrta_; } return true; @@ -141,7 +141,7 @@ ceres::CostFunction* cost_function = new QuadraticCostFunction( coefficient, image.PixelFromLinearIndex(index)); problem->AddResidualBlock( - cost_function, NULL, solution->MutablePixelFromLinearIndex(index)); + cost_function, nullptr, solution->MutablePixelFromLinearIndex(index)); } // Create Ceres cost and loss functions for regularization. One is needed for
diff --git a/examples/ellipse_approximation.cc b/examples/ellipse_approximation.cc index 74782f4..b3a3284 100644 --- a/examples/ellipse_approximation.cc +++ b/examples/ellipse_approximation.cc
@@ -302,16 +302,16 @@ residuals[0] = y_[0] - ((1.0 - u) * x[1 + i0][0] + u * x[1 + i1][0]); residuals[1] = y_[1] - ((1.0 - u) * x[1 + i0][1] + u * x[1 + i1][1]); - if (jacobians == NULL) { + if (jacobians == nullptr) { return true; } - if (jacobians[0] != NULL) { + if (jacobians[0] != nullptr) { jacobians[0][0] = x[1 + i0][0] - x[1 + i1][0]; jacobians[0][1] = x[1 + i0][1] - x[1 + i1][1]; } for (int i = 0; i < num_segments_; ++i) { - if (jacobians[i + 1] != NULL) { + if (jacobians[i + 1] != nullptr) { ceres::MatrixRef(jacobians[i + 1], 2, 2).setZero(); if (i == i0) { jacobians[i + 1][0] = -(1.0 - u); @@ -415,7 +415,7 @@ // For each data point add a residual which measures its distance to its // corresponding position on the line segment contour. std::vector<double*> parameter_blocks(1 + num_segments); - parameter_blocks[0] = NULL; + parameter_blocks[0] = nullptr; for (int i = 0; i < num_segments; ++i) { parameter_blocks[i + 1] = X.data() + 2 * i; } @@ -423,7 +423,7 @@ parameter_blocks[0] = &t[i]; problem.AddResidualBlock( PointToLineSegmentContourCostFunction::Create(num_segments, kY.row(i)), - NULL, + nullptr, parameter_blocks); } @@ -431,7 +431,7 @@ for (int i = 0; i < num_segments; ++i) { problem.AddResidualBlock( EuclideanDistanceFunctor::Create(sqrt(regularization_weight)), - NULL, + nullptr, X.data() + 2 * i, X.data() + 2 * ((i + 1) % num_segments)); }
diff --git a/examples/fields_of_experts.cc b/examples/fields_of_experts.cc index 7b7983e..c2d901b 100644 --- a/examples/fields_of_experts.cc +++ b/examples/fields_of_experts.cc
@@ -60,9 +60,9 @@ residuals[0] += filter_[i] * parameters[i][0]; } - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_variables; ++i) { - if (jacobians[i] != NULL) { + if (jacobians[i] != nullptr) { jacobians[i][0] = filter_[i]; } }
diff --git a/examples/helloworld_analytic_diff.cc b/examples/helloworld_analytic_diff.cc index 6e120b5..5e452cc 100644 --- a/examples/helloworld_analytic_diff.cc +++ b/examples/helloworld_analytic_diff.cc
@@ -64,14 +64,14 @@ // jacobians. // // Since the Evaluate function can be called with the jacobians - // pointer equal to NULL, the Evaluate function must check to see + // pointer equal to nullptr, the Evaluate function must check to see // if jacobians need to be computed. // // For this simple problem it is overkill to check if jacobians[0] - // is NULL, but in general when writing more complex + // is nullptr, but in general when writing more complex // CostFunctions, it is possible that Ceres may only demand the // derivatives w.r.t. a subset of the parameter blocks. - if (jacobians != NULL && jacobians[0] != NULL) { + if (jacobians != nullptr && jacobians[0] != nullptr) { jacobians[0][0] = -1; } @@ -92,7 +92,7 @@ // Set up the only cost function (also known as residual). CostFunction* cost_function = new QuadraticCostFunction; - problem.AddResidualBlock(cost_function, NULL, &x); + problem.AddResidualBlock(cost_function, nullptr, &x); // Run the solver! Solver::Options options;
diff --git a/examples/helloworld_numeric_diff.cc b/examples/helloworld_numeric_diff.cc index 474adf3..0459305 100644 --- a/examples/helloworld_numeric_diff.cc +++ b/examples/helloworld_numeric_diff.cc
@@ -64,7 +64,7 @@ // numeric differentiation to obtain the derivative (jacobian). CostFunction* cost_function = new NumericDiffCostFunction<CostFunctor, CENTRAL, 1, 1>(new CostFunctor); - problem.AddResidualBlock(cost_function, NULL, &x); + problem.AddResidualBlock(cost_function, nullptr, &x); // Run the solver! Solver::Options options;
diff --git a/examples/libmv_bundle_adjuster.cc b/examples/libmv_bundle_adjuster.cc index 77d9ac8..91629c9 100644 --- a/examples/libmv_bundle_adjuster.cc +++ b/examples/libmv_bundle_adjuster.cc
@@ -206,11 +206,11 @@ EuclideanCamera* CameraForImage(vector<EuclideanCamera>* all_cameras, const int image) { if (image < 0 || image >= all_cameras->size()) { - return NULL; + return nullptr; } EuclideanCamera* camera = &(*all_cameras)[image]; if (camera->image == -1) { - return NULL; + return nullptr; } return camera; } @@ -218,11 +218,11 @@ const EuclideanCamera* CameraForImage( const vector<EuclideanCamera>& all_cameras, const int image) { if (image < 0 || image >= all_cameras.size()) { - return NULL; + return nullptr; } const EuclideanCamera* camera = &all_cameras[image]; if (camera->image == -1) { - return NULL; + return nullptr; } return camera; } @@ -244,11 +244,11 @@ EuclideanPoint* PointForTrack(vector<EuclideanPoint>* all_points, const int track) { if (track < 0 || track >= all_points->size()) { - return NULL; + return nullptr; } EuclideanPoint* point = &(*all_points)[track]; if (point->track == -1) { - return NULL; + return nullptr; } return point; } @@ -670,8 +670,8 @@ vector<Vec6> all_cameras_R_t = PackCamerasRotationAndTranslation(all_markers, *all_cameras); - // Parameterization used to restrict camera motion for modal solvers. - ceres::SubsetParameterization* constant_transform_parameterization = NULL; + // Manifold used to restrict camera motion for modal solvers. + ceres::SubsetManifold* constant_transform_manifold = nullptr; if (bundle_constraints & BUNDLE_NO_TRANSLATION) { std::vector<int> constant_translation; @@ -680,8 +680,8 @@ constant_translation.push_back(4); constant_translation.push_back(5); - constant_transform_parameterization = - new ceres::SubsetParameterization(6, constant_translation); + constant_transform_manifold = + new ceres::SubsetManifold(6, constant_translation); } std::vector<OpenCVReprojectionError> errors; @@ -696,7 +696,7 @@ const Marker& marker = all_markers[i]; EuclideanCamera* camera = CameraForImage(all_cameras, marker.image); EuclideanPoint* point = PointForTrack(all_points, marker.track); - if (camera == NULL || point == NULL) { + if (camera == nullptr || point == nullptr) { continue; } @@ -708,7 +708,7 @@ costFunctions.emplace_back(&errors.back(), ceres::DO_NOT_TAKE_OWNERSHIP); problem.AddResidualBlock(&costFunctions.back(), - NULL, + nullptr, camera_intrinsics, current_camera_R_t, &point->X(0)); @@ -720,8 +720,7 @@ } if (bundle_constraints & BUNDLE_NO_TRANSLATION) { - problem.SetParameterization(current_camera_R_t, - constant_transform_parameterization); + problem.SetManifold(current_camera_R_t, constant_transform_manifold); } num_residuals++; @@ -760,10 +759,9 @@ // Always set K3 constant, it's not used at the moment. constant_intrinsics.push_back(OFFSET_K3); - ceres::SubsetParameterization* subset_parameterization = - new ceres::SubsetParameterization(8, constant_intrinsics); - - problem.SetParameterization(camera_intrinsics, subset_parameterization); + ceres::SubsetManifold* subset_manifold = + new ceres::SubsetManifold(8, constant_intrinsics); + problem.SetManifold(camera_intrinsics, subset_manifold); } // Configure the solver. @@ -804,8 +802,11 @@ bool is_image_space; vector<Marker> all_markers; - if (!ReadProblemFromFile(CERES_GET_FLAG(FLAGS_input), camera_intrinsics, - &all_cameras, &all_points, &is_image_space, + if (!ReadProblemFromFile(CERES_GET_FLAG(FLAGS_input), + camera_intrinsics, + &all_cameras, + &all_points, + &is_image_space, &all_markers)) { LOG(ERROR) << "Error reading problem file"; return EXIT_FAILURE;
diff --git a/examples/libmv_homography.cc b/examples/libmv_homography.cc index 55f3b70..1b51b82 100644 --- a/examples/libmv_homography.cc +++ b/examples/libmv_homography.cc
@@ -335,7 +335,7 @@ 4, // num_residuals 9>( homography_symmetric_geometric_cost_function), - NULL, + nullptr, H->data()); }
diff --git a/examples/more_garbow_hillstrom.cc b/examples/more_garbow_hillstrom.cc index aac7bf9..ab5f381 100644 --- a/examples/more_garbow_hillstrom.cc +++ b/examples/more_garbow_hillstrom.cc
@@ -95,20 +95,26 @@ ceres::NumericDiffOptions options; \ SetNumericDiffOptions(&options); \ if (CERES_GET_FLAG(FLAGS_numeric_diff_method) == "central") { \ - return new NumericDiffCostFunction<name, ceres::CENTRAL, \ - num_residuals, num_parameters>( \ + return new NumericDiffCostFunction<name, \ + ceres::CENTRAL, \ + num_residuals, \ + num_parameters>( \ new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \ } else if (CERES_GET_FLAG(FLAGS_numeric_diff_method) == "forward") { \ - return new NumericDiffCostFunction<name, ceres::FORWARD, \ - num_residuals, num_parameters>( \ + return new NumericDiffCostFunction<name, \ + ceres::FORWARD, \ + num_residuals, \ + num_parameters>( \ new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \ } else if (CERES_GET_FLAG(FLAGS_numeric_diff_method) == "ridders") { \ - return new NumericDiffCostFunction<name, ceres::RIDDERS, \ - num_residuals, num_parameters>( \ + return new NumericDiffCostFunction<name, \ + ceres::RIDDERS, \ + num_residuals, \ + num_parameters>( \ new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \ } else { \ LOG(ERROR) << "Invalid numeric diff method specified"; \ - return NULL; \ + return nullptr; \ } \ } else { \ return new AutoDiffCostFunction<name, num_residuals, num_parameters>( \ @@ -543,7 +549,7 @@ } Problem problem; - problem.AddResidualBlock(TestProblem::Create(), NULL, x); + problem.AddResidualBlock(TestProblem::Create(), nullptr, x); double optimal_cost = TestProblem::unconstrained_optimal_cost; if (is_constrained) {
diff --git a/examples/nist.cc b/examples/nist.cc index 1456612..f12dbf3 100644 --- a/examples/nist.cc +++ b/examples/nist.cc
@@ -523,7 +523,7 @@ const Matrix& response, const int num_observations) { Model* model = new Model(predictor.data(), response.data(), num_observations); - ceres::CostFunction* cost_function = NULL; + ceres::CostFunction* cost_function = nullptr; if (CERES_GET_FLAG(FLAGS_use_numeric_diff)) { ceres::NumericDiffOptions options; SetNumericDiffOptions(&options); @@ -603,7 +603,8 @@ if (!CERES_GET_FLAG(FLAGS_use_tiny_solver)) { ceres::Problem problem; - problem.AddResidualBlock(cost_function, NULL, initial_parameters.data()); + problem.AddResidualBlock( + cost_function, nullptr, initial_parameters.data()); ceres::Solver::Summary summary; ceres::Solver::Options options; SetMinimizerOptions(&options);
diff --git a/examples/powell.cc b/examples/powell.cc index 1a09a73..806542f 100644 --- a/examples/powell.cc +++ b/examples/powell.cc
@@ -110,17 +110,18 @@ // wrapper to get the derivatives automatically. The parameters, x1 through // x4, are modified in place. problem.AddResidualBlock( - new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x1, &x2); + new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), nullptr, &x1, &x2); problem.AddResidualBlock( - new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4); + new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), nullptr, &x3, &x4); problem.AddResidualBlock( - new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3); + new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), nullptr, &x2, &x3); problem.AddResidualBlock( - new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4); + new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), nullptr, &x1, &x4); Solver::Options options; - LOG_IF(FATAL, !ceres::StringToMinimizerType(CERES_GET_FLAG(FLAGS_minimizer), - &options.minimizer_type)) + LOG_IF(FATAL, + !ceres::StringToMinimizerType(CERES_GET_FLAG(FLAGS_minimizer), + &options.minimizer_type)) << "Invalid minimizer: " << CERES_GET_FLAG(FLAGS_minimizer) << ", valid options are: trust_region and line_search.";
diff --git a/examples/robot_pose_mle.cc b/examples/robot_pose_mle.cc index 2913758..90d5ab6 100644 --- a/examples/robot_pose_mle.cc +++ b/examples/robot_pose_mle.cc
@@ -315,12 +315,12 @@ RangeConstraint::RangeCostFunction* range_cost_function = RangeConstraint::Create( i, range_readings[i], &odometry_values, ¶meter_blocks); - problem.AddResidualBlock(range_cost_function, NULL, parameter_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]), - NULL, + nullptr, &(odometry_values[i])); }
diff --git a/examples/simple_bundle_adjuster.cc b/examples/simple_bundle_adjuster.cc index 8180d73..b02d317 100644 --- a/examples/simple_bundle_adjuster.cc +++ b/examples/simple_bundle_adjuster.cc
@@ -66,7 +66,7 @@ bool LoadFile(const char* filename) { FILE* fptr = fopen(filename, "r"); - if (fptr == NULL) { + if (fptr == nullptr) { return false; }; @@ -198,7 +198,7 @@ ceres::CostFunction* cost_function = SnavelyReprojectionError::Create( observations[2 * i + 0], observations[2 * i + 1]); problem.AddResidualBlock(cost_function, - NULL /* squared loss */, + nullptr /* squared loss */, bal_problem.mutable_camera_for_observation(i), bal_problem.mutable_point_for_observation(i)); }
diff --git a/examples/slam/pose_graph_2d/CMakeLists.txt b/examples/slam/pose_graph_2d/CMakeLists.txt index 20af056..8c006d5 100644 --- a/examples/slam/pose_graph_2d/CMakeLists.txt +++ b/examples/slam/pose_graph_2d/CMakeLists.txt
@@ -30,7 +30,7 @@ if (GFLAGS) add_executable(pose_graph_2d - angle_local_parameterization.h + angle_manifold.h normalize_angle.h pose_graph_2d.cc pose_graph_2d_error_term.h
diff --git a/examples/slam/pose_graph_2d/angle_local_parameterization.h b/examples/slam/pose_graph_2d/angle_manifold.h similarity index 65% rename from examples/slam/pose_graph_2d/angle_local_parameterization.h rename to examples/slam/pose_graph_2d/angle_manifold.h index a81637c..f13285b 100644 --- a/examples/slam/pose_graph_2d/angle_local_parameterization.h +++ b/examples/slam/pose_graph_2d/angle_manifold.h
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2016 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -27,38 +27,44 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: vitus@google.com (Michael Vitus) +// sameeragarwal@google.com (Sameer Agarwal) -#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ -#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ -#include "ceres/local_parameterization.h" +#include "ceres/autodiff_manifold.h" #include "normalize_angle.h" namespace ceres { namespace examples { -// Defines a local parameterization for updating the angle to be constrained in -// [-pi to pi). -class AngleLocalParameterization { +// Defines a manifold for updating the angle to be constrained in [-pi to pi). +class AngleManifold { public: template <typename T> - bool operator()(const T* theta_radians, - const T* delta_theta_radians, - T* theta_radians_plus_delta) const { - *theta_radians_plus_delta = - NormalizeAngle(*theta_radians + *delta_theta_radians); + bool Plus(const T* x_radians, + const T* delta_radians, + T* x_plus_delta_radians) const { + *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians); + return true; + } + + template <typename T> + bool Minus(const T* y_radians, + const T* x_radians, + T* y_minus_x_radians) const { + *y_minus_x_radians = + NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians); return true; } - static ceres::LocalParameterization* Create() { - return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, - 1, - 1>); + static ceres::Manifold* Create() { + return new ceres::AutoDiffManifold<AngleManifold, 1, 1>; } }; } // namespace examples } // namespace ceres -#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d.cc b/examples/slam/pose_graph_2d/pose_graph_2d.cc index 1172123..b57f4e7 100644 --- a/examples/slam/pose_graph_2d/pose_graph_2d.cc +++ b/examples/slam/pose_graph_2d/pose_graph_2d.cc
@@ -39,7 +39,7 @@ #include <string> #include <vector> -#include "angle_local_parameterization.h" +#include "angle_manifold.h" #include "ceres/ceres.h" #include "common/read_g2o.h" #include "gflags/gflags.h" @@ -58,16 +58,15 @@ void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints, std::map<int, Pose2d>* poses, ceres::Problem* problem) { - CHECK(poses != NULL); - CHECK(problem != NULL); + CHECK(poses != nullptr); + CHECK(problem != nullptr); if (constraints.empty()) { LOG(INFO) << "No constraints, no problem to optimize."; return; } - ceres::LossFunction* loss_function = NULL; - ceres::LocalParameterization* angle_local_parameterization = - AngleLocalParameterization::Create(); + ceres::LossFunction* loss_function = nullptr; + ceres::Manifold* angle_manifold = AngleManifold::Create(); for (std::vector<Constraint2d>::const_iterator constraints_iter = constraints.begin(); @@ -98,10 +97,8 @@ &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians); - problem->SetParameterization(&pose_begin_iter->second.yaw_radians, - angle_local_parameterization); - problem->SetParameterization(&pose_end_iter->second.yaw_radians, - angle_local_parameterization); + problem->SetManifold(&pose_begin_iter->second.yaw_radians, angle_manifold); + problem->SetManifold(&pose_end_iter->second.yaw_radians, angle_manifold); } // The pose graph optimization problem has three DOFs that are not fully @@ -120,7 +117,7 @@ // Returns true if the solve was successful. bool SolveOptimizationProblem(ceres::Problem* problem) { - CHECK(problem != NULL); + CHECK(problem != nullptr); ceres::Solver::Options options; options.max_num_iterations = 100;
diff --git a/examples/slam/pose_graph_3d/pose_graph_3d.cc b/examples/slam/pose_graph_3d/pose_graph_3d.cc index 2f8d6a4..798e236 100644 --- a/examples/slam/pose_graph_3d/pose_graph_3d.cc +++ b/examples/slam/pose_graph_3d/pose_graph_3d.cc
@@ -50,16 +50,15 @@ void BuildOptimizationProblem(const VectorOfConstraints& constraints, MapOfPoses* poses, ceres::Problem* problem) { - CHECK(poses != NULL); - CHECK(problem != NULL); + CHECK(poses != nullptr); + CHECK(problem != nullptr); if (constraints.empty()) { LOG(INFO) << "No constraints, no problem to optimize."; return; } - ceres::LossFunction* loss_function = NULL; - ceres::LocalParameterization* quaternion_local_parameterization = - new EigenQuaternionParameterization; + ceres::LossFunction* loss_function = nullptr; + ceres::Manifold* quaternion_manifold = new EigenQuaternionManifold; for (VectorOfConstraints::const_iterator constraints_iter = constraints.begin(); @@ -87,10 +86,10 @@ pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data()); - problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(), - quaternion_local_parameterization); - problem->SetParameterization(pose_end_iter->second.q.coeffs().data(), - quaternion_local_parameterization); + problem->SetManifold(pose_begin_iter->second.q.coeffs().data(), + quaternion_manifold); + problem->SetManifold(pose_end_iter->second.q.coeffs().data(), + quaternion_manifold); } // The pose graph optimization problem has six DOFs that are not fully @@ -108,7 +107,7 @@ // Returns true if the solve was successful. bool SolveOptimizationProblem(ceres::Problem* problem) { - CHECK(problem != NULL); + CHECK(problem != nullptr); ceres::Solver::Options options; options.max_num_iterations = 200;
diff --git a/examples/snavely_reprojection_error.h b/examples/snavely_reprojection_error.h index eb39d23..8f848d0 100644 --- a/examples/snavely_reprojection_error.h +++ b/examples/snavely_reprojection_error.h
@@ -123,7 +123,7 @@ // We use QuaternionRotatePoint as it does not assume that the // quaternion is normalized, since one of the ways to run the // bundle adjuster is to let Ceres optimize all 4 quaternion - // parameters without a local parameterization. + // parameters without using a Quaternion manifold. T p[3]; QuaternionRotatePoint(camera, point, p);
diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h index a070c96..8827b30 100644 --- a/include/ceres/ceres.h +++ b/include/ceres/ceres.h
@@ -57,6 +57,7 @@ #include "ceres/jet.h" #include "ceres/local_parameterization.h" #include "ceres/loss_function.h" +#include "ceres/manifold.h" #include "ceres/numeric_diff_cost_function.h" #include "ceres/numeric_diff_first_order_function.h" #include "ceres/numeric_diff_options.h"
diff --git a/internal/ceres/gradient_checking_cost_function_test.cc b/internal/ceres/gradient_checking_cost_function_test.cc index d5b696a..6f99b12 100644 --- a/internal/ceres/gradient_checking_cost_function_test.cc +++ b/internal/ceres/gradient_checking_cost_function_test.cc
@@ -429,7 +429,7 @@ problem_impl.AddParameterBlock(y, 4); problem_impl.SetParameterBlockConstant(y); problem_impl.AddParameterBlock(z, 5); - problem_impl.AddParameterBlock(w, 4, new Quaternion); + problem_impl.AddParameterBlock(w, 4, new QuaternionManifold); // clang-format off problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), nullptr, x);