Merge commit 'cfb09d18272bb3c30585042533cbe7d876ef7ce0'
Upgrade ceres to the latest.
Change-Id: I2d8fb7d506a98f704e4e2f30e60030a0d6763b43
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/third_party/ceres/examples/BUILD b/third_party/ceres/examples/BUILD
index 80d9a67..8741b94 100644
--- a/third_party/ceres/examples/BUILD
+++ b/third_party/ceres/examples/BUILD
@@ -32,6 +32,7 @@
# Needed to silence GFlags complaints.
"-Wno-sign-compare",
"-Wno-unused-parameter",
+ # Needed to put fscanf in a function.
"-Wno-format-nonliteral",
]
@@ -47,7 +48,6 @@
"bal_problem.cc",
"bal_problem.h",
"bundle_adjuster.cc",
- "random.h",
"snavely_reprojection_error.h",
],
copts = EXAMPLE_COPTS,
@@ -69,7 +69,6 @@
cc_binary(
name = "robot_pose_mle",
srcs = [
- "random.h",
"robot_pose_mle.cc",
],
copts = EXAMPLE_COPTS,
@@ -80,7 +79,7 @@
name = "pose_graph_2d",
srcs = [
"slam/common/read_g2o.h",
- "slam/pose_graph_2d/angle_local_parameterization.h",
+ "slam/pose_graph_2d/angle_manifold.h",
"slam/pose_graph_2d/normalize_angle.h",
"slam/pose_graph_2d/pose_graph_2d.cc",
"slam/pose_graph_2d/pose_graph_2d_error_term.h",
diff --git a/third_party/ceres/examples/CMakeLists.txt b/third_party/ceres/examples/CMakeLists.txt
index 7f9b117..8af2077 100644
--- a/third_party/ceres/examples/CMakeLists.txt
+++ b/third_party/ceres/examples/CMakeLists.txt
@@ -1,5 +1,5 @@
# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 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
@@ -28,86 +28,96 @@
#
# Author: keir@google.com (Keir Mierle)
-# Only Ceres itself should be compiled with CERES_BUILDING_SHARED_LIBRARY
-# defined, any users of Ceres will have CERES_USING_SHARED_LIBRARY defined
-# for them in Ceres' config.h if appropriate.
-if (BUILD_SHARED_LIBS)
- remove_definitions(-DCERES_BUILDING_SHARED_LIBRARY)
-endif()
-
add_executable(helloworld helloworld.cc)
-target_link_libraries(helloworld Ceres::ceres)
+target_link_libraries(helloworld PRIVATE Ceres::ceres)
add_executable(helloworld_numeric_diff helloworld_numeric_diff.cc)
-target_link_libraries(helloworld_numeric_diff Ceres::ceres)
+target_link_libraries(helloworld_numeric_diff PRIVATE Ceres::ceres)
add_executable(helloworld_analytic_diff helloworld_analytic_diff.cc)
-target_link_libraries(helloworld_analytic_diff Ceres::ceres)
+target_link_libraries(helloworld_analytic_diff PRIVATE Ceres::ceres)
add_executable(curve_fitting curve_fitting.cc)
-target_link_libraries(curve_fitting Ceres::ceres)
+target_link_libraries(curve_fitting PRIVATE Ceres::ceres)
add_executable(rosenbrock rosenbrock.cc)
-target_link_libraries(rosenbrock Ceres::ceres)
+target_link_libraries(rosenbrock PRIVATE Ceres::ceres)
+
+add_executable(rosenbrock_analytic_diff rosenbrock_analytic_diff.cc)
+target_link_libraries(rosenbrock_analytic_diff PRIVATE Ceres::ceres)
+
+add_executable(rosenbrock_numeric_diff rosenbrock_numeric_diff.cc)
+target_link_libraries(rosenbrock_numeric_diff PRIVATE Ceres::ceres)
add_executable(curve_fitting_c curve_fitting.c)
-target_link_libraries(curve_fitting_c Ceres::ceres)
-# Force CMake to link curve_fitting_c using the C linker.
-set_target_properties(curve_fitting_c PROPERTIES LINKER_LANGUAGE C)
+target_link_libraries(curve_fitting_c PRIVATE Ceres::ceres)
+# Force CMake to link curve_fitting_c using the C++ linker.
+set_target_properties(curve_fitting_c PROPERTIES LINKER_LANGUAGE CXX)
# As this is a C file #including <math.h> we have to explicitly add the math
# library (libm). Although some compilers (dependent upon options) will accept
# the indirect link to libm via Ceres, at least GCC 4.8 on pure Debian won't.
-if (NOT MSVC)
- target_link_libraries(curve_fitting_c m)
-endif (NOT MSVC)
+if (HAVE_LIBM)
+ target_link_libraries(curve_fitting_c PRIVATE m)
+endif (HAVE_LIBM)
add_executable(ellipse_approximation ellipse_approximation.cc)
-target_link_libraries(ellipse_approximation Ceres::ceres)
+target_link_libraries(ellipse_approximation PRIVATE Ceres::ceres)
add_executable(robust_curve_fitting robust_curve_fitting.cc)
-target_link_libraries(robust_curve_fitting Ceres::ceres)
+target_link_libraries(robust_curve_fitting PRIVATE Ceres::ceres)
add_executable(simple_bundle_adjuster simple_bundle_adjuster.cc)
-target_link_libraries(simple_bundle_adjuster Ceres::ceres)
+target_link_libraries(simple_bundle_adjuster PRIVATE Ceres::ceres)
+
+add_executable(bicubic_interpolation bicubic_interpolation.cc)
+target_link_libraries(bicubic_interpolation PRIVATE Ceres::ceres)
+
+add_executable(bicubic_interpolation_analytic bicubic_interpolation_analytic.cc)
+target_link_libraries(bicubic_interpolation_analytic PRIVATE Ceres::ceres)
+
+add_executable(iteration_callback_example iteration_callback_example.cc)
+target_link_libraries(iteration_callback_example PRIVATE Ceres::ceres)
+
+add_executable(evaluation_callback_example evaluation_callback_example.cc)
+target_link_libraries(evaluation_callback_example PRIVATE Ceres::ceres)
if (GFLAGS)
add_executable(powell powell.cc)
- target_link_libraries(powell Ceres::ceres gflags)
+ target_link_libraries(powell PRIVATE Ceres::ceres gflags)
add_executable(nist nist.cc)
- target_link_libraries(nist Ceres::ceres gflags)
- if (MSVC)
- target_compile_options(nist PRIVATE "/bigobj")
+ target_link_libraries(nist PRIVATE Ceres::ceres gflags)
+ if (HAVE_BIGOBJ)
+ target_compile_options(nist PRIVATE /bigobj)
endif()
add_executable(more_garbow_hillstrom more_garbow_hillstrom.cc)
- target_link_libraries(more_garbow_hillstrom Ceres::ceres gflags)
+ target_link_libraries(more_garbow_hillstrom PRIVATE Ceres::ceres gflags)
add_executable(circle_fit circle_fit.cc)
- target_link_libraries(circle_fit Ceres::ceres gflags)
+ target_link_libraries(circle_fit PRIVATE Ceres::ceres gflags)
add_executable(bundle_adjuster
bundle_adjuster.cc
bal_problem.cc)
- target_link_libraries(bundle_adjuster Ceres::ceres gflags)
+ target_link_libraries(bundle_adjuster PRIVATE Ceres::ceres gflags)
add_executable(libmv_bundle_adjuster
libmv_bundle_adjuster.cc)
- target_link_libraries(libmv_bundle_adjuster Ceres::ceres gflags)
+ target_link_libraries(libmv_bundle_adjuster PRIVATE Ceres::ceres gflags)
add_executable(libmv_homography
libmv_homography.cc)
- target_link_libraries(libmv_homography Ceres::ceres gflags)
+ target_link_libraries(libmv_homography PRIVATE Ceres::ceres gflags)
add_executable(denoising
denoising.cc
fields_of_experts.cc)
- target_link_libraries(denoising Ceres::ceres gflags)
+ target_link_libraries(denoising PRIVATE Ceres::ceres gflags)
add_executable(robot_pose_mle
robot_pose_mle.cc)
- target_link_libraries(robot_pose_mle Ceres::ceres gflags)
-
+ target_link_libraries(robot_pose_mle PRIVATE Ceres::ceres gflags)
endif (GFLAGS)
add_subdirectory(sampled_function)
diff --git a/third_party/ceres/examples/Makefile.example b/third_party/ceres/examples/Makefile.example
deleted file mode 100644
index f2b0dc0..0000000
--- a/third_party/ceres/examples/Makefile.example
+++ /dev/null
@@ -1,82 +0,0 @@
-# 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: keir@google.com (Keir Mierle)
-#
-# This is an example Makefile for using Ceres. In practice, the Ceres authors
-# suggest using CMake instead, but if Make is needed for some reason, this
-# example serves to make it easy to do so.
-
-# This should point to place where you unpacked or cloned Ceres.
-CERES_SRC_DIR := /home/keir/wrk/ceres-extra
-
-# This should point to the place where you built Ceres. If you got Ceres by
-# installing it, then this will likely be /usr/local/lib.
-CERES_BIN_DIR := /home/keir/wrk/ceres-extra-bin
-
-# The place you unpacked or cloned Eigen. If Eigen was installed from packages,
-# this will likely be /usr/local/include.
-EIGEN_SRC_DIR := /home/keir/src/eigen-3.0.5
-
-INCLUDES := -I$(CERES_SRC_DIR)/include \
- -I$(EIGEN_SRC_DIR)
-
-CERES_LIBRARY := -lceres
-CERES_LIBRARY_PATH := -L$(CERES_BIN_DIR)/lib
-CERES_LIBRARY_DEPENDENCIES = -lgflags -lglog
-
-# If Ceres was built with Suitesparse:
-CERES_LIBRARY_DEPENDENCIES += -llapack -lcamd -lamd -lccolamd -lcolamd -lcholmod
-
-# If Ceres was built with CXSparse:
-CERES_LIBRARY_DEPENDENCIES += -lcxsparse
-
-# If Ceres was built with OpenMP:
-CERES_LIBRARY_DEPENDENCIES += -fopenmp -lpthread -lgomp -lm
-
-# The set of object files for your application.
-APPLICATION_OBJS := simple_bundle_adjuster.o
-
-all: simple_bundle_adjuster
-
-simple_bundle_adjuster: $(APPLICATION_OBJS)
- g++ \
- $(APPLICATION_OBJS) \
- $(CERES_LIBRARY_PATH) \
- $(CERES_LIBRARY) \
- $(CERES_LIBRARY_DEPENDENCIES) \
- -o simple_bundle_adjuster
-
-# Disabling debug asserts via -DNDEBUG helps make Eigen faster, at the cost of
-# not getting handy assert failures when there are issues in your code.
-CFLAGS := -O2 -DNDEBUG
-
-# If you have files ending in .cpp instead of .cc, fix the next line
-# appropriately.
-%.o: %.cc $(DEPS)
- g++ -c -o $@ $< $(CFLAGS) $(INCLUDES)
diff --git a/third_party/ceres/examples/bal_problem.cc b/third_party/ceres/examples/bal_problem.cc
index ceac89a..ccf7449 100644
--- a/third_party/ceres/examples/bal_problem.cc
+++ b/third_party/ceres/examples/bal_problem.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -30,22 +30,22 @@
#include "bal_problem.h"
+#include <algorithm>
#include <cstdio>
-#include <cstdlib>
#include <fstream>
+#include <functional>
+#include <random>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "ceres/rotation.h"
#include "glog/logging.h"
-#include "random.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
namespace {
-typedef Eigen::Map<Eigen::VectorXd> VectorRef;
-typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
+using VectorRef = Eigen::Map<Eigen::VectorXd>;
+using ConstVectorRef = Eigen::Map<const Eigen::VectorXd>;
template <typename T>
void FscanfOrDie(FILE* fptr, const char* format, T* value) {
@@ -55,15 +55,14 @@
}
}
-void PerturbPoint3(const double sigma, double* point) {
+void PerturbPoint3(std::function<double()> dist, double* point) {
for (int i = 0; i < 3; ++i) {
- point[i] += RandNormal() * sigma;
+ point[i] += dist();
}
}
double Median(std::vector<double>* data) {
- int n = data->size();
- std::vector<double>::iterator mid_point = data->begin() + n / 2;
+ auto mid_point = data->begin() + data->size() / 2;
std::nth_element(data->begin(), mid_point, data->end());
return *mid_point;
}
@@ -73,12 +72,12 @@
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;
};
- // This wil die horribly on invalid files. Them's the breaks.
+ // This will die horribly on invalid files. Them's the breaks.
FscanfOrDie(fptr, "%d", &num_cameras_);
FscanfOrDie(fptr, "%d", &num_points_);
FscanfOrDie(fptr, "%d", &num_observations_);
@@ -111,7 +110,7 @@
if (use_quaternions) {
// Switch the angle-axis rotations to quaternions.
num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
- double* quaternion_parameters = new double[num_parameters_];
+ auto* quaternion_parameters = new double[num_parameters_];
double* original_cursor = parameters_;
double* quaternion_cursor = quaternion_parameters;
for (int i = 0; i < num_cameras_; ++i) {
@@ -137,7 +136,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;
};
@@ -161,8 +160,8 @@
} else {
memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
}
- for (int j = 0; j < 9; ++j) {
- fprintf(fptr, "%.16g\n", angleaxis[j]);
+ for (double coeff : angleaxis) {
+ fprintf(fptr, "%.16g\n", coeff);
}
}
@@ -297,14 +296,20 @@
CHECK_GE(point_sigma, 0.0);
CHECK_GE(rotation_sigma, 0.0);
CHECK_GE(translation_sigma, 0.0);
-
+ std::mt19937 prng;
+ std::normal_distribution<double> point_noise_distribution(0.0, point_sigma);
double* points = mutable_points();
if (point_sigma > 0) {
for (int i = 0; i < num_points_; ++i) {
- PerturbPoint3(point_sigma, points + 3 * i);
+ PerturbPoint3(std::bind(point_noise_distribution, std::ref(prng)),
+ points + 3 * i);
}
}
+ std::normal_distribution<double> rotation_noise_distribution(0.0,
+ point_sigma);
+ std::normal_distribution<double> translation_noise_distribution(
+ 0.0, translation_sigma);
for (int i = 0; i < num_cameras_; ++i) {
double* camera = mutable_cameras() + camera_block_size() * i;
@@ -314,12 +319,14 @@
// representation.
CameraToAngleAxisAndCenter(camera, angle_axis, center);
if (rotation_sigma > 0.0) {
- PerturbPoint3(rotation_sigma, angle_axis);
+ PerturbPoint3(std::bind(rotation_noise_distribution, std::ref(prng)),
+ angle_axis);
}
AngleAxisAndCenterToCamera(angle_axis, center, camera);
if (translation_sigma > 0.0) {
- PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
+ PerturbPoint3(std::bind(translation_noise_distribution, std::ref(prng)),
+ camera + camera_block_size() - 6);
}
}
}
@@ -331,5 +338,4 @@
delete[] parameters_;
}
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
diff --git a/third_party/ceres/examples/bal_problem.h b/third_party/ceres/examples/bal_problem.h
index e6d4ace..93effc3 100644
--- a/third_party/ceres/examples/bal_problem.h
+++ b/third_party/ceres/examples/bal_problem.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -39,8 +39,7 @@
#include <string>
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
class BALProblem {
public:
@@ -105,7 +104,6 @@
double* parameters_;
};
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // CERES_EXAMPLES_BAL_PROBLEM_H_
diff --git a/third_party/ceres/examples/bicubic_interpolation.cc b/third_party/ceres/examples/bicubic_interpolation.cc
new file mode 100644
index 0000000..21b3c7e
--- /dev/null
+++ b/third_party/ceres/examples/bicubic_interpolation.cc
@@ -0,0 +1,155 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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.
+//
+// Bicubic interpolation with automatic differentiation
+//
+// We will use estimation of 2d shift as a sample problem for bicubic
+// interpolation.
+//
+// Let us define f(x, y) = x * x - y * x + y * y
+// And optimize cost function sum_i [f(x_i + s_x, y_i + s_y) - v_i]^2
+//
+// Bicubic interpolation of f(x, y) will be exact, thus we can expect close to
+// perfect convergence
+
+#include <utility>
+
+#include "ceres/ceres.h"
+#include "ceres/cubic_interpolation.h"
+#include "glog/logging.h"
+
+using Grid = ceres::Grid2D<double>;
+using Interpolator = ceres::BiCubicInterpolator<Grid>;
+
+// Cost-function using autodiff interface of BiCubicInterpolator
+struct AutoDiffBiCubicCost {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ template <typename T>
+ bool operator()(const T* s, T* residual) const {
+ using Vector2T = Eigen::Matrix<T, 2, 1>;
+ Eigen::Map<const Vector2T> shift(s);
+
+ const Vector2T point = point_ + shift;
+
+ T v;
+ interpolator_.Evaluate(point.y(), point.x(), &v);
+
+ *residual = v - value_;
+ return true;
+ }
+
+ AutoDiffBiCubicCost(const Interpolator& interpolator,
+ Eigen::Vector2d point,
+ double value)
+ : point_(std::move(point)), value_(value), interpolator_(interpolator) {}
+
+ static ceres::CostFunction* Create(const Interpolator& interpolator,
+ const Eigen::Vector2d& point,
+ double value) {
+ return new ceres::AutoDiffCostFunction<AutoDiffBiCubicCost, 1, 2>(
+ interpolator, point, value);
+ }
+
+ const Eigen::Vector2d point_;
+ const double value_;
+ const Interpolator& interpolator_;
+};
+
+// Function for input data generation
+static double f(const double& x, const double& y) {
+ return x * x - y * x + y * y;
+}
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+ // Problem sizes
+ const int kGridRowsHalf = 9;
+ const int kGridColsHalf = 11;
+ const int kGridRows = 2 * kGridRowsHalf + 1;
+ const int kGridCols = 2 * kGridColsHalf + 1;
+ const int kPoints = 4;
+
+ const Eigen::Vector2d shift(1.234, 2.345);
+ const std::array<Eigen::Vector2d, kPoints> points = {
+ Eigen::Vector2d{-2., -3.},
+ Eigen::Vector2d{-2., 3.},
+ Eigen::Vector2d{2., 3.},
+ Eigen::Vector2d{2., -3.}};
+
+ // Data is a row-major array of kGridRows x kGridCols values of function
+ // f(x, y) on the grid, with x in {-kGridColsHalf, ..., +kGridColsHalf},
+ // and y in {-kGridRowsHalf, ..., +kGridRowsHalf}
+ double data[kGridRows * kGridCols];
+ for (int i = 0; i < kGridRows; ++i) {
+ for (int j = 0; j < kGridCols; ++j) {
+ // Using row-major order
+ int index = i * kGridCols + j;
+ double y = i - kGridRowsHalf;
+ double x = j - kGridColsHalf;
+
+ data[index] = f(x, y);
+ }
+ }
+ const Grid grid(data,
+ -kGridRowsHalf,
+ kGridRowsHalf + 1,
+ -kGridColsHalf,
+ kGridColsHalf + 1);
+ const Interpolator interpolator(grid);
+
+ Eigen::Vector2d shift_estimate(3.1415, 1.337);
+
+ ceres::Problem problem;
+ problem.AddParameterBlock(shift_estimate.data(), 2);
+
+ for (const auto& p : points) {
+ const Eigen::Vector2d shifted = p + shift;
+
+ const double v = f(shifted.x(), shifted.y());
+ problem.AddResidualBlock(AutoDiffBiCubicCost::Create(interpolator, p, v),
+ nullptr,
+ shift_estimate.data());
+ }
+
+ ceres::Solver::Options options;
+ options.minimizer_progress_to_stdout = true;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+ std::cout << summary.BriefReport() << '\n';
+
+ std::cout << "Bicubic interpolation with automatic derivatives:\n";
+ std::cout << "Estimated shift: " << shift_estimate.transpose()
+ << ", ground-truth: " << shift.transpose()
+ << " (error: " << (shift_estimate - shift).transpose() << ")"
+ << std::endl;
+
+ CHECK_LT((shift_estimate - shift).norm(), 1e-9);
+ return 0;
+}
diff --git a/third_party/ceres/examples/bicubic_interpolation_analytic.cc b/third_party/ceres/examples/bicubic_interpolation_analytic.cc
new file mode 100644
index 0000000..4b79d56
--- /dev/null
+++ b/third_party/ceres/examples/bicubic_interpolation_analytic.cc
@@ -0,0 +1,166 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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.
+//
+// Bicubic interpolation with analytic differentiation
+//
+// We will use estimation of 2d shift as a sample problem for bicubic
+// interpolation.
+//
+// Let us define f(x, y) = x * x - y * x + y * y
+// And optimize cost function sum_i [f(x_i + s_x, y_i + s_y) - v_i]^2
+//
+// Bicubic interpolation of f(x, y) will be exact, thus we can expect close to
+// perfect convergence
+
+#include <utility>
+
+#include "ceres/ceres.h"
+#include "ceres/cubic_interpolation.h"
+#include "glog/logging.h"
+
+using Grid = ceres::Grid2D<double>;
+using Interpolator = ceres::BiCubicInterpolator<Grid>;
+
+// Cost-function using analytic interface of BiCubicInterpolator
+struct AnalyticBiCubicCost : public ceres::CostFunction {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override {
+ Eigen::Map<const Eigen::Vector2d> shift(parameters[0]);
+
+ const Eigen::Vector2d point = point_ + shift;
+
+ double* f = residuals;
+ double* dfdr = nullptr;
+ double* dfdc = nullptr;
+ if (jacobians && jacobians[0]) {
+ dfdc = jacobians[0];
+ dfdr = dfdc + 1;
+ }
+
+ interpolator_.Evaluate(point.y(), point.x(), f, dfdr, dfdc);
+
+ if (residuals) {
+ *f -= value_;
+ }
+ return true;
+ }
+
+ AnalyticBiCubicCost(const Interpolator& interpolator,
+ Eigen::Vector2d point,
+ double value)
+ : point_(std::move(point)), value_(value), interpolator_(interpolator) {
+ set_num_residuals(1);
+ *mutable_parameter_block_sizes() = {2};
+ }
+
+ static ceres::CostFunction* Create(const Interpolator& interpolator,
+ const Eigen::Vector2d& point,
+ double value) {
+ return new AnalyticBiCubicCost(interpolator, point, value);
+ }
+
+ const Eigen::Vector2d point_;
+ const double value_;
+ const Interpolator& interpolator_;
+};
+
+// Function for input data generation
+static double f(const double& x, const double& y) {
+ return x * x - y * x + y * y;
+}
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+ // Problem sizes
+ const int kGridRowsHalf = 9;
+ const int kGridColsHalf = 11;
+ const int kGridRows = 2 * kGridRowsHalf + 1;
+ const int kGridCols = 2 * kGridColsHalf + 1;
+ const int kPoints = 4;
+
+ const Eigen::Vector2d shift(1.234, 2.345);
+ const std::array<Eigen::Vector2d, kPoints> points = {
+ Eigen::Vector2d{-2., -3.},
+ Eigen::Vector2d{-2., 3.},
+ Eigen::Vector2d{2., 3.},
+ Eigen::Vector2d{2., -3.}};
+
+ // Data is a row-major array of kGridRows x kGridCols values of function
+ // f(x, y) on the grid, with x in {-kGridColsHalf, ..., +kGridColsHalf},
+ // and y in {-kGridRowsHalf, ..., +kGridRowsHalf}
+ double data[kGridRows * kGridCols];
+ for (int i = 0; i < kGridRows; ++i) {
+ for (int j = 0; j < kGridCols; ++j) {
+ // Using row-major order
+ int index = i * kGridCols + j;
+ double y = i - kGridRowsHalf;
+ double x = j - kGridColsHalf;
+
+ data[index] = f(x, y);
+ }
+ }
+ const Grid grid(data,
+ -kGridRowsHalf,
+ kGridRowsHalf + 1,
+ -kGridColsHalf,
+ kGridColsHalf + 1);
+ const Interpolator interpolator(grid);
+
+ Eigen::Vector2d shift_estimate(3.1415, 1.337);
+
+ ceres::Problem problem;
+ problem.AddParameterBlock(shift_estimate.data(), 2);
+
+ for (const auto& p : points) {
+ const Eigen::Vector2d shifted = p + shift;
+
+ const double v = f(shifted.x(), shifted.y());
+ problem.AddResidualBlock(AnalyticBiCubicCost::Create(interpolator, p, v),
+ nullptr,
+ shift_estimate.data());
+ }
+
+ ceres::Solver::Options options;
+ options.minimizer_progress_to_stdout = true;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+ std::cout << summary.BriefReport() << '\n';
+
+ std::cout << "Bicubic interpolation with analytic derivatives:\n";
+ std::cout << "Estimated shift: " << shift_estimate.transpose()
+ << ", ground-truth: " << shift.transpose()
+ << " (error: " << (shift_estimate - shift).transpose() << ")"
+ << std::endl;
+
+ CHECK_LT((shift_estimate - shift).norm(), 1e-9);
+ return 0;
+}
diff --git a/third_party/ceres/examples/bundle_adjuster.cc b/third_party/ceres/examples/bundle_adjuster.cc
index e7b154e..582ae2e 100644
--- a/third_party/ceres/examples/bundle_adjuster.cc
+++ b/third_party/ceres/examples/bundle_adjuster.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -55,7 +55,9 @@
#include <cmath>
#include <cstdio>
#include <cstdlib>
+#include <memory>
#include <string>
+#include <thread>
#include <vector>
#include "bal_problem.h"
@@ -80,34 +82,45 @@
"automatic, cameras, points, cameras,points, points,cameras");
DEFINE_string(linear_solver, "sparse_schur", "Options are: "
- "sparse_schur, dense_schur, iterative_schur, sparse_normal_cholesky, "
- "dense_qr, dense_normal_cholesky and cgnr.");
+ "sparse_schur, dense_schur, iterative_schur, "
+ "sparse_normal_cholesky, dense_qr, dense_normal_cholesky, "
+ "and cgnr.");
DEFINE_bool(explicit_schur_complement, false, "If using ITERATIVE_SCHUR "
"then explicitly compute the Schur complement.");
DEFINE_string(preconditioner, "jacobi", "Options are: "
- "identity, jacobi, schur_jacobi, cluster_jacobi, "
+ "identity, jacobi, schur_jacobi, schur_power_series_expansion, cluster_jacobi, "
"cluster_tridiagonal.");
DEFINE_string(visibility_clustering, "canonical_views",
"single_linkage, canonical_views");
+DEFINE_bool(use_spse_initialization, false,
+ "Use power series expansion to initialize the solution in ITERATIVE_SCHUR linear solver.");
DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
- "Options are: suite_sparse and cx_sparse.");
+ "Options are: suite_sparse, accelerate_sparse, eigen_sparse, and "
+ "cuda_sparse.");
DEFINE_string(dense_linear_algebra_library, "eigen",
- "Options are: eigen and lapack.");
-DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
+ "Options are: eigen, lapack, and cuda");
+DEFINE_string(ordering_type, "amd", "Options are: amd, nesdis");
+DEFINE_string(linear_solver_ordering, "user",
+ "Options are: automatic and user");
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 "
"accuracy of each linear solve of the truncated newton step. "
"Changing this parameter can affect solve performance.");
-DEFINE_int32(num_threads, 1, "Number of threads.");
+DEFINE_int32(num_threads, -1, "Number of threads. -1 = std::thread::hardware_concurrency.");
DEFINE_int32(num_iterations, 5, "Number of iterations.");
+DEFINE_int32(max_linear_solver_iterations, 500, "Maximum number of iterations"
+ " for solution of linear system.");
+DEFINE_double(spse_tolerance, 0.1,
+ "Tolerance to reach during the iterations of power series expansion initialization or preconditioning.");
+DEFINE_int32(max_num_spse_iterations, 5,
+ "Maximum number of iterations for power series expansion initialization or preconditioning.");
DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds.");
DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
" nonmonotic steps.");
@@ -120,7 +133,7 @@
"perturbation.");
DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
"of the pseudo random number generator used to generate "
- "the pertubations.");
+ "the perturbations.");
DEFINE_bool(line_search, false, "Use a line search instead of trust region "
"algorithm.");
DEFINE_bool(mixed_precision_solves, false, "Use mixed precision solves.");
@@ -128,29 +141,41 @@
DEFINE_string(initial_ply, "", "Export the BAL file data as a PLY file.");
DEFINE_string(final_ply, "", "Export the refined BAL file data as a PLY "
"file.");
-
// clang-format on
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
namespace {
void SetLinearSolver(Solver::Options* options) {
- CHECK(StringToLinearSolverType(FLAGS_linear_solver,
+ CHECK(StringToLinearSolverType(CERES_GET_FLAG(FLAGS_linear_solver),
&options->linear_solver_type));
- CHECK(StringToPreconditionerType(FLAGS_preconditioner,
+ CHECK(StringToPreconditionerType(CERES_GET_FLAG(FLAGS_preconditioner),
&options->preconditioner_type));
- CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
- &options->visibility_clustering_type));
+ CHECK(StringToVisibilityClusteringType(
+ CERES_GET_FLAG(FLAGS_visibility_clustering),
+ &options->visibility_clustering_type));
CHECK(StringToSparseLinearAlgebraLibraryType(
- FLAGS_sparse_linear_algebra_library,
+ CERES_GET_FLAG(FLAGS_sparse_linear_algebra_library),
&options->sparse_linear_algebra_library_type));
CHECK(StringToDenseLinearAlgebraLibraryType(
- FLAGS_dense_linear_algebra_library,
+ CERES_GET_FLAG(FLAGS_dense_linear_algebra_library),
&options->dense_linear_algebra_library_type));
- options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;
- options->use_mixed_precision_solves = FLAGS_mixed_precision_solves;
- options->max_num_refinement_iterations = FLAGS_max_num_refinement_iterations;
+ CHECK(
+ StringToLinearSolverOrderingType(CERES_GET_FLAG(FLAGS_ordering_type),
+ &options->linear_solver_ordering_type));
+ options->use_explicit_schur_complement =
+ CERES_GET_FLAG(FLAGS_explicit_schur_complement);
+ options->use_mixed_precision_solves =
+ CERES_GET_FLAG(FLAGS_mixed_precision_solves);
+ options->max_num_refinement_iterations =
+ CERES_GET_FLAG(FLAGS_max_num_refinement_iterations);
+ options->max_linear_solver_iterations =
+ CERES_GET_FLAG(FLAGS_max_linear_solver_iterations);
+ options->use_spse_initialization =
+ CERES_GET_FLAG(FLAGS_use_spse_initialization);
+ options->spse_tolerance = CERES_GET_FLAG(FLAGS_spse_tolerance);
+ options->max_num_spse_iterations =
+ CERES_GET_FLAG(FLAGS_max_num_spse_iterations);
}
void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
@@ -163,23 +188,27 @@
double* cameras = bal_problem->mutable_cameras();
if (options->use_inner_iterations) {
- if (FLAGS_blocks_for_inner_iterations == "cameras") {
+ if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == "cameras") {
LOG(INFO) << "Camera blocks for inner iterations";
- options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ options->inner_iteration_ordering =
+ std::make_shared<ParameterBlockOrdering>();
for (int i = 0; i < num_cameras; ++i) {
options->inner_iteration_ordering->AddElementToGroup(
cameras + camera_block_size * i, 0);
}
- } else if (FLAGS_blocks_for_inner_iterations == "points") {
+ } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == "points") {
LOG(INFO) << "Point blocks for inner iterations";
- options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ options->inner_iteration_ordering =
+ std::make_shared<ParameterBlockOrdering>();
for (int i = 0; i < num_points; ++i) {
options->inner_iteration_ordering->AddElementToGroup(
points + point_block_size * i, 0);
}
- } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {
+ } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) ==
+ "cameras,points") {
LOG(INFO) << "Camera followed by point blocks for inner iterations";
- options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ options->inner_iteration_ordering =
+ std::make_shared<ParameterBlockOrdering>();
for (int i = 0; i < num_cameras; ++i) {
options->inner_iteration_ordering->AddElementToGroup(
cameras + camera_block_size * i, 0);
@@ -188,9 +217,11 @@
options->inner_iteration_ordering->AddElementToGroup(
points + point_block_size * i, 1);
}
- } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
+ } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) ==
+ "points,cameras") {
LOG(INFO) << "Point followed by camera blocks for inner iterations";
- options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ options->inner_iteration_ordering =
+ std::make_shared<ParameterBlockOrdering>();
for (int i = 0; i < num_cameras; ++i) {
options->inner_iteration_ordering->AddElementToGroup(
cameras + camera_block_size * i, 1);
@@ -199,11 +230,12 @@
options->inner_iteration_ordering->AddElementToGroup(
points + point_block_size * i, 0);
}
- } else if (FLAGS_blocks_for_inner_iterations == "automatic") {
+ } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) ==
+ "automatic") {
LOG(INFO) << "Choosing automatic blocks for inner iterations";
} else {
LOG(FATAL) << "Unknown block type for inner iterations: "
- << FLAGS_blocks_for_inner_iterations;
+ << CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations);
}
}
@@ -213,46 +245,54 @@
// ITERATIVE_SCHUR solvers make use of this specialized
// structure.
//
- // This can either be done by specifying Options::ordering_type =
- // ceres::SCHUR, in which case Ceres will automatically determine
- // the right ParameterBlock ordering, or by manually specifying a
- // suitable ordering vector and defining
- // Options::num_eliminate_blocks.
- if (FLAGS_ordering == "automatic") {
- return;
+ // This can either be done by specifying a
+ // Options::linear_solver_ordering or having Ceres figure it out
+ // automatically using a greedy maximum independent set algorithm.
+ if (CERES_GET_FLAG(FLAGS_linear_solver_ordering) == "user") {
+ auto* ordering = new ceres::ParameterBlockOrdering;
+
+ // The points come before the cameras.
+ for (int i = 0; i < num_points; ++i) {
+ ordering->AddElementToGroup(points + point_block_size * i, 0);
+ }
+
+ for (int i = 0; i < num_cameras; ++i) {
+ // When using axis-angle, there is a single parameter block for
+ // the entire camera.
+ ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
+ }
+
+ options->linear_solver_ordering.reset(ordering);
}
-
- ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering;
-
- // The points come before the cameras.
- for (int i = 0; i < num_points; ++i) {
- ordering->AddElementToGroup(points + point_block_size * i, 0);
- }
-
- for (int i = 0; i < num_cameras; ++i) {
- // When using axis-angle, there is a single parameter block for
- // the entire camera.
- ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
- }
-
- options->linear_solver_ordering.reset(ordering);
}
void SetMinimizerOptions(Solver::Options* options) {
- options->max_num_iterations = FLAGS_num_iterations;
+ options->max_num_iterations = CERES_GET_FLAG(FLAGS_num_iterations);
options->minimizer_progress_to_stdout = true;
- options->num_threads = FLAGS_num_threads;
- options->eta = FLAGS_eta;
- options->max_solver_time_in_seconds = FLAGS_max_solver_time;
- options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
- if (FLAGS_line_search) {
+ if (CERES_GET_FLAG(FLAGS_num_threads) == -1) {
+ const int num_available_threads =
+ static_cast<int>(std::thread::hardware_concurrency());
+ if (num_available_threads > 0) {
+ options->num_threads = num_available_threads;
+ }
+ } else {
+ options->num_threads = CERES_GET_FLAG(FLAGS_num_threads);
+ }
+ CHECK_GE(options->num_threads, 1);
+
+ options->eta = CERES_GET_FLAG(FLAGS_eta);
+ options->max_solver_time_in_seconds = CERES_GET_FLAG(FLAGS_max_solver_time);
+ options->use_nonmonotonic_steps = CERES_GET_FLAG(FLAGS_nonmonotonic_steps);
+ if (CERES_GET_FLAG(FLAGS_line_search)) {
options->minimizer_type = ceres::LINE_SEARCH;
}
- CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
- &options->trust_region_strategy_type));
- CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
- options->use_inner_iterations = FLAGS_inner_iterations;
+ CHECK(StringToTrustRegionStrategyType(
+ CERES_GET_FLAG(FLAGS_trust_region_strategy),
+ &options->trust_region_strategy_type));
+ CHECK(
+ StringToDoglegType(CERES_GET_FLAG(FLAGS_dogleg), &options->dogleg_type));
+ options->use_inner_iterations = CERES_GET_FLAG(FLAGS_inner_iterations);
}
void SetSolverOptionsFromFlags(BALProblem* bal_problem,
@@ -276,16 +316,17 @@
CostFunction* cost_function;
// Each Residual block takes a point and a camera as input and
// outputs a 2 dimensional residual.
- cost_function = (FLAGS_use_quaternions)
+ cost_function = (CERES_GET_FLAG(FLAGS_use_quaternions))
? SnavelyReprojectionErrorWithQuaternions::Create(
observations[2 * i + 0], observations[2 * i + 1])
: SnavelyReprojectionError::Create(
observations[2 * i + 0], observations[2 * i + 1]);
// If enabled use Huber's loss function.
- LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
+ LossFunction* loss_function =
+ CERES_GET_FLAG(FLAGS_robustify) ? new HuberLoss(1.0) : nullptr;
- // Each observation correponds to a pair of a camera and a point
+ // Each observation corresponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
double* camera =
@@ -294,60 +335,60 @@
problem->AddResidualBlock(cost_function, loss_function, camera, point);
}
- if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
- LocalParameterization* camera_parameterization =
- new ProductParameterization(new QuaternionParameterization(),
- new IdentityParameterization(6));
+ if (CERES_GET_FLAG(FLAGS_use_quaternions) &&
+ CERES_GET_FLAG(FLAGS_use_manifolds)) {
+ Manifold* camera_manifold =
+ new ProductManifold<QuaternionManifold, 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);
}
}
}
void SolveProblem(const char* filename) {
- BALProblem bal_problem(filename, FLAGS_use_quaternions);
+ BALProblem bal_problem(filename, CERES_GET_FLAG(FLAGS_use_quaternions));
- if (!FLAGS_initial_ply.empty()) {
- bal_problem.WriteToPLYFile(FLAGS_initial_ply);
+ if (!CERES_GET_FLAG(FLAGS_initial_ply).empty()) {
+ bal_problem.WriteToPLYFile(CERES_GET_FLAG(FLAGS_initial_ply));
}
Problem problem;
- srand(FLAGS_random_seed);
+ srand(CERES_GET_FLAG(FLAGS_random_seed));
bal_problem.Normalize();
- bal_problem.Perturb(
- FLAGS_rotation_sigma, FLAGS_translation_sigma, FLAGS_point_sigma);
+ bal_problem.Perturb(CERES_GET_FLAG(FLAGS_rotation_sigma),
+ CERES_GET_FLAG(FLAGS_translation_sigma),
+ CERES_GET_FLAG(FLAGS_point_sigma));
BuildProblem(&bal_problem, &problem);
Solver::Options options;
SetSolverOptionsFromFlags(&bal_problem, &options);
options.gradient_tolerance = 1e-16;
options.function_tolerance = 1e-16;
+ options.parameter_tolerance = 1e-16;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
- if (!FLAGS_final_ply.empty()) {
- bal_problem.WriteToPLYFile(FLAGS_final_ply);
+ if (!CERES_GET_FLAG(FLAGS_final_ply).empty()) {
+ bal_problem.WriteToPLYFile(CERES_GET_FLAG(FLAGS_final_ply));
}
}
} // namespace
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
int main(int argc, char** argv) {
GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
- if (FLAGS_input.empty()) {
+ if (CERES_GET_FLAG(FLAGS_input).empty()) {
LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem";
return 1;
}
- CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
- << "--use_local_parameterization can only be used with "
- << "--use_quaternions.";
- ceres::examples::SolveProblem(FLAGS_input.c_str());
+ CHECK(CERES_GET_FLAG(FLAGS_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/third_party/ceres/examples/circle_fit.cc b/third_party/ceres/examples/circle_fit.cc
index c542475..fd848d9 100644
--- a/third_party/ceres/examples/circle_fit.cc
+++ b/third_party/ceres/examples/circle_fit.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -57,14 +57,6 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
-using ceres::AutoDiffCostFunction;
-using ceres::CauchyLoss;
-using ceres::CostFunction;
-using ceres::LossFunction;
-using ceres::Problem;
-using ceres::Solve;
-using ceres::Solver;
-
DEFINE_double(robust_threshold,
0.0,
"Robust loss parameter. Set to 0 for normal squared error (no "
@@ -128,21 +120,21 @@
// Parameterize r as m^2 so that it can't be negative.
double m = sqrt(r);
- Problem problem;
+ ceres::Problem problem;
// Configure the loss function.
- LossFunction* loss = NULL;
- if (FLAGS_robust_threshold) {
- loss = new CauchyLoss(FLAGS_robust_threshold);
+ ceres::LossFunction* loss = nullptr;
+ if (CERES_GET_FLAG(FLAGS_robust_threshold)) {
+ loss = new ceres::CauchyLoss(CERES_GET_FLAG(FLAGS_robust_threshold));
}
// Add the residuals.
double xx, yy;
int num_points = 0;
while (scanf("%lf %lf\n", &xx, &yy) == 2) {
- CostFunction* cost =
- new AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(
- new DistanceFromCircleCost(xx, yy));
+ ceres::CostFunction* cost =
+ new ceres::AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(xx,
+ yy);
problem.AddResidualBlock(cost, loss, &x, &y, &m);
num_points++;
}
@@ -150,11 +142,11 @@
std::cout << "Got " << num_points << " points.\n";
// Build and solve the problem.
- Solver::Options options;
+ ceres::Solver::Options options;
options.max_num_iterations = 500;
options.linear_solver_type = ceres::DENSE_QR;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
// Recover r from m.
r = m * m;
diff --git a/third_party/ceres/examples/curve_fitting.cc b/third_party/ceres/examples/curve_fitting.cc
index fc7ff94..105402e 100644
--- a/third_party/ceres/examples/curve_fitting.cc
+++ b/third_party/ceres/examples/curve_fitting.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -27,16 +27,13 @@
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// This example fits the curve f(x;m,c) = e^(m * x + c) to data, minimizing the
+// sum squared loss.
#include "ceres/ceres.h"
#include "glog/logging.h"
-using ceres::AutoDiffCostFunction;
-using ceres::CostFunction;
-using ceres::Problem;
-using ceres::Solve;
-using ceres::Solver;
-
// Data generated using the following octave code.
// randn('seed', 23497);
// m = 0.3;
@@ -137,28 +134,30 @@
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
- double m = 0.0;
- double c = 0.0;
+ const double initial_m = 0.0;
+ const double initial_c = 0.0;
+ double m = initial_m;
+ double c = initial_c;
- Problem problem;
+ ceres::Problem problem;
for (int i = 0; i < kNumObservations; ++i) {
problem.AddResidualBlock(
- new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
- new ExponentialResidual(data[2 * i], data[2 * i + 1])),
- NULL,
+ new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
+ data[2 * i], data[2 * i + 1]),
+ nullptr,
&m,
&c);
}
- Solver::Options options;
+ ceres::Solver::Options options;
options.max_num_iterations = 25;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
- std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n";
+ std::cout << "Initial m: " << initial_m << " c: " << initial_c << "\n";
std::cout << "Final m: " << m << " c: " << c << "\n";
return 0;
}
diff --git a/third_party/ceres/examples/denoising.cc b/third_party/ceres/examples/denoising.cc
index 61ea2c6..dc13d19 100644
--- a/third_party/ceres/examples/denoising.cc
+++ b/third_party/ceres/examples/denoising.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -33,7 +33,7 @@
// Note that for good denoising results the weighting between the data term
// and the Fields of Experts term needs to be adjusted. This is discussed
// in [1]. This program assumes Gaussian noise. The noise model can be changed
-// by substituing another function for QuadraticCostFunction.
+// by substituting another function for QuadraticCostFunction.
//
// [1] S. Roth and M.J. Black. "Fields of Experts." International Journal of
// Computer Vision, 82(2):205--229, 2009.
@@ -102,8 +102,7 @@
"The fraction of residual blocks to use for the"
" subset preconditioner.");
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
namespace {
// This cost function is used to build the data term.
@@ -113,12 +112,12 @@
class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
public:
QuadraticCostFunction(double a, double b) : sqrta_(std::sqrt(a)), b_(b) {}
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
+ bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override {
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;
@@ -134,13 +133,14 @@
Problem* problem,
PGMImage<double>* solution) {
// Create the data term
- CHECK_GT(FLAGS_sigma, 0.0);
- const double coefficient = 1 / (2.0 * FLAGS_sigma * FLAGS_sigma);
+ CHECK_GT(CERES_GET_FLAG(FLAGS_sigma), 0.0);
+ const double coefficient =
+ 1 / (2.0 * CERES_GET_FLAG(FLAGS_sigma) * CERES_GET_FLAG(FLAGS_sigma));
for (int index = 0; index < image.NumPixels(); ++index) {
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
@@ -175,31 +175,35 @@
}
void SetLinearSolver(Solver::Options* options) {
- CHECK(StringToLinearSolverType(FLAGS_linear_solver,
+ CHECK(StringToLinearSolverType(CERES_GET_FLAG(FLAGS_linear_solver),
&options->linear_solver_type));
- CHECK(StringToPreconditionerType(FLAGS_preconditioner,
+ CHECK(StringToPreconditionerType(CERES_GET_FLAG(FLAGS_preconditioner),
&options->preconditioner_type));
CHECK(StringToSparseLinearAlgebraLibraryType(
- FLAGS_sparse_linear_algebra_library,
+ CERES_GET_FLAG(FLAGS_sparse_linear_algebra_library),
&options->sparse_linear_algebra_library_type));
- options->use_mixed_precision_solves = FLAGS_mixed_precision_solves;
- options->max_num_refinement_iterations = FLAGS_max_num_refinement_iterations;
+ options->use_mixed_precision_solves =
+ CERES_GET_FLAG(FLAGS_mixed_precision_solves);
+ options->max_num_refinement_iterations =
+ CERES_GET_FLAG(FLAGS_max_num_refinement_iterations);
}
void SetMinimizerOptions(Solver::Options* options) {
- options->max_num_iterations = FLAGS_num_iterations;
+ options->max_num_iterations = CERES_GET_FLAG(FLAGS_num_iterations);
options->minimizer_progress_to_stdout = true;
- options->num_threads = FLAGS_num_threads;
- options->eta = FLAGS_eta;
- options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
- if (FLAGS_line_search) {
+ options->num_threads = CERES_GET_FLAG(FLAGS_num_threads);
+ options->eta = CERES_GET_FLAG(FLAGS_eta);
+ options->use_nonmonotonic_steps = CERES_GET_FLAG(FLAGS_nonmonotonic_steps);
+ if (CERES_GET_FLAG(FLAGS_line_search)) {
options->minimizer_type = ceres::LINE_SEARCH;
}
- CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
- &options->trust_region_strategy_type));
- CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
- options->use_inner_iterations = FLAGS_inner_iterations;
+ CHECK(StringToTrustRegionStrategyType(
+ CERES_GET_FLAG(FLAGS_trust_region_strategy),
+ &options->trust_region_strategy_type));
+ CHECK(
+ StringToDoglegType(CERES_GET_FLAG(FLAGS_dogleg), &options->dogleg_type));
+ options->use_inner_iterations = CERES_GET_FLAG(FLAGS_inner_iterations);
}
// Solves the FoE problem using Ceres and post-processes it to make sure the
@@ -226,7 +230,7 @@
std::default_random_engine engine;
std::uniform_real_distribution<> distribution(0, 1); // rage 0 - 1
for (auto residual_block : residual_blocks) {
- if (distribution(engine) <= FLAGS_subset_fraction) {
+ if (distribution(engine) <= CERES_GET_FLAG(FLAGS_subset_fraction)) {
options.residual_blocks_for_subset_preconditioner.insert(
residual_block);
}
@@ -247,20 +251,19 @@
}
} // namespace
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
int main(int argc, char** argv) {
using namespace ceres::examples;
GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
- if (FLAGS_input.empty()) {
+ if (CERES_GET_FLAG(FLAGS_input).empty()) {
std::cerr << "Please provide an image file name using -input.\n";
return 1;
}
- if (FLAGS_foe_file.empty()) {
+ if (CERES_GET_FLAG(FLAGS_foe_file).empty()) {
std::cerr << "Please provide a Fields of Experts file name using -foe_file."
"\n";
return 1;
@@ -268,15 +271,16 @@
// Load the Fields of Experts filters from file.
FieldsOfExperts foe;
- if (!foe.LoadFromFile(FLAGS_foe_file)) {
- std::cerr << "Loading \"" << FLAGS_foe_file << "\" failed.\n";
+ if (!foe.LoadFromFile(CERES_GET_FLAG(FLAGS_foe_file))) {
+ std::cerr << "Loading \"" << CERES_GET_FLAG(FLAGS_foe_file)
+ << "\" failed.\n";
return 2;
}
// Read the images
- PGMImage<double> image(FLAGS_input);
+ PGMImage<double> image(CERES_GET_FLAG(FLAGS_input));
if (image.width() == 0) {
- std::cerr << "Reading \"" << FLAGS_input << "\" failed.\n";
+ std::cerr << "Reading \"" << CERES_GET_FLAG(FLAGS_input) << "\" failed.\n";
return 3;
}
PGMImage<double> solution(image.width(), image.height());
@@ -287,9 +291,9 @@
SolveProblem(&problem, &solution);
- if (!FLAGS_output.empty()) {
- CHECK(solution.WriteToFile(FLAGS_output))
- << "Writing \"" << FLAGS_output << "\" failed.";
+ if (!CERES_GET_FLAG(FLAGS_output).empty()) {
+ CHECK(solution.WriteToFile(CERES_GET_FLAG(FLAGS_output)))
+ << "Writing \"" << CERES_GET_FLAG(FLAGS_output) << "\" failed.";
}
return 0;
diff --git a/third_party/ceres/examples/ellipse_approximation.cc b/third_party/ceres/examples/ellipse_approximation.cc
index 74782f4..6fa8f1c 100644
--- a/third_party/ceres/examples/ellipse_approximation.cc
+++ b/third_party/ceres/examples/ellipse_approximation.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -36,6 +36,7 @@
// dense but dynamically sparse.
#include <cmath>
+#include <utility>
#include <vector>
#include "ceres/ceres.h"
@@ -275,8 +276,8 @@
class PointToLineSegmentContourCostFunction : public ceres::CostFunction {
public:
PointToLineSegmentContourCostFunction(const int num_segments,
- const Eigen::Vector2d& y)
- : num_segments_(num_segments), y_(y) {
+ Eigen::Vector2d y)
+ : num_segments_(num_segments), y_(std::move(y)) {
// The first parameter is the preimage position.
mutable_parameter_block_sizes()->push_back(1);
// The next parameters are the control points for the line segment contour.
@@ -286,9 +287,9 @@
set_num_residuals(2);
}
- virtual bool Evaluate(const double* const* x,
- double* residuals,
- double** jacobians) const {
+ bool Evaluate(const double* const* x,
+ double* residuals,
+ double** jacobians) const override {
// Convert the preimage position `t` into a segment index `i0` and the
// line segment interpolation parameter `u`. `i1` is the index of the next
// control point.
@@ -302,16 +303,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);
@@ -353,7 +354,7 @@
static ceres::CostFunction* Create(const double sqrt_weight) {
return new ceres::AutoDiffCostFunction<EuclideanDistanceFunctor, 2, 2, 2>(
- new EuclideanDistanceFunctor(sqrt_weight));
+ sqrt_weight);
}
private:
@@ -385,8 +386,8 @@
// Eigen::MatrixXd is column major so we define our own MatrixXd which is
// row major. Eigen::VectorXd can be used directly.
- typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
- MatrixXd;
+ using MatrixXd =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using Eigen::VectorXd;
// `X` is the matrix of control points which make up the contour of line
@@ -395,7 +396,7 @@
//
// Initialize `X` to points on the unit circle.
VectorXd w(num_segments + 1);
- w.setLinSpaced(num_segments + 1, 0.0, 2.0 * M_PI);
+ w.setLinSpaced(num_segments + 1, 0.0, 2.0 * ceres::constants::pi);
w.conservativeResize(num_segments);
MatrixXd X(num_segments, 2);
X.col(0) = w.array().cos();
@@ -404,9 +405,9 @@
// Each data point has an associated preimage position on the line segment
// contour. For each data point we initialize the preimage positions to
// the index of the closest control point.
- const int num_observations = kY.rows();
+ const int64_t num_observations = kY.rows();
VectorXd t(num_observations);
- for (int i = 0; i < num_observations; ++i) {
+ for (int64_t i = 0; i < num_observations; ++i) {
(X.rowwise() - kY.row(i)).rowwise().squaredNorm().minCoeff(&t[i]);
}
@@ -415,7 +416,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 +424,7 @@
parameter_blocks[0] = &t[i];
problem.AddResidualBlock(
PointToLineSegmentContourCostFunction::Create(num_segments, kY.row(i)),
- NULL,
+ nullptr,
parameter_blocks);
}
@@ -431,7 +432,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/third_party/ceres/examples/evaluation_callback_example.cc b/third_party/ceres/examples/evaluation_callback_example.cc
new file mode 100644
index 0000000..6dbf932
--- /dev/null
+++ b/third_party/ceres/examples/evaluation_callback_example.cc
@@ -0,0 +1,257 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: sameeragarwal@google.com (Sameer Agarwal)
+//
+// This example illustrates the use of the EvaluationCallback, which can be used
+// to perform high performance computation of the residual and Jacobians outside
+// Ceres (in this case using Eigen's vectorized code) and then the CostFunctions
+// just copy these computed residuals and Jacobians appropriately and pass them
+// to Ceres Solver.
+//
+// The results of running this example should be identical to the results
+// obtained by running curve_fitting.cc. The only difference between the two
+// examples is how the residuals and Jacobians are computed.
+//
+// The observant reader will note that both here and curve_fitting.cc instead of
+// creating one ResidualBlock for each observation one can just do one
+// ResidualBlock/CostFunction for the entire problem. The reason for keeping one
+// residual per observation is that it is what is needed if and when we need to
+// introduce a loss function which is what we do in robust_curve_fitting.cc
+
+#include <iostream>
+
+#include "Eigen/Core"
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+// Data generated using the following octave code.
+// randn('seed', 23497);
+// m = 0.3;
+// c = 0.1;
+// x=[0:0.075:5];
+// y = exp(m * x + c);
+// noise = randn(size(x)) * 0.2;
+// y_observed = y + noise;
+// data = [x', y_observed'];
+
+const int kNumObservations = 67;
+// clang-format off
+const double data[] = {
+ 0.000000e+00, 1.133898e+00,
+ 7.500000e-02, 1.334902e+00,
+ 1.500000e-01, 1.213546e+00,
+ 2.250000e-01, 1.252016e+00,
+ 3.000000e-01, 1.392265e+00,
+ 3.750000e-01, 1.314458e+00,
+ 4.500000e-01, 1.472541e+00,
+ 5.250000e-01, 1.536218e+00,
+ 6.000000e-01, 1.355679e+00,
+ 6.750000e-01, 1.463566e+00,
+ 7.500000e-01, 1.490201e+00,
+ 8.250000e-01, 1.658699e+00,
+ 9.000000e-01, 1.067574e+00,
+ 9.750000e-01, 1.464629e+00,
+ 1.050000e+00, 1.402653e+00,
+ 1.125000e+00, 1.713141e+00,
+ 1.200000e+00, 1.527021e+00,
+ 1.275000e+00, 1.702632e+00,
+ 1.350000e+00, 1.423899e+00,
+ 1.425000e+00, 1.543078e+00,
+ 1.500000e+00, 1.664015e+00,
+ 1.575000e+00, 1.732484e+00,
+ 1.650000e+00, 1.543296e+00,
+ 1.725000e+00, 1.959523e+00,
+ 1.800000e+00, 1.685132e+00,
+ 1.875000e+00, 1.951791e+00,
+ 1.950000e+00, 2.095346e+00,
+ 2.025000e+00, 2.361460e+00,
+ 2.100000e+00, 2.169119e+00,
+ 2.175000e+00, 2.061745e+00,
+ 2.250000e+00, 2.178641e+00,
+ 2.325000e+00, 2.104346e+00,
+ 2.400000e+00, 2.584470e+00,
+ 2.475000e+00, 1.914158e+00,
+ 2.550000e+00, 2.368375e+00,
+ 2.625000e+00, 2.686125e+00,
+ 2.700000e+00, 2.712395e+00,
+ 2.775000e+00, 2.499511e+00,
+ 2.850000e+00, 2.558897e+00,
+ 2.925000e+00, 2.309154e+00,
+ 3.000000e+00, 2.869503e+00,
+ 3.075000e+00, 3.116645e+00,
+ 3.150000e+00, 3.094907e+00,
+ 3.225000e+00, 2.471759e+00,
+ 3.300000e+00, 3.017131e+00,
+ 3.375000e+00, 3.232381e+00,
+ 3.450000e+00, 2.944596e+00,
+ 3.525000e+00, 3.385343e+00,
+ 3.600000e+00, 3.199826e+00,
+ 3.675000e+00, 3.423039e+00,
+ 3.750000e+00, 3.621552e+00,
+ 3.825000e+00, 3.559255e+00,
+ 3.900000e+00, 3.530713e+00,
+ 3.975000e+00, 3.561766e+00,
+ 4.050000e+00, 3.544574e+00,
+ 4.125000e+00, 3.867945e+00,
+ 4.200000e+00, 4.049776e+00,
+ 4.275000e+00, 3.885601e+00,
+ 4.350000e+00, 4.110505e+00,
+ 4.425000e+00, 4.345320e+00,
+ 4.500000e+00, 4.161241e+00,
+ 4.575000e+00, 4.363407e+00,
+ 4.650000e+00, 4.161576e+00,
+ 4.725000e+00, 4.619728e+00,
+ 4.800000e+00, 4.737410e+00,
+ 4.875000e+00, 4.727863e+00,
+ 4.950000e+00, 4.669206e+00,
+};
+// clang-format on
+
+// This implementation of the EvaluationCallback interface also stores the
+// residuals and Jacobians that the CostFunction copies their values from.
+class MyEvaluationCallback : public ceres::EvaluationCallback {
+ public:
+ // m and c are passed by reference so that we have access to their values as
+ // they evolve over time through the course of optimization.
+ MyEvaluationCallback(const double& m, const double& c) : m_(m), c_(c) {
+ x_ = Eigen::VectorXd::Zero(kNumObservations);
+ y_ = Eigen::VectorXd::Zero(kNumObservations);
+ residuals_ = Eigen::VectorXd::Zero(kNumObservations);
+ jacobians_ = Eigen::MatrixXd::Zero(kNumObservations, 2);
+ for (int i = 0; i < kNumObservations; ++i) {
+ x_[i] = data[2 * i];
+ y_[i] = data[2 * i + 1];
+ }
+ PrepareForEvaluation(true, true);
+ }
+
+ void PrepareForEvaluation(bool evaluate_jacobians,
+ bool new_evaluation_point) final {
+ if (new_evaluation_point) {
+ ComputeResidualAndJacobian(evaluate_jacobians);
+ jacobians_are_stale_ = !evaluate_jacobians;
+ } else {
+ if (evaluate_jacobians && jacobians_are_stale_) {
+ ComputeResidualAndJacobian(evaluate_jacobians);
+ jacobians_are_stale_ = false;
+ }
+ }
+ }
+
+ const Eigen::VectorXd& residuals() const { return residuals_; }
+ const Eigen::MatrixXd& jacobians() const { return jacobians_; }
+ bool jacobians_are_stale() const { return jacobians_are_stale_; }
+
+ private:
+ void ComputeResidualAndJacobian(bool evaluate_jacobians) {
+ residuals_ = -(m_ * x_.array() + c_).exp();
+ if (evaluate_jacobians) {
+ jacobians_.col(0) = residuals_.array() * x_.array();
+ jacobians_.col(1) = residuals_;
+ }
+ residuals_ += y_;
+ }
+
+ const double& m_;
+ const double& c_;
+ Eigen::VectorXd x_;
+ Eigen::VectorXd y_;
+ Eigen::VectorXd residuals_;
+ Eigen::MatrixXd jacobians_;
+
+ // jacobians_are_stale_ keeps track of whether the jacobian matrix matches the
+ // residuals or not, we only compute it if we know that Solver is going to
+ // need access to it.
+ bool jacobians_are_stale_ = true;
+};
+
+// As the name implies this CostFunction does not do any computation, it just
+// copies the appropriate residual and Jacobian from the matrices stored in
+// MyEvaluationCallback.
+class CostAndJacobianCopyingCostFunction
+ : public ceres::SizedCostFunction<1, 1, 1> {
+ public:
+ CostAndJacobianCopyingCostFunction(
+ int index, const MyEvaluationCallback& evaluation_callback)
+ : index_(index), evaluation_callback_(evaluation_callback) {}
+ ~CostAndJacobianCopyingCostFunction() override = default;
+
+ bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const final {
+ residuals[0] = evaluation_callback_.residuals()(index_);
+ if (!jacobians) return true;
+
+ // Ensure that we are not using stale Jacobians.
+ CHECK(!evaluation_callback_.jacobians_are_stale());
+
+ if (jacobians[0] != nullptr)
+ jacobians[0][0] = evaluation_callback_.jacobians()(index_, 0);
+ if (jacobians[1] != nullptr)
+ jacobians[1][0] = evaluation_callback_.jacobians()(index_, 1);
+ return true;
+ }
+
+ private:
+ int index_ = -1;
+ const MyEvaluationCallback& evaluation_callback_;
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ const double initial_m = 0.0;
+ const double initial_c = 0.0;
+ double m = initial_m;
+ double c = initial_c;
+
+ MyEvaluationCallback evaluation_callback(m, c);
+ ceres::Problem::Options problem_options;
+ problem_options.evaluation_callback = &evaluation_callback;
+ ceres::Problem problem(problem_options);
+ for (int i = 0; i < kNumObservations; ++i) {
+ problem.AddResidualBlock(
+ new CostAndJacobianCopyingCostFunction(i, evaluation_callback),
+ nullptr,
+ &m,
+ &c);
+ }
+
+ ceres::Solver::Options options;
+ options.max_num_iterations = 25;
+ options.linear_solver_type = ceres::DENSE_QR;
+ options.minimizer_progress_to_stdout = true;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+ std::cout << summary.BriefReport() << "\n";
+ std::cout << "Initial m: " << initial_m << " c: " << initial_c << "\n";
+ std::cout << "Final m: " << m << " c: " << c << "\n";
+ return 0;
+}
diff --git a/third_party/ceres/examples/fields_of_experts.cc b/third_party/ceres/examples/fields_of_experts.cc
index 7b7983e..f59fe16 100644
--- a/third_party/ceres/examples/fields_of_experts.cc
+++ b/third_party/ceres/examples/fields_of_experts.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -28,7 +28,7 @@
//
// Author: strandmark@google.com (Petter Strandmark)
//
-// Class for loading the data required for descibing a Fields of Experts (FoE)
+// Class for loading the data required for describing a Fields of Experts (FoE)
// model.
#include "fields_of_experts.h"
@@ -38,13 +38,12 @@
#include "pgm_image.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
FieldsOfExpertsCost::FieldsOfExpertsCost(const std::vector<double>& filter)
: filter_(filter) {
set_num_residuals(1);
- for (int i = 0; i < filter_.size(); ++i) {
+ for (int64_t i = 0; i < filter_.size(); ++i) {
mutable_parameter_block_sizes()->push_back(1);
}
}
@@ -54,15 +53,15 @@
bool FieldsOfExpertsCost::Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
- int num_variables = filter_.size();
+ const int64_t num_variables = filter_.size();
residuals[0] = 0;
- for (int i = 0; i < num_variables; ++i) {
+ for (int64_t i = 0; i < num_variables; ++i) {
residuals[0] += filter_[i] * parameters[i][0];
}
- if (jacobians != NULL) {
- for (int i = 0; i < num_variables; ++i) {
- if (jacobians[i] != NULL) {
+ if (jacobians != nullptr) {
+ for (int64_t i = 0; i < num_variables; ++i) {
+ if (jacobians[i] != nullptr) {
jacobians[i][0] = filter_[i];
}
}
@@ -145,5 +144,4 @@
return new FieldsOfExpertsLoss(alpha_[alpha_index]);
}
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
diff --git a/third_party/ceres/examples/fields_of_experts.h b/third_party/ceres/examples/fields_of_experts.h
index 429881d..2ff8c94 100644
--- a/third_party/ceres/examples/fields_of_experts.h
+++ b/third_party/ceres/examples/fields_of_experts.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -28,7 +28,7 @@
//
// Author: strandmark@google.com (Petter Strandmark)
//
-// Class for loading the data required for descibing a Fields of Experts (FoE)
+// Class for loading the data required for describing a Fields of Experts (FoE)
// model. The Fields of Experts regularization consists of terms of the type
//
// alpha * log(1 + (1/2)*sum(F .* X)^2),
@@ -52,8 +52,7 @@
#include "ceres/sized_cost_function.h"
#include "pgm_image.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
// One sum in the FoE regularizer. This is a dot product between a filter and an
// image patch. It simply calculates the dot product between the filter
@@ -63,9 +62,9 @@
explicit FieldsOfExpertsCost(const std::vector<double>& filter);
// The number of scalar parameters passed to Evaluate must equal the number of
// filter coefficients passed to the constructor.
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const;
+ bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override;
private:
const std::vector<double>& filter_;
@@ -78,7 +77,7 @@
class FieldsOfExpertsLoss : public ceres::LossFunction {
public:
explicit FieldsOfExpertsLoss(double alpha) : alpha_(alpha) {}
- virtual void Evaluate(double, double*) const;
+ void Evaluate(double, double*) const override;
private:
const double alpha_;
@@ -128,7 +127,6 @@
std::vector<std::vector<double>> filters_;
};
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // CERES_EXAMPLES_FIELDS_OF_EXPERTS_H_
diff --git a/third_party/ceres/examples/helloworld.cc b/third_party/ceres/examples/helloworld.cc
index 9e32fad..40c2f2c 100644
--- a/third_party/ceres/examples/helloworld.cc
+++ b/third_party/ceres/examples/helloworld.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -36,12 +36,6 @@
#include "ceres/ceres.h"
#include "glog/logging.h"
-using ceres::AutoDiffCostFunction;
-using ceres::CostFunction;
-using ceres::Problem;
-using ceres::Solve;
-using ceres::Solver;
-
// A templated cost functor that implements the residual r = 10 -
// x. The method operator() is templated so that we can then use an
// automatic differentiation wrapper around it to generate its
@@ -63,19 +57,19 @@
const double initial_x = x;
// Build the problem.
- Problem problem;
+ ceres::Problem problem;
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
- CostFunction* cost_function =
- new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
+ ceres::CostFunction* cost_function =
+ new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>();
problem.AddResidualBlock(cost_function, nullptr, &x);
// Run the solver!
- Solver::Options options;
+ ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x << " -> " << x << "\n";
diff --git a/third_party/ceres/examples/helloworld_analytic_diff.cc b/third_party/ceres/examples/helloworld_analytic_diff.cc
index 6e120b5..b4826a2 100644
--- a/third_party/ceres/examples/helloworld_analytic_diff.cc
+++ b/third_party/ceres/examples/helloworld_analytic_diff.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -37,23 +37,15 @@
#include "ceres/ceres.h"
#include "glog/logging.h"
-using ceres::CostFunction;
-using ceres::Problem;
-using ceres::SizedCostFunction;
-using ceres::Solve;
-using ceres::Solver;
-
// A CostFunction implementing analytically derivatives for the
// function f(x) = 10 - x.
class QuadraticCostFunction
- : public SizedCostFunction<1 /* number of residuals */,
- 1 /* size of first parameter */> {
+ : public ceres::SizedCostFunction<1 /* number of residuals */,
+ 1 /* size of first parameter */> {
public:
- virtual ~QuadraticCostFunction() {}
-
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
+ bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override {
double x = parameters[0][0];
// f(x) = 10 - x.
@@ -64,14 +56,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;
}
@@ -88,17 +80,17 @@
const double initial_x = x;
// Build the problem.
- Problem problem;
+ ceres::Problem problem;
// Set up the only cost function (also known as residual).
- CostFunction* cost_function = new QuadraticCostFunction;
- problem.AddResidualBlock(cost_function, NULL, &x);
+ ceres::CostFunction* cost_function = new QuadraticCostFunction;
+ problem.AddResidualBlock(cost_function, nullptr, &x);
// Run the solver!
- Solver::Options options;
+ ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x << " -> " << x << "\n";
diff --git a/third_party/ceres/examples/helloworld_numeric_diff.cc b/third_party/ceres/examples/helloworld_numeric_diff.cc
index 474adf3..4ed9ca6 100644
--- a/third_party/ceres/examples/helloworld_numeric_diff.cc
+++ b/third_party/ceres/examples/helloworld_numeric_diff.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -34,13 +34,6 @@
#include "ceres/ceres.h"
#include "glog/logging.h"
-using ceres::CENTRAL;
-using ceres::CostFunction;
-using ceres::NumericDiffCostFunction;
-using ceres::Problem;
-using ceres::Solve;
-using ceres::Solver;
-
// A cost functor that implements the residual r = 10 - x.
struct CostFunctor {
bool operator()(const double* const x, double* residual) const {
@@ -58,19 +51,20 @@
const double initial_x = x;
// Build the problem.
- Problem problem;
+ ceres::Problem problem;
// Set up the only cost function (also known as residual). This uses
// numeric differentiation to obtain the derivative (jacobian).
- CostFunction* cost_function =
- new NumericDiffCostFunction<CostFunctor, CENTRAL, 1, 1>(new CostFunctor);
- problem.AddResidualBlock(cost_function, NULL, &x);
+ ceres::CostFunction* cost_function =
+ new ceres::NumericDiffCostFunction<CostFunctor, ceres::CENTRAL, 1, 1>(
+ new CostFunctor);
+ problem.AddResidualBlock(cost_function, nullptr, &x);
// Run the solver!
- Solver::Options options;
+ ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x << " -> " << x << "\n";
diff --git a/third_party/ceres/examples/iteration_callback_example.cc b/third_party/ceres/examples/iteration_callback_example.cc
new file mode 100644
index 0000000..0be2f36
--- /dev/null
+++ b/third_party/ceres/examples/iteration_callback_example.cc
@@ -0,0 +1,199 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: sameeragarwal@google.com (Sameer Agarwal)
+//
+// This example is a variant of curve_fitting.cc where we use an
+// IterationCallback to implement custom logging which prints out the values of
+// the parameter blocks as they evolve over the course of the optimization. This
+// also requires the use of Solver::Options::update_state_every_iteration.
+
+#include <iostream>
+
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+// Data generated using the following octave code.
+// randn('seed', 23497);
+// m = 0.3;
+// c = 0.1;
+// x=[0:0.075:5];
+// y = exp(m * x + c);
+// noise = randn(size(x)) * 0.2;
+// y_observed = y + noise;
+// data = [x', y_observed'];
+
+const int kNumObservations = 67;
+// clang-format off
+const double data[] = {
+ 0.000000e+00, 1.133898e+00,
+ 7.500000e-02, 1.334902e+00,
+ 1.500000e-01, 1.213546e+00,
+ 2.250000e-01, 1.252016e+00,
+ 3.000000e-01, 1.392265e+00,
+ 3.750000e-01, 1.314458e+00,
+ 4.500000e-01, 1.472541e+00,
+ 5.250000e-01, 1.536218e+00,
+ 6.000000e-01, 1.355679e+00,
+ 6.750000e-01, 1.463566e+00,
+ 7.500000e-01, 1.490201e+00,
+ 8.250000e-01, 1.658699e+00,
+ 9.000000e-01, 1.067574e+00,
+ 9.750000e-01, 1.464629e+00,
+ 1.050000e+00, 1.402653e+00,
+ 1.125000e+00, 1.713141e+00,
+ 1.200000e+00, 1.527021e+00,
+ 1.275000e+00, 1.702632e+00,
+ 1.350000e+00, 1.423899e+00,
+ 1.425000e+00, 1.543078e+00,
+ 1.500000e+00, 1.664015e+00,
+ 1.575000e+00, 1.732484e+00,
+ 1.650000e+00, 1.543296e+00,
+ 1.725000e+00, 1.959523e+00,
+ 1.800000e+00, 1.685132e+00,
+ 1.875000e+00, 1.951791e+00,
+ 1.950000e+00, 2.095346e+00,
+ 2.025000e+00, 2.361460e+00,
+ 2.100000e+00, 2.169119e+00,
+ 2.175000e+00, 2.061745e+00,
+ 2.250000e+00, 2.178641e+00,
+ 2.325000e+00, 2.104346e+00,
+ 2.400000e+00, 2.584470e+00,
+ 2.475000e+00, 1.914158e+00,
+ 2.550000e+00, 2.368375e+00,
+ 2.625000e+00, 2.686125e+00,
+ 2.700000e+00, 2.712395e+00,
+ 2.775000e+00, 2.499511e+00,
+ 2.850000e+00, 2.558897e+00,
+ 2.925000e+00, 2.309154e+00,
+ 3.000000e+00, 2.869503e+00,
+ 3.075000e+00, 3.116645e+00,
+ 3.150000e+00, 3.094907e+00,
+ 3.225000e+00, 2.471759e+00,
+ 3.300000e+00, 3.017131e+00,
+ 3.375000e+00, 3.232381e+00,
+ 3.450000e+00, 2.944596e+00,
+ 3.525000e+00, 3.385343e+00,
+ 3.600000e+00, 3.199826e+00,
+ 3.675000e+00, 3.423039e+00,
+ 3.750000e+00, 3.621552e+00,
+ 3.825000e+00, 3.559255e+00,
+ 3.900000e+00, 3.530713e+00,
+ 3.975000e+00, 3.561766e+00,
+ 4.050000e+00, 3.544574e+00,
+ 4.125000e+00, 3.867945e+00,
+ 4.200000e+00, 4.049776e+00,
+ 4.275000e+00, 3.885601e+00,
+ 4.350000e+00, 4.110505e+00,
+ 4.425000e+00, 4.345320e+00,
+ 4.500000e+00, 4.161241e+00,
+ 4.575000e+00, 4.363407e+00,
+ 4.650000e+00, 4.161576e+00,
+ 4.725000e+00, 4.619728e+00,
+ 4.800000e+00, 4.737410e+00,
+ 4.875000e+00, 4.727863e+00,
+ 4.950000e+00, 4.669206e+00,
+};
+// clang-format on
+
+struct ExponentialResidual {
+ ExponentialResidual(double x, double y) : x(x), y(y) {}
+
+ template <typename T>
+ bool operator()(const T* const m, const T* const c, T* residual) const {
+ residual[0] = y - exp(m[0] * x + c[0]);
+ return true;
+ }
+
+ private:
+ const double x;
+ const double y;
+};
+
+// MyIterationCallback prints the iteration number, the cost and the value of
+// the parameter blocks every iteration.
+class MyIterationCallback : public ceres::IterationCallback {
+ public:
+ MyIterationCallback(const double* m, const double* c) : m_(m), c_(c) {}
+
+ ~MyIterationCallback() override = default;
+
+ ceres::CallbackReturnType operator()(
+ const ceres::IterationSummary& summary) final {
+ std::cout << "Iteration: " << summary.iteration << " cost: " << summary.cost
+ << " m: " << *m_ << " c: " << *c_ << std::endl;
+ return ceres::SOLVER_CONTINUE;
+ }
+
+ private:
+ const double* m_ = nullptr;
+ const double* c_ = nullptr;
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ const double initial_m = 0.0;
+ const double initial_c = 0.0;
+
+ double m = initial_m;
+ double c = initial_c;
+
+ ceres::Problem problem;
+ for (int i = 0; i < kNumObservations; ++i) {
+ problem.AddResidualBlock(
+ new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
+ data[2 * i], data[2 * i + 1]),
+ nullptr,
+ &m,
+ &c);
+ }
+
+ ceres::Solver::Options options;
+ options.max_num_iterations = 25;
+ options.linear_solver_type = ceres::DENSE_QR;
+
+ // Turn off the default logging from Ceres so that it does not interfere with
+ // MyIterationCallback.
+ options.minimizer_progress_to_stdout = false;
+
+ MyIterationCallback callback(&m, &c);
+ options.callbacks.push_back(&callback);
+
+ // Tell Ceres to update the value of the parameter blocks on each each
+ // iteration (successful or not) so that MyIterationCallback will be able to
+ // see them when called.
+ options.update_state_every_iteration = true;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+ std::cout << summary.BriefReport() << "\n";
+ std::cout << "Initial m: " << initial_m << " c: " << initial_c << "\n";
+ std::cout << "Final m: " << m << " c: " << c << "\n";
+ return 0;
+}
diff --git a/third_party/ceres/examples/libmv_bundle_adjuster.cc b/third_party/ceres/examples/libmv_bundle_adjuster.cc
index b1eb220..9315ed7 100644
--- a/third_party/ceres/examples/libmv_bundle_adjuster.cc
+++ b/third_party/ceres/examples/libmv_bundle_adjuster.cc
@@ -60,7 +60,7 @@
// Image number shall be greater or equal to zero. Order of cameras does not
// matter and gaps are possible.
//
-// Every 3D point is decribed by:
+// Every 3D point is described by:
//
// - Track number point belongs to (single 4 bytes integer value).
// - 3D position vector, 3-component vector of float values.
@@ -100,12 +100,16 @@
#define close _close
typedef unsigned __int32 uint32_t;
#else
-#include <stdint.h>
#include <unistd.h>
+#include <cstdint>
+
+// NOTE MinGW does define the macro.
+#ifndef O_BINARY
// O_BINARY is not defined on unix like platforms, as there is no
// difference between binary and text files.
#define O_BINARY 0
+#endif
#endif
@@ -114,12 +118,10 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
-typedef Eigen::Matrix<double, 3, 3> Mat3;
-typedef Eigen::Matrix<double, 6, 1> Vec6;
-typedef Eigen::Vector3d Vec3;
-typedef Eigen::Vector4d Vec4;
-
-using std::vector;
+using Mat3 = Eigen::Matrix<double, 3, 3>;
+using Vec6 = Eigen::Matrix<double, 6, 1>;
+using Vec3 = Eigen::Vector3d;
+using Vec4 = Eigen::Vector4d;
DEFINE_string(input, "", "Input File name");
DEFINE_string(refine_intrinsics,
@@ -135,10 +137,10 @@
// R is a 3x3 matrix representing the rotation of the camera.
// t is a translation vector representing its positions.
struct EuclideanCamera {
- EuclideanCamera() : image(-1) {}
- EuclideanCamera(const EuclideanCamera& c) : image(c.image), R(c.R), t(c.t) {}
+ EuclideanCamera() = default;
+ EuclideanCamera(const EuclideanCamera& c) = default;
- int image;
+ int image{-1};
Mat3 R;
Vec3 t;
};
@@ -148,9 +150,9 @@
// track identifies which track this point corresponds to.
// X represents the 3D position of the track.
struct EuclideanPoint {
- EuclideanPoint() : track(-1) {}
- EuclideanPoint(const EuclideanPoint& p) : track(p.track), X(p.X) {}
- int track;
+ EuclideanPoint() = default;
+ EuclideanPoint(const EuclideanPoint& p) = default;
+ int track{-1};
Vec3 X;
};
@@ -203,32 +205,32 @@
};
// Returns a pointer to the camera corresponding to a image.
-EuclideanCamera* CameraForImage(vector<EuclideanCamera>* all_cameras,
+EuclideanCamera* CameraForImage(std::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;
}
const EuclideanCamera* CameraForImage(
- const vector<EuclideanCamera>& all_cameras, const int image) {
+ const std::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;
}
// Returns maximal image number at which marker exists.
-int MaxImage(const vector<Marker>& all_markers) {
+int MaxImage(const std::vector<Marker>& all_markers) {
if (all_markers.size() == 0) {
return -1;
}
@@ -241,14 +243,14 @@
}
// Returns a pointer to the point corresponding to a track.
-EuclideanPoint* PointForTrack(vector<EuclideanPoint>* all_points,
+EuclideanPoint* PointForTrack(std::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;
}
@@ -262,7 +264,7 @@
// denotes file endianness in this way.
class EndianAwareFileReader {
public:
- EndianAwareFileReader(void) : file_descriptor_(-1) {
+ EndianAwareFileReader() {
// Get an endian type of the host machine.
union {
unsigned char bytes[4];
@@ -272,7 +274,7 @@
file_endian_type_ = host_endian_type_;
}
- ~EndianAwareFileReader(void) {
+ ~EndianAwareFileReader() {
if (file_descriptor_ > 0) {
close(file_descriptor_);
}
@@ -284,7 +286,7 @@
return false;
}
// Get an endian tpye of data in the file.
- unsigned char file_endian_type_flag = Read<unsigned char>();
+ auto file_endian_type_flag = Read<unsigned char>();
if (file_endian_type_flag == 'V') {
file_endian_type_ = kBigEndian;
} else if (file_endian_type_flag == 'v') {
@@ -297,9 +299,11 @@
// Read value from the file, will switch endian if needed.
template <typename T>
- T Read(void) const {
+ T Read() const {
T value;
+ CERES_DISABLE_DEPRECATED_WARNING
CHECK_GT(read(file_descriptor_, &value, sizeof(value)), 0);
+ CERES_RESTORE_DEPRECATED_WARNING
// Switch endian type if file contains data in different type
// that current machine.
if (file_endian_type_ != host_endian_type_) {
@@ -316,7 +320,7 @@
template <typename T>
T SwitchEndian(const T value) const {
if (sizeof(T) == 4) {
- unsigned int temp_value = static_cast<unsigned int>(value);
+ auto temp_value = static_cast<unsigned int>(value);
// clang-format off
return ((temp_value >> 24)) |
((temp_value << 8) & 0x00ff0000) |
@@ -333,7 +337,7 @@
int host_endian_type_;
int file_endian_type_;
- int file_descriptor_;
+ int file_descriptor_{-1};
};
// Read 3x3 column-major matrix from the file
@@ -369,17 +373,17 @@
// reading.
bool ReadProblemFromFile(const std::string& file_name,
double camera_intrinsics[8],
- vector<EuclideanCamera>* all_cameras,
- vector<EuclideanPoint>* all_points,
+ std::vector<EuclideanCamera>* all_cameras,
+ std::vector<EuclideanPoint>* all_points,
bool* is_image_space,
- vector<Marker>* all_markers) {
+ std::vector<Marker>* all_markers) {
EndianAwareFileReader file_reader;
if (!file_reader.OpenFile(file_name)) {
return false;
}
// Read markers' space flag.
- unsigned char is_image_space_flag = file_reader.Read<unsigned char>();
+ auto is_image_space_flag = file_reader.Read<unsigned char>();
if (is_image_space_flag == 'P') {
*is_image_space = true;
} else if (is_image_space_flag == 'N') {
@@ -610,10 +614,10 @@
//
// Element with index i matches to a rotation+translation for
// camera at image i.
-vector<Vec6> PackCamerasRotationAndTranslation(
- const vector<Marker>& all_markers,
- const vector<EuclideanCamera>& all_cameras) {
- vector<Vec6> all_cameras_R_t;
+std::vector<Vec6> PackCamerasRotationAndTranslation(
+ const std::vector<Marker>& all_markers,
+ const std::vector<EuclideanCamera>& all_cameras) {
+ std::vector<Vec6> all_cameras_R_t;
int max_image = MaxImage(all_markers);
all_cameras_R_t.resize(max_image + 1);
@@ -633,9 +637,10 @@
}
// Convert cameras rotations fro mangle axis back to rotation matrix.
-void UnpackCamerasRotationAndTranslation(const vector<Marker>& all_markers,
- const vector<Vec6>& all_cameras_R_t,
- vector<EuclideanCamera>* all_cameras) {
+void UnpackCamerasRotationAndTranslation(
+ const std::vector<Marker>& all_markers,
+ const std::vector<Vec6>& all_cameras_R_t,
+ std::vector<EuclideanCamera>* all_cameras) {
int max_image = MaxImage(all_markers);
for (int i = 0; i <= max_image; i++) {
@@ -650,12 +655,12 @@
}
}
-void EuclideanBundleCommonIntrinsics(const vector<Marker>& all_markers,
+void EuclideanBundleCommonIntrinsics(const std::vector<Marker>& all_markers,
const int bundle_intrinsics,
const int bundle_constraints,
double* camera_intrinsics,
- vector<EuclideanCamera>* all_cameras,
- vector<EuclideanPoint>* all_points) {
+ std::vector<EuclideanCamera>* all_cameras,
+ std::vector<EuclideanPoint>* all_points) {
PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics);
ceres::Problem::Options problem_options;
@@ -667,11 +672,11 @@
//
// Block for minimization has got the following structure:
// <3 elements for angle-axis> <3 elements for translation>
- vector<Vec6> all_cameras_R_t =
+ std::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 +685,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;
@@ -692,11 +697,10 @@
int num_residuals = 0;
bool have_locked_camera = false;
- for (int i = 0; i < all_markers.size(); ++i) {
- const Marker& marker = all_markers[i];
+ for (const auto& marker : all_markers) {
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 +712,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 +724,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 +763,8 @@
// 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);
+ auto* subset_manifold = new ceres::SubsetManifold(8, constant_intrinsics);
+ problem.SetManifold(camera_intrinsics, subset_manifold);
}
// Configure the solver.
@@ -793,18 +794,18 @@
GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
- if (FLAGS_input.empty()) {
+ if (CERES_GET_FLAG(FLAGS_input).empty()) {
LOG(ERROR) << "Usage: libmv_bundle_adjuster --input=blender_problem";
return EXIT_FAILURE;
}
double camera_intrinsics[8];
- vector<EuclideanCamera> all_cameras;
- vector<EuclideanPoint> all_points;
+ std::vector<EuclideanCamera> all_cameras;
+ std::vector<EuclideanPoint> all_points;
bool is_image_space;
- vector<Marker> all_markers;
+ std::vector<Marker> all_markers;
- if (!ReadProblemFromFile(FLAGS_input,
+ if (!ReadProblemFromFile(CERES_GET_FLAG(FLAGS_input),
camera_intrinsics,
&all_cameras,
&all_points,
@@ -828,14 +829,14 @@
// declare which intrinsics need to be refined and in this case
// refining flags does not depend on problem at all.
int bundle_intrinsics = BUNDLE_NO_INTRINSICS;
- if (FLAGS_refine_intrinsics.empty()) {
+ if (CERES_GET_FLAG(FLAGS_refine_intrinsics).empty()) {
if (is_image_space) {
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
}
} else {
- if (FLAGS_refine_intrinsics == "radial") {
+ if (CERES_GET_FLAG(FLAGS_refine_intrinsics) == "radial") {
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
- } else if (FLAGS_refine_intrinsics != "none") {
+ } else if (CERES_GET_FLAG(FLAGS_refine_intrinsics) != "none") {
LOG(ERROR) << "Unsupported value for refine-intrinsics";
return EXIT_FAILURE;
}
diff --git a/third_party/ceres/examples/libmv_homography.cc b/third_party/ceres/examples/libmv_homography.cc
index 55f3b70..b7c9eda 100644
--- a/third_party/ceres/examples/libmv_homography.cc
+++ b/third_party/ceres/examples/libmv_homography.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -60,17 +60,19 @@
// This example demonstrates custom exit criterion by having a callback check
// for image-space error.
+#include <utility>
+
#include "ceres/ceres.h"
#include "glog/logging.h"
-typedef Eigen::NumTraits<double> EigenDouble;
+using EigenDouble = Eigen::NumTraits<double>;
-typedef Eigen::MatrixXd Mat;
-typedef Eigen::VectorXd Vec;
-typedef Eigen::Matrix<double, 3, 3> Mat3;
-typedef Eigen::Matrix<double, 2, 1> Vec2;
-typedef Eigen::Matrix<double, Eigen::Dynamic, 8> MatX8;
-typedef Eigen::Vector3d Vec3;
+using Mat = Eigen::MatrixXd;
+using Vec = Eigen::VectorXd;
+using Mat3 = Eigen::Matrix<double, 3, 3>;
+using Vec2 = Eigen::Matrix<double, 2, 1>;
+using MatX8 = Eigen::Matrix<double, Eigen::Dynamic, 8>;
+using Vec3 = Eigen::Vector3d;
namespace {
@@ -82,11 +84,10 @@
struct EstimateHomographyOptions {
// Default settings for homography estimation which should be suitable
// for a wide range of use cases.
- EstimateHomographyOptions()
- : max_num_iterations(50), expected_average_symmetric_distance(1e-16) {}
+ EstimateHomographyOptions() = default;
// Maximal number of iterations for the refinement step.
- int max_num_iterations;
+ int max_num_iterations{50};
// Expected average of symmetric geometric distance between
// actual destination points and original ones transformed by
@@ -96,7 +97,7 @@
// geometric distance is less or equal to this value.
//
// This distance is measured in the same units as input points are.
- double expected_average_symmetric_distance;
+ double expected_average_symmetric_distance{1e-16};
};
// Calculate symmetric geometric cost terms:
@@ -111,7 +112,7 @@
const Eigen::Matrix<T, 2, 1>& x2,
T forward_error[2],
T backward_error[2]) {
- typedef Eigen::Matrix<T, 3, 1> Vec3;
+ using Vec3 = Eigen::Matrix<T, 3, 1>;
Vec3 x(x1(0), x1(1), T(1.0));
Vec3 y(x2(0), x2(1), T(1.0));
@@ -152,8 +153,8 @@
template <typename T = double>
class Homography2DNormalizedParameterization {
public:
- typedef Eigen::Matrix<T, 8, 1> Parameters; // a, b, ... g, h
- typedef Eigen::Matrix<T, 3, 3> Parameterized; // H
+ using Parameters = Eigen::Matrix<T, 8, 1>; // a, b, ... g, h
+ using Parameterized = Eigen::Matrix<T, 3, 3>; // H
// Convert from the 8 parameters to a H matrix.
static void To(const Parameters& p, Parameterized* h) {
@@ -202,11 +203,11 @@
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
- int n = x1.cols();
+ const int64_t n = x1.cols();
MatX8 L = Mat::Zero(n * 3, 8);
Mat b = Mat::Zero(n * 3, 1);
- for (int i = 0; i < n; ++i) {
- int j = 3 * i;
+ for (int64_t i = 0; i < n; ++i) {
+ int64_t j = 3 * i;
L(j, 0) = x1(0, i); // a
L(j, 1) = x1(1, i); // b
L(j, 2) = 1.0; // c
@@ -242,13 +243,13 @@
// used for homography matrix refinement.
class HomographySymmetricGeometricCostFunctor {
public:
- HomographySymmetricGeometricCostFunctor(const Vec2& x, const Vec2& y)
- : x_(x), y_(y) {}
+ HomographySymmetricGeometricCostFunctor(Vec2 x, Vec2 y)
+ : x_(std::move(x)), y_(std::move(y)) {}
template <typename T>
bool operator()(const T* homography_parameters, T* residuals) const {
- typedef Eigen::Matrix<T, 3, 3> Mat3;
- typedef Eigen::Matrix<T, 2, 1> Vec2;
+ using Mat3 = Eigen::Matrix<T, 3, 3>;
+ using Vec2 = Eigen::Matrix<T, 2, 1>;
Mat3 H(homography_parameters);
Vec2 x(T(x_(0)), T(x_(1)));
@@ -277,8 +278,8 @@
Mat3* H)
: options_(options), x1_(x1), x2_(x2), H_(H) {}
- virtual ceres::CallbackReturnType operator()(
- const ceres::IterationSummary& summary) {
+ ceres::CallbackReturnType operator()(
+ const ceres::IterationSummary& summary) override {
// If the step wasn't successful, there's nothing to do.
if (!summary.step_is_successful) {
return ceres::SOLVER_CONTINUE;
@@ -326,16 +327,11 @@
// Step 2: Refine matrix using Ceres minimizer.
ceres::Problem problem;
for (int i = 0; i < x1.cols(); i++) {
- HomographySymmetricGeometricCostFunctor*
- homography_symmetric_geometric_cost_function =
- new HomographySymmetricGeometricCostFunctor(x1.col(i), x2.col(i));
-
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<HomographySymmetricGeometricCostFunctor,
4, // num_residuals
- 9>(
- homography_symmetric_geometric_cost_function),
- NULL,
+ 9>(x1.col(i), x2.col(i)),
+ nullptr,
H->data());
}
@@ -380,10 +376,10 @@
Mat x2 = x1;
for (int i = 0; i < x2.cols(); ++i) {
- Vec3 homogenous_x1 = Vec3(x1(0, i), x1(1, i), 1.0);
- Vec3 homogenous_x2 = homography_matrix * homogenous_x1;
- x2(0, i) = homogenous_x2(0) / homogenous_x2(2);
- x2(1, i) = homogenous_x2(1) / homogenous_x2(2);
+ Vec3 homogeneous_x1 = Vec3(x1(0, i), x1(1, i), 1.0);
+ Vec3 homogeneous_x2 = homography_matrix * homogeneous_x1;
+ x2(0, i) = homogeneous_x2(0) / homogeneous_x2(2);
+ x2(1, i) = homogeneous_x2(1) / homogeneous_x2(2);
// Apply some noise so algebraic estimation is not good enough.
x2(0, i) += static_cast<double>(rand() % 1000) / 5000.0;
diff --git a/third_party/ceres/examples/more_garbow_hillstrom.cc b/third_party/ceres/examples/more_garbow_hillstrom.cc
index e39d23c..f15e576 100644
--- a/third_party/ceres/examples/more_garbow_hillstrom.cc
+++ b/third_party/ceres/examples/more_garbow_hillstrom.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -72,55 +72,56 @@
3,
"Maximal number of extrapolations in Ridders' method.");
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
const double kDoubleMax = std::numeric_limits<double>::max();
static void SetNumericDiffOptions(ceres::NumericDiffOptions* options) {
- options->max_num_ridders_extrapolations = FLAGS_ridders_extrapolations;
+ options->max_num_ridders_extrapolations =
+ CERES_GET_FLAG(FLAGS_ridders_extrapolations);
}
-#define BEGIN_MGH_PROBLEM(name, num_parameters, num_residuals) \
- struct name { \
- static constexpr int kNumParameters = num_parameters; \
- static const double initial_x[kNumParameters]; \
- static const double lower_bounds[kNumParameters]; \
- static const double upper_bounds[kNumParameters]; \
- static const double constrained_optimal_cost; \
- static const double unconstrained_optimal_cost; \
- static CostFunction* Create() { \
- if (FLAGS_use_numeric_diff) { \
- ceres::NumericDiffOptions options; \
- SetNumericDiffOptions(&options); \
- if (FLAGS_numeric_diff_method == "central") { \
- return new NumericDiffCostFunction<name, \
- ceres::CENTRAL, \
- num_residuals, \
- num_parameters>( \
- new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \
- } else if (FLAGS_numeric_diff_method == "forward") { \
- return new NumericDiffCostFunction<name, \
- ceres::FORWARD, \
- num_residuals, \
- num_parameters>( \
- new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \
- } else if (FLAGS_numeric_diff_method == "ridders") { \
- 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; \
- } \
- } else { \
- return new AutoDiffCostFunction<name, num_residuals, num_parameters>( \
- new name); \
- } \
- } \
- template <typename T> \
+#define BEGIN_MGH_PROBLEM(name, num_parameters, num_residuals) \
+ struct name { \
+ static constexpr int kNumParameters = num_parameters; \
+ static const double initial_x[kNumParameters]; \
+ static const double lower_bounds[kNumParameters]; \
+ static const double upper_bounds[kNumParameters]; \
+ static const double constrained_optimal_cost; \
+ static const double unconstrained_optimal_cost; \
+ static CostFunction* Create() { \
+ if (CERES_GET_FLAG(FLAGS_use_numeric_diff)) { \
+ ceres::NumericDiffOptions options; \
+ SetNumericDiffOptions(&options); \
+ if (CERES_GET_FLAG(FLAGS_numeric_diff_method) == "central") { \
+ 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>( \
+ 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>( \
+ new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \
+ } else { \
+ LOG(ERROR) << "Invalid numeric diff method specified"; \
+ return nullptr; \
+ } \
+ } else { \
+ return new AutoDiffCostFunction<name, \
+ num_residuals, \
+ num_parameters>(); \
+ } \
+ } \
+ template <typename T> \
bool operator()(const T* const x, T* residual) const {
// clang-format off
@@ -223,7 +224,7 @@
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
- const T theta = (0.5 / M_PI) * atan(x2 / x1) + (x1 > 0.0 ? 0.0 : 0.5);
+ const T theta = (0.5 / constants::pi) * atan(x2 / x1) + (x1 > 0.0 ? 0.0 : 0.5);
residual[0] = 10.0 * (x3 - 10.0 * theta);
residual[1] = 10.0 * (sqrt(x1 * x1 + x2 * x2) - 1.0);
residual[2] = x3;
@@ -548,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) {
@@ -580,13 +581,11 @@
return success;
}
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
int main(int argc, char** argv) {
GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
-
using ceres::examples::Solve;
int unconstrained_problems = 0;
@@ -597,7 +596,8 @@
#define UNCONSTRAINED_SOLVE(n) \
ss << "Unconstrained Problem " << n << " : "; \
- if (FLAGS_problem == #n || FLAGS_problem == "all") { \
+ if (CERES_GET_FLAG(FLAGS_problem) == #n || \
+ CERES_GET_FLAG(FLAGS_problem) == "all") { \
unconstrained_problems += 3; \
if (Solve<ceres::examples::TestProblem##n>(false, 0)) { \
unconstrained_successes += 1; \
@@ -645,7 +645,8 @@
#define CONSTRAINED_SOLVE(n) \
ss << "Constrained Problem " << n << " : "; \
- if (FLAGS_problem == #n || FLAGS_problem == "all") { \
+ if (CERES_GET_FLAG(FLAGS_problem) == #n || \
+ CERES_GET_FLAG(FLAGS_problem) == "all") { \
constrained_problems += 1; \
if (Solve<ceres::examples::TestProblem##n>(true, 0)) { \
constrained_successes += 1; \
diff --git a/third_party/ceres/examples/nist.cc b/third_party/ceres/examples/nist.cc
index 977b69d..b92c918 100644
--- a/third_party/ceres/examples/nist.cc
+++ b/third_party/ceres/examples/nist.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2017 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -71,9 +71,12 @@
// Average LRE 2.3 4.3 4.0 6.8 4.4 9.4
// Winner 0 0 5 11 2 41
+#include <cstdlib>
#include <fstream>
#include <iostream>
#include <iterator>
+#include <string>
+#include <vector>
#include "Eigen/Core"
#include "ceres/ceres.h"
@@ -99,6 +102,9 @@
"dense_qr",
"Options are: sparse_cholesky, dense_qr, dense_normal_cholesky "
"and cgnr");
+DEFINE_string(dense_linear_algebra_library,
+ "eigen",
+ "Options are: eigen, lapack, and cuda.");
DEFINE_string(preconditioner, "jacobi", "Options are: identity, jacobi");
DEFINE_string(line_search,
"wolfe",
@@ -114,7 +120,7 @@
"Maximum number of restarts of line search direction algorithm.");
DEFINE_string(line_search_interpolation,
"cubic",
- "Degree of polynomial aproximation in line search, choices are: "
+ "Degree of polynomial approximation in line search, choices are: "
"bisection, quadratic & cubic.");
DEFINE_int32(lbfgs_rank,
20,
@@ -148,26 +154,18 @@
3,
"Maximal number of Ridders extrapolations.");
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
namespace {
using Eigen::Dynamic;
using Eigen::RowMajor;
-typedef Eigen::Matrix<double, Dynamic, 1> Vector;
-typedef Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> Matrix;
+using Vector = Eigen::Matrix<double, Dynamic, 1>;
+using Matrix = Eigen::Matrix<double, Dynamic, Dynamic, RowMajor>;
-using std::atof;
-using std::atoi;
-using std::cout;
-using std::ifstream;
-using std::string;
-using std::vector;
-
-void SplitStringUsingChar(const string& full,
+void SplitStringUsingChar(const std::string& full,
const char delim,
- vector<string>* result) {
- std::back_insert_iterator<vector<string>> it(*result);
+ std::vector<std::string>* result) {
+ std::back_insert_iterator<std::vector<std::string>> it(*result);
const char* p = full.data();
const char* end = p + full.size();
@@ -177,22 +175,22 @@
} else {
const char* start = p;
while (++p != end && *p != delim) {
- // Skip to the next occurence of the delimiter.
+ // Skip to the next occurrence of the delimiter.
}
- *it++ = string(start, p - start);
+ *it++ = std::string(start, p - start);
}
}
}
-bool GetAndSplitLine(ifstream& ifs, vector<string>* pieces) {
+bool GetAndSplitLine(std::ifstream& ifs, std::vector<std::string>* pieces) {
pieces->clear();
char buf[256];
ifs.getline(buf, 256);
- SplitStringUsingChar(string(buf), ' ', pieces);
+ SplitStringUsingChar(std::string(buf), ' ', pieces);
return true;
}
-void SkipLines(ifstream& ifs, int num_lines) {
+void SkipLines(std::ifstream& ifs, int num_lines) {
char buf[256];
for (int i = 0; i < num_lines; ++i) {
ifs.getline(buf, 256);
@@ -201,24 +199,24 @@
class NISTProblem {
public:
- explicit NISTProblem(const string& filename) {
- ifstream ifs(filename.c_str(), ifstream::in);
+ explicit NISTProblem(const std::string& filename) {
+ std::ifstream ifs(filename.c_str(), std::ifstream::in);
CHECK(ifs) << "Unable to open : " << filename;
- vector<string> pieces;
+ std::vector<std::string> pieces;
SkipLines(ifs, 24);
GetAndSplitLine(ifs, &pieces);
- const int kNumResponses = atoi(pieces[1].c_str());
+ const int kNumResponses = std::atoi(pieces[1].c_str());
GetAndSplitLine(ifs, &pieces);
- const int kNumPredictors = atoi(pieces[0].c_str());
+ const int kNumPredictors = std::atoi(pieces[0].c_str());
GetAndSplitLine(ifs, &pieces);
- const int kNumObservations = atoi(pieces[0].c_str());
+ const int kNumObservations = std::atoi(pieces[0].c_str());
SkipLines(ifs, 4);
GetAndSplitLine(ifs, &pieces);
- const int kNumParameters = atoi(pieces[0].c_str());
+ const int kNumParameters = std::atoi(pieces[0].c_str());
SkipLines(ifs, 8);
// Get the first line of initial and final parameter values to
@@ -234,24 +232,26 @@
// Parse the line for parameter b1.
int parameter_id = 0;
for (int i = 0; i < kNumTries; ++i) {
- initial_parameters_(i, parameter_id) = atof(pieces[i + 2].c_str());
+ initial_parameters_(i, parameter_id) = std::atof(pieces[i + 2].c_str());
}
- final_parameters_(0, parameter_id) = atof(pieces[2 + kNumTries].c_str());
+ final_parameters_(0, parameter_id) =
+ std::atof(pieces[2 + kNumTries].c_str());
// Parse the remaining parameter lines.
for (int parameter_id = 1; parameter_id < kNumParameters; ++parameter_id) {
GetAndSplitLine(ifs, &pieces);
// b2, b3, ....
for (int i = 0; i < kNumTries; ++i) {
- initial_parameters_(i, parameter_id) = atof(pieces[i + 2].c_str());
+ initial_parameters_(i, parameter_id) = std::atof(pieces[i + 2].c_str());
}
- final_parameters_(0, parameter_id) = atof(pieces[2 + kNumTries].c_str());
+ final_parameters_(0, parameter_id) =
+ std::atof(pieces[2 + kNumTries].c_str());
}
- // Certfied cost
+ // Certified cost
SkipLines(ifs, 1);
GetAndSplitLine(ifs, &pieces);
- certified_cost_ = atof(pieces[4].c_str()) / 2.0;
+ certified_cost_ = std::atof(pieces[4].c_str()) / 2.0;
// Read the observations.
SkipLines(ifs, 18 - kNumParameters);
@@ -259,12 +259,12 @@
GetAndSplitLine(ifs, &pieces);
// Response.
for (int j = 0; j < kNumResponses; ++j) {
- response_(i, j) = atof(pieces[j].c_str());
+ response_(i, j) = std::atof(pieces[j].c_str());
}
// Predictor variables.
for (int j = 0; j < kNumPredictors; ++j) {
- predictor_(i, j) = atof(pieces[j + kNumResponses].c_str());
+ predictor_(i, j) = std::atof(pieces[j + kNumResponses].c_str());
}
}
}
@@ -455,47 +455,57 @@
// clang-format on
static void SetNumericDiffOptions(ceres::NumericDiffOptions* options) {
- options->max_num_ridders_extrapolations = FLAGS_ridders_extrapolations;
- options->ridders_relative_initial_step_size = FLAGS_ridders_step_size;
+ options->max_num_ridders_extrapolations =
+ CERES_GET_FLAG(FLAGS_ridders_extrapolations);
+ options->ridders_relative_initial_step_size =
+ CERES_GET_FLAG(FLAGS_ridders_step_size);
}
void SetMinimizerOptions(ceres::Solver::Options* options) {
- CHECK(
- ceres::StringToMinimizerType(FLAGS_minimizer, &options->minimizer_type));
- CHECK(ceres::StringToLinearSolverType(FLAGS_linear_solver,
+ CHECK(ceres::StringToMinimizerType(CERES_GET_FLAG(FLAGS_minimizer),
+ &options->minimizer_type));
+ CHECK(ceres::StringToLinearSolverType(CERES_GET_FLAG(FLAGS_linear_solver),
&options->linear_solver_type));
- CHECK(ceres::StringToPreconditionerType(FLAGS_preconditioner,
+ CHECK(StringToDenseLinearAlgebraLibraryType(
+ CERES_GET_FLAG(FLAGS_dense_linear_algebra_library),
+ &options->dense_linear_algebra_library_type));
+ CHECK(ceres::StringToPreconditionerType(CERES_GET_FLAG(FLAGS_preconditioner),
&options->preconditioner_type));
CHECK(ceres::StringToTrustRegionStrategyType(
- FLAGS_trust_region_strategy, &options->trust_region_strategy_type));
- CHECK(ceres::StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
+ CERES_GET_FLAG(FLAGS_trust_region_strategy),
+ &options->trust_region_strategy_type));
+ CHECK(ceres::StringToDoglegType(CERES_GET_FLAG(FLAGS_dogleg),
+ &options->dogleg_type));
CHECK(ceres::StringToLineSearchDirectionType(
- FLAGS_line_search_direction, &options->line_search_direction_type));
- CHECK(ceres::StringToLineSearchType(FLAGS_line_search,
+ CERES_GET_FLAG(FLAGS_line_search_direction),
+ &options->line_search_direction_type));
+ CHECK(ceres::StringToLineSearchType(CERES_GET_FLAG(FLAGS_line_search),
&options->line_search_type));
CHECK(ceres::StringToLineSearchInterpolationType(
- FLAGS_line_search_interpolation,
+ CERES_GET_FLAG(FLAGS_line_search_interpolation),
&options->line_search_interpolation_type));
- options->max_num_iterations = FLAGS_num_iterations;
- options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
- options->initial_trust_region_radius = FLAGS_initial_trust_region_radius;
- options->max_lbfgs_rank = FLAGS_lbfgs_rank;
- options->line_search_sufficient_function_decrease = FLAGS_sufficient_decrease;
+ options->max_num_iterations = CERES_GET_FLAG(FLAGS_num_iterations);
+ options->use_nonmonotonic_steps = CERES_GET_FLAG(FLAGS_nonmonotonic_steps);
+ options->initial_trust_region_radius =
+ CERES_GET_FLAG(FLAGS_initial_trust_region_radius);
+ options->max_lbfgs_rank = CERES_GET_FLAG(FLAGS_lbfgs_rank);
+ options->line_search_sufficient_function_decrease =
+ CERES_GET_FLAG(FLAGS_sufficient_decrease);
options->line_search_sufficient_curvature_decrease =
- FLAGS_sufficient_curvature_decrease;
+ CERES_GET_FLAG(FLAGS_sufficient_curvature_decrease);
options->max_num_line_search_step_size_iterations =
- FLAGS_max_line_search_iterations;
+ CERES_GET_FLAG(FLAGS_max_line_search_iterations);
options->max_num_line_search_direction_restarts =
- FLAGS_max_line_search_restarts;
+ CERES_GET_FLAG(FLAGS_max_line_search_restarts);
options->use_approximate_eigenvalue_bfgs_scaling =
- FLAGS_approximate_eigenvalue_bfgs_scaling;
+ CERES_GET_FLAG(FLAGS_approximate_eigenvalue_bfgs_scaling);
options->function_tolerance = std::numeric_limits<double>::epsilon();
options->gradient_tolerance = std::numeric_limits<double>::epsilon();
options->parameter_tolerance = std::numeric_limits<double>::epsilon();
}
-string JoinPath(const string& dirname, const string& basename) {
+std::string JoinPath(const std::string& dirname, const std::string& basename) {
#ifdef _WIN32
static const char separator = '\\';
#else
@@ -507,7 +517,7 @@
} else if (dirname[dirname.size() - 1] == separator) {
return dirname + basename;
} else {
- return dirname + string(&separator, 1) + basename;
+ return dirname + std::string(&separator, 1) + basename;
}
}
@@ -515,24 +525,24 @@
CostFunction* CreateCostFunction(const Matrix& predictor,
const Matrix& response,
const int num_observations) {
- Model* model = new Model(predictor.data(), response.data(), num_observations);
- ceres::CostFunction* cost_function = NULL;
- if (FLAGS_use_numeric_diff) {
+ auto* model = new Model(predictor.data(), response.data(), num_observations);
+ ceres::CostFunction* cost_function = nullptr;
+ if (CERES_GET_FLAG(FLAGS_use_numeric_diff)) {
ceres::NumericDiffOptions options;
SetNumericDiffOptions(&options);
- if (FLAGS_numeric_diff_method == "central") {
+ if (CERES_GET_FLAG(FLAGS_numeric_diff_method) == "central") {
cost_function = new NumericDiffCostFunction<Model,
ceres::CENTRAL,
ceres::DYNAMIC,
num_parameters>(
model, ceres::TAKE_OWNERSHIP, num_observations, options);
- } else if (FLAGS_numeric_diff_method == "forward") {
+ } else if (CERES_GET_FLAG(FLAGS_numeric_diff_method) == "forward") {
cost_function = new NumericDiffCostFunction<Model,
ceres::FORWARD,
ceres::DYNAMIC,
num_parameters>(
model, ceres::TAKE_OWNERSHIP, num_observations, options);
- } else if (FLAGS_numeric_diff_method == "ridders") {
+ } else if (CERES_GET_FLAG(FLAGS_numeric_diff_method) == "ridders") {
cost_function = new NumericDiffCostFunction<Model,
ceres::RIDDERS,
ceres::DYNAMIC,
@@ -540,7 +550,7 @@
model, ceres::TAKE_OWNERSHIP, num_observations, options);
} else {
LOG(ERROR) << "Invalid numeric diff method specified";
- return 0;
+ return nullptr;
}
} else {
cost_function =
@@ -571,8 +581,9 @@
}
template <typename Model, int num_parameters>
-int RegressionDriver(const string& filename) {
- NISTProblem nist_problem(JoinPath(FLAGS_nist_data_dir, filename));
+int RegressionDriver(const std::string& filename) {
+ NISTProblem nist_problem(
+ JoinPath(CERES_GET_FLAG(FLAGS_nist_data_dir), filename));
CHECK_EQ(num_parameters, nist_problem.num_parameters());
Matrix predictor = nist_problem.predictor();
@@ -593,9 +604,10 @@
double initial_cost;
double final_cost;
- if (!FLAGS_use_tiny_solver) {
+ 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);
@@ -605,15 +617,15 @@
} else {
ceres::TinySolverCostFunctionAdapter<Eigen::Dynamic, num_parameters> cfa(
*cost_function);
- typedef ceres::TinySolver<
- ceres::TinySolverCostFunctionAdapter<Eigen::Dynamic, num_parameters>>
- Solver;
+ using Solver = ceres::TinySolver<
+ ceres::TinySolverCostFunctionAdapter<Eigen::Dynamic, num_parameters>>;
Solver solver;
- solver.options.max_num_iterations = FLAGS_num_iterations;
+ solver.options.max_num_iterations = CERES_GET_FLAG(FLAGS_num_iterations);
solver.options.gradient_tolerance =
std::numeric_limits<double>::epsilon();
solver.options.parameter_tolerance =
std::numeric_limits<double>::epsilon();
+ solver.options.function_tolerance = 0.0;
Eigen::Matrix<double, num_parameters, 1> x;
x = initial_parameters.transpose();
@@ -645,11 +657,11 @@
}
void SolveNISTProblems() {
- if (FLAGS_nist_data_dir.empty()) {
+ if (CERES_GET_FLAG(FLAGS_nist_data_dir).empty()) {
LOG(FATAL) << "Must specify the directory containing the NIST problems";
}
- cout << "Lower Difficulty\n";
+ std::cout << "Lower Difficulty\n";
int easy_success = 0;
easy_success += RegressionDriver<Misra1a, 2>("Misra1a.dat");
easy_success += RegressionDriver<Chwirut, 3>("Chwirut1.dat");
@@ -660,7 +672,7 @@
easy_success += RegressionDriver<DanWood, 2>("DanWood.dat");
easy_success += RegressionDriver<Misra1b, 2>("Misra1b.dat");
- cout << "\nMedium Difficulty\n";
+ std::cout << "\nMedium Difficulty\n";
int medium_success = 0;
medium_success += RegressionDriver<Kirby2, 5>("Kirby2.dat");
medium_success += RegressionDriver<Hahn1, 7>("Hahn1.dat");
@@ -674,7 +686,7 @@
medium_success += RegressionDriver<Roszman1, 4>("Roszman1.dat");
medium_success += RegressionDriver<ENSO, 9>("ENSO.dat");
- cout << "\nHigher Difficulty\n";
+ std::cout << "\nHigher Difficulty\n";
int hard_success = 0;
hard_success += RegressionDriver<MGH09, 4>("MGH09.dat");
hard_success += RegressionDriver<Thurber, 7>("Thurber.dat");
@@ -685,17 +697,16 @@
hard_success += RegressionDriver<Rat43, 4>("Rat43.dat");
hard_success += RegressionDriver<Bennet5, 3>("Bennett5.dat");
- cout << "\n";
- cout << "Easy : " << easy_success << "/16\n";
- cout << "Medium : " << medium_success << "/22\n";
- cout << "Hard : " << hard_success << "/16\n";
- cout << "Total : " << easy_success + medium_success + hard_success
- << "/54\n";
+ std::cout << "\n";
+ std::cout << "Easy : " << easy_success << "/16\n";
+ std::cout << "Medium : " << medium_success << "/22\n";
+ std::cout << "Hard : " << hard_success << "/16\n";
+ std::cout << "Total : " << easy_success + medium_success + hard_success
+ << "/54\n";
}
} // namespace
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
int main(int argc, char** argv) {
GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
diff --git a/third_party/ceres/examples/pgm_image.h b/third_party/ceres/examples/pgm_image.h
index 3d2df63..033ab4d 100644
--- a/third_party/ceres/examples/pgm_image.h
+++ b/third_party/ceres/examples/pgm_image.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -43,8 +43,7 @@
#include "glog/logging.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
template <typename Real>
class PGMImage {
@@ -311,7 +310,6 @@
return data_;
}
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // CERES_EXAMPLES_PGM_IMAGE_H_
diff --git a/third_party/ceres/examples/powell.cc b/third_party/ceres/examples/powell.cc
index c75ad24..a4ca1b7 100644
--- a/third_party/ceres/examples/powell.cc
+++ b/third_party/ceres/examples/powell.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -50,12 +50,6 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
-using ceres::AutoDiffCostFunction;
-using ceres::CostFunction;
-using ceres::Problem;
-using ceres::Solve;
-using ceres::Solver;
-
struct F1 {
template <typename T>
bool operator()(const T* const x1, const T* const x2, T* residual) const {
@@ -105,24 +99,24 @@
double x3 = 0.0;
double x4 = 1.0;
- Problem problem;
- // Add residual terms to the problem using the using the autodiff
+ ceres::Problem problem;
+ // Add residual terms to the problem using the autodiff
// 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 ceres::AutoDiffCostFunction<F1, 1, 1, 1>(), nullptr, &x1, &x2);
problem.AddResidualBlock(
- new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4);
+ new ceres::AutoDiffCostFunction<F2, 1, 1, 1>(), nullptr, &x3, &x4);
problem.AddResidualBlock(
- new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3);
+ new ceres::AutoDiffCostFunction<F3, 1, 1, 1>(), nullptr, &x2, &x3);
problem.AddResidualBlock(
- new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4);
+ new ceres::AutoDiffCostFunction<F4, 1, 1, 1>(), nullptr, &x1, &x4);
- Solver::Options options;
- LOG_IF(
- FATAL,
- !ceres::StringToMinimizerType(FLAGS_minimizer, &options.minimizer_type))
- << "Invalid minimizer: " << FLAGS_minimizer
+ ceres::Solver::Options options;
+ 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.";
options.max_num_iterations = 100;
@@ -138,8 +132,8 @@
// clang-format on
// Run the solver!
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
// clang-format off
diff --git a/third_party/ceres/examples/random.h b/third_party/ceres/examples/random.h
deleted file mode 100644
index ace0711..0000000
--- a/third_party/ceres/examples/random.h
+++ /dev/null
@@ -1,64 +0,0 @@
-// 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: sameeragarwal@google.com (Sameer Agarwal)
-
-#ifndef CERES_EXAMPLES_RANDOM_H_
-#define CERES_EXAMPLES_RANDOM_H_
-
-#include <math.h>
-#include <stdlib.h>
-
-namespace ceres {
-namespace examples {
-
-// Return a random number sampled from a uniform distribution in the range
-// [0,1].
-inline double RandDouble() {
- double r = static_cast<double>(rand());
- return r / RAND_MAX;
-}
-
-// Marsaglia Polar method for generation standard normal (pseudo)
-// random numbers http://en.wikipedia.org/wiki/Marsaglia_polar_method
-inline double RandNormal() {
- double x1, x2, w;
- do {
- x1 = 2.0 * RandDouble() - 1.0;
- x2 = 2.0 * RandDouble() - 1.0;
- w = x1 * x1 + x2 * x2;
- } while (w >= 1.0 || w == 0.0);
-
- w = sqrt((-2.0 * log(w)) / w);
- return x1 * w;
-}
-
-} // namespace examples
-} // namespace ceres
-
-#endif // CERES_EXAMPLES_RANDOM_H_
diff --git a/third_party/ceres/examples/robot_pose_mle.cc b/third_party/ceres/examples/robot_pose_mle.cc
index ab9a098..cc60e14 100644
--- a/third_party/ceres/examples/robot_pose_mle.cc
+++ b/third_party/ceres/examples/robot_pose_mle.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -50,7 +50,7 @@
//
// 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.
+// between successive pose estimates of the robot.
// 2) The RangeConstraint residual, that accounts for the errors in the observed
// range readings from each pose.
//
@@ -97,14 +97,14 @@
// 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.
+// 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 odoemtry
+// 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}
@@ -123,30 +123,18 @@
// 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.
+// odometry observations will only be known at run time.
-#include <math.h>
-
+#include <algorithm>
+#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"
-#include "random.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 ceres::examples::RandNormal;
-using std::min;
-using std::vector;
DEFINE_double(corridor_length,
30.0,
@@ -169,7 +157,8 @@
static constexpr int kStride = 10;
struct OdometryConstraint {
- typedef AutoDiffCostFunction<OdometryConstraint, 1, 1> OdometryCostFunction;
+ using OdometryCostFunction =
+ ceres::AutoDiffCostFunction<OdometryConstraint, 1, 1>;
OdometryConstraint(double odometry_mean, double odometry_stddev)
: odometry_mean(odometry_mean), odometry_stddev(odometry_stddev) {}
@@ -181,8 +170,8 @@
}
static OdometryCostFunction* Create(const double odometry_value) {
- return new OdometryCostFunction(
- new OdometryConstraint(odometry_value, FLAGS_odometry_stddev));
+ return new OdometryCostFunction(new OdometryConstraint(
+ odometry_value, CERES_GET_FLAG(FLAGS_odometry_stddev)));
}
const double odometry_mean;
@@ -190,8 +179,8 @@
};
struct RangeConstraint {
- typedef DynamicAutoDiffCostFunction<RangeConstraint, kStride>
- RangeCostFunction;
+ using RangeCostFunction =
+ ceres::DynamicAutoDiffCostFunction<RangeConstraint, kStride>;
RangeConstraint(int pose_index,
double range_reading,
@@ -217,11 +206,14 @@
// 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);
+ std::vector<double>* odometry_values,
+ std::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) {
@@ -240,37 +232,45 @@
namespace {
-void SimulateRobot(vector<double>* odometry_values,
- vector<double>* range_readings) {
+void SimulateRobot(std::vector<double>* odometry_values,
+ std::vector<double>* range_readings) {
const int num_steps =
- static_cast<int>(ceil(FLAGS_corridor_length / FLAGS_pose_separation));
+ 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(FLAGS_pose_separation, FLAGS_corridor_length - robot_location);
+ std::min(CERES_GET_FLAG(FLAGS_pose_separation),
+ CERES_GET_FLAG(FLAGS_corridor_length) - robot_location);
robot_location += actual_odometry_value;
- const double actual_range = FLAGS_corridor_length - robot_location;
+ const double actual_range =
+ CERES_GET_FLAG(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;
+ 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) {
+void PrintState(const std::vector<double>& odometry_readings,
+ const std::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];
+ 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,
@@ -287,13 +287,13 @@
google::InitGoogleLogging(argv[0]);
GFLAGS_NAMESPACE::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);
+ 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;
+ std::vector<double> odometry_values;
+ std::vector<double> range_readings;
SimulateRobot(&odometry_values, &range_readings);
printf("Initial values:\n");
@@ -303,25 +303,25 @@
for (int i = 0; i < odometry_values.size(); ++i) {
// Create and add a DynamicAutoDiffCostFunction for the RangeConstraint from
// pose i.
- vector<double*> parameter_blocks;
+ std::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);
+ 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]));
}
ceres::Solver::Options solver_options;
solver_options.minimizer_progress_to_stdout = true;
- Solver::Summary summary;
+ ceres::Solver::Summary summary;
printf("Solving...\n");
- Solve(solver_options, &problem, &summary);
+ ceres::Solve(solver_options, &problem, &summary);
printf("Done.\n");
std::cout << summary.FullReport() << "\n";
printf("Final values:\n");
diff --git a/third_party/ceres/examples/robust_curve_fitting.cc b/third_party/ceres/examples/robust_curve_fitting.cc
index 9b526c5..e08b0df 100644
--- a/third_party/ceres/examples/robust_curve_fitting.cc
+++ b/third_party/ceres/examples/robust_curve_fitting.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -27,6 +27,12 @@
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// This example fits the curve f(x;m,c) = e^(m * x + c) to data. However unlike
+// the data in curve_fitting.cc, the data here has outliers in it, so minimizing
+// the sum squared loss will result in a bad fit. So this example illustrates
+// the use of a robust loss function (CauchyLoss) to reduce the influence of the
+// outliers on the fit.
#include "ceres/ceres.h"
#include "glog/logging.h"
@@ -115,13 +121,6 @@
};
// clang-format on
-using ceres::AutoDiffCostFunction;
-using ceres::CauchyLoss;
-using ceres::CostFunction;
-using ceres::Problem;
-using ceres::Solve;
-using ceres::Solver;
-
struct ExponentialResidual {
ExponentialResidual(double x, double y) : x_(x), y_(y) {}
@@ -139,25 +138,28 @@
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
- double m = 0.0;
- double c = 0.0;
+ const double initial_m = 0.0;
+ const double initial_c = 0.0;
+ double m = initial_m;
+ double c = initial_c;
- Problem problem;
+ ceres::Problem problem;
for (int i = 0; i < kNumObservations; ++i) {
- CostFunction* cost_function =
- new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
- new ExponentialResidual(data[2 * i], data[2 * i + 1]));
- problem.AddResidualBlock(cost_function, new CauchyLoss(0.5), &m, &c);
+ ceres::CostFunction* cost_function =
+ new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
+ data[2 * i], data[2 * i + 1]);
+ problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(0.5), &m, &c);
}
- Solver::Options options;
+ ceres::Solver::Options options;
+ options.max_num_iterations = 25;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
- std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n";
+ std::cout << "Initial m: " << initial_m << " c: " << initial_c << "\n";
std::cout << "Final m: " << m << " c: " << c << "\n";
return 0;
}
diff --git a/third_party/ceres/examples/rosenbrock.cc b/third_party/ceres/examples/rosenbrock.cc
index 1b9aef6..a382ccd 100644
--- a/third_party/ceres/examples/rosenbrock.cc
+++ b/third_party/ceres/examples/rosenbrock.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -27,30 +27,29 @@
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Example of minimizing the Rosenbrock function
+// (https://en.wikipedia.org/wiki/Rosenbrock_function) using
+// GradientProblemSolver using automatically computed derivatives.
#include "ceres/ceres.h"
#include "glog/logging.h"
// f(x,y) = (1-x)^2 + 100(y - x^2)^2;
-class Rosenbrock : public ceres::FirstOrderFunction {
- public:
- virtual ~Rosenbrock() {}
-
- virtual bool Evaluate(const double* parameters,
- double* cost,
- double* gradient) const {
- const double x = parameters[0];
- const double y = parameters[1];
-
+struct Rosenbrock {
+ template <typename T>
+ bool operator()(const T* parameters, T* cost) const {
+ const T x = parameters[0];
+ const T y = parameters[1];
cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
- if (gradient != NULL) {
- gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x;
- gradient[1] = 200.0 * (y - x * x);
- }
return true;
}
- virtual int NumParameters() const { return 2; }
+ static ceres::FirstOrderFunction* Create() {
+ constexpr int kNumParameters = 2;
+ return new ceres::AutoDiffFirstOrderFunction<Rosenbrock, kNumParameters>(
+ new Rosenbrock);
+ }
};
int main(int argc, char** argv) {
@@ -62,7 +61,7 @@
options.minimizer_progress_to_stdout = true;
ceres::GradientProblemSolver::Summary summary;
- ceres::GradientProblem problem(new Rosenbrock());
+ ceres::GradientProblem problem(Rosenbrock::Create());
ceres::Solve(options, problem, parameters, &summary);
std::cout << summary.FullReport() << "\n";
diff --git a/third_party/ceres/examples/rosenbrock_analytic_diff.cc b/third_party/ceres/examples/rosenbrock_analytic_diff.cc
new file mode 100644
index 0000000..65e49eb
--- /dev/null
+++ b/third_party/ceres/examples/rosenbrock_analytic_diff.cc
@@ -0,0 +1,77 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Example of minimizing the Rosenbrock function
+// (https://en.wikipedia.org/wiki/Rosenbrock_function) using
+// GradientProblemSolver using analytic derivatives.
+
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+// f(x,y) = (1-x)^2 + 100(y - x^2)^2;
+class Rosenbrock final : public ceres::FirstOrderFunction {
+ public:
+ bool Evaluate(const double* parameters,
+ double* cost,
+ double* gradient) const override {
+ const double x = parameters[0];
+ const double y = parameters[1];
+
+ cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
+
+ if (gradient) {
+ gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x;
+ gradient[1] = 200.0 * (y - x * x);
+ }
+
+ return true;
+ }
+
+ int NumParameters() const override { return 2; }
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ double parameters[2] = {-1.2, 1.0};
+
+ ceres::GradientProblemSolver::Options options;
+ options.minimizer_progress_to_stdout = true;
+
+ ceres::GradientProblemSolver::Summary summary;
+ ceres::GradientProblem problem(new Rosenbrock());
+ ceres::Solve(options, problem, parameters, &summary);
+
+ std::cout << summary.FullReport() << "\n";
+ std::cout << "Initial x: " << -1.2 << " y: " << 1.0 << "\n";
+ std::cout << "Final x: " << parameters[0] << " y: " << parameters[1]
+ << "\n";
+ return 0;
+}
diff --git a/third_party/ceres/examples/rosenbrock_numeric_diff.cc b/third_party/ceres/examples/rosenbrock_numeric_diff.cc
new file mode 100644
index 0000000..a711b2f
--- /dev/null
+++ b/third_party/ceres/examples/rosenbrock_numeric_diff.cc
@@ -0,0 +1,74 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Example of minimizing the Rosenbrock function
+// (https://en.wikipedia.org/wiki/Rosenbrock_function) using
+// GradientProblemSolver using derivatives computed using numeric
+// differentiation.
+
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+// f(x,y) = (1-x)^2 + 100(y - x^2)^2;
+struct Rosenbrock {
+ bool operator()(const double* parameters, double* cost) const {
+ const double x = parameters[0];
+ const double y = parameters[1];
+ cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
+ return true;
+ }
+
+ static ceres::FirstOrderFunction* Create() {
+ constexpr int kNumParameters = 2;
+ return new ceres::NumericDiffFirstOrderFunction<Rosenbrock,
+ ceres::CENTRAL,
+ kNumParameters>(
+ new Rosenbrock);
+ }
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ double parameters[2] = {-1.2, 1.0};
+
+ ceres::GradientProblemSolver::Options options;
+ options.minimizer_progress_to_stdout = true;
+
+ ceres::GradientProblemSolver::Summary summary;
+ ceres::GradientProblem problem(Rosenbrock::Create());
+ ceres::Solve(options, problem, parameters, &summary);
+
+ std::cout << summary.FullReport() << "\n";
+ std::cout << "Initial x: " << -1.2 << " y: " << 1.0 << "\n";
+ std::cout << "Final x: " << parameters[0] << " y: " << parameters[1]
+ << "\n";
+ return 0;
+}
diff --git a/third_party/ceres/examples/sampled_function/CMakeLists.txt b/third_party/ceres/examples/sampled_function/CMakeLists.txt
index 8a17cad..1013753 100644
--- a/third_party/ceres/examples/sampled_function/CMakeLists.txt
+++ b/third_party/ceres/examples/sampled_function/CMakeLists.txt
@@ -1,5 +1,5 @@
# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
+# Copyright 2023 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
@@ -29,4 +29,4 @@
# Author: vitus@google.com (Michael Vitus)
add_executable(sampled_function sampled_function.cc)
-target_link_libraries(sampled_function Ceres::ceres)
+target_link_libraries(sampled_function PRIVATE Ceres::ceres)
diff --git a/third_party/ceres/examples/sampled_function/README.md b/third_party/ceres/examples/sampled_function/README.md
index ef1af43..5fde415 100644
--- a/third_party/ceres/examples/sampled_function/README.md
+++ b/third_party/ceres/examples/sampled_function/README.md
@@ -32,7 +32,7 @@
```c++
bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
- if (jacobians == NULL || jacobians[0] == NULL)
+ if (jacobians == nullptr || jacobians[0] == nullptr)
interpolator_.Evaluate(parameters[0][0], residuals);
else
interpolator_.Evaluate(parameters[0][0], residuals, jacobians[0]);
diff --git a/third_party/ceres/examples/sampled_function/sampled_function.cc b/third_party/ceres/examples/sampled_function/sampled_function.cc
index e96018d..40e9c1f 100644
--- a/third_party/ceres/examples/sampled_function/sampled_function.cc
+++ b/third_party/ceres/examples/sampled_function/sampled_function.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -35,35 +35,27 @@
#include "ceres/cubic_interpolation.h"
#include "glog/logging.h"
-using ceres::AutoDiffCostFunction;
-using ceres::CostFunction;
-using ceres::CubicInterpolator;
-using ceres::Grid1D;
-using ceres::Problem;
-using ceres::Solve;
-using ceres::Solver;
+using Interpolator = ceres::CubicInterpolator<ceres::Grid1D<double>>;
// A simple cost functor that interfaces an interpolated table of
// values with automatic differentiation.
struct InterpolatedCostFunctor {
- explicit InterpolatedCostFunctor(
- const CubicInterpolator<Grid1D<double>>& interpolator)
- : interpolator_(interpolator) {}
+ explicit InterpolatedCostFunctor(const Interpolator& interpolator)
+ : interpolator(interpolator) {}
template <typename T>
bool operator()(const T* x, T* residuals) const {
- interpolator_.Evaluate(*x, residuals);
+ interpolator.Evaluate(*x, residuals);
return true;
}
- static CostFunction* Create(
- const CubicInterpolator<Grid1D<double>>& interpolator) {
- return new AutoDiffCostFunction<InterpolatedCostFunctor, 1, 1>(
- new InterpolatedCostFunctor(interpolator));
+ static ceres::CostFunction* Create(const Interpolator& interpolator) {
+ return new ceres::AutoDiffCostFunction<InterpolatedCostFunctor, 1, 1>(
+ interpolator);
}
private:
- const CubicInterpolator<Grid1D<double>>& interpolator_;
+ const Interpolator& interpolator;
};
int main(int argc, char** argv) {
@@ -76,18 +68,19 @@
values[i] = (i - 4.5) * (i - 4.5);
}
- Grid1D<double> array(values, 0, kNumSamples);
- CubicInterpolator<Grid1D<double>> interpolator(array);
+ ceres::Grid1D<double> array(values, 0, kNumSamples);
+ Interpolator interpolator(array);
double x = 1.0;
- Problem problem;
- CostFunction* cost_function = InterpolatedCostFunctor::Create(interpolator);
- problem.AddResidualBlock(cost_function, NULL, &x);
+ ceres::Problem problem;
+ ceres::CostFunction* cost_function =
+ InterpolatedCostFunctor::Create(interpolator);
+ problem.AddResidualBlock(cost_function, nullptr, &x);
- Solver::Options options;
+ ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "Expected x: 4.5. Actual x : " << x << std::endl;
return 0;
diff --git a/third_party/ceres/examples/simple_bundle_adjuster.cc b/third_party/ceres/examples/simple_bundle_adjuster.cc
index 8180d73..bb0ba1c 100644
--- a/third_party/ceres/examples/simple_bundle_adjuster.cc
+++ b/third_party/ceres/examples/simple_bundle_adjuster.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -66,7 +66,7 @@
bool LoadFile(const char* filename) {
FILE* fptr = fopen(filename, "r");
- if (fptr == NULL) {
+ if (fptr == nullptr) {
return false;
};
@@ -164,8 +164,8 @@
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
- return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
- new SnavelyReprojectionError(observed_x, observed_y)));
+ return new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
+ observed_x, observed_y);
}
double observed_x;
@@ -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/third_party/ceres/examples/slam/CMakeLists.txt b/third_party/ceres/examples/slam/CMakeLists.txt
index c72aa16..d0b03d7 100644
--- a/third_party/ceres/examples/slam/CMakeLists.txt
+++ b/third_party/ceres/examples/slam/CMakeLists.txt
@@ -1,5 +1,5 @@
# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2016 Google Inc. All rights reserved.
+# Copyright 2023 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
diff --git a/third_party/ceres/examples/slam/common/read_g2o.h b/third_party/ceres/examples/slam/common/read_g2o.h
index fea32e9..490b054 100644
--- a/third_party/ceres/examples/slam/common/read_g2o.h
+++ b/third_party/ceres/examples/slam/common/read_g2o.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -38,8 +38,7 @@
#include "glog/logging.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
// Reads a single pose from the input and inserts it into the map. Returns false
// if there is a duplicate entry.
@@ -60,7 +59,7 @@
return true;
}
-// Reads the contraints between two vertices in the pose graph
+// Reads the constraints between two vertices in the pose graph
template <typename Constraint, typename Allocator>
void ReadConstraint(std::ifstream* infile,
std::vector<Constraint, Allocator>* constraints) {
@@ -104,8 +103,8 @@
bool ReadG2oFile(const std::string& filename,
std::map<int, Pose, std::less<int>, MapAllocator>* poses,
std::vector<Constraint, VectorAllocator>* constraints) {
- CHECK(poses != NULL);
- CHECK(constraints != NULL);
+ CHECK(poses != nullptr);
+ CHECK(constraints != nullptr);
poses->clear();
constraints->clear();
@@ -137,7 +136,6 @@
return true;
}
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // EXAMPLES_CERES_READ_G2O_H_
diff --git a/third_party/ceres/examples/slam/pose_graph_2d/CMakeLists.txt b/third_party/ceres/examples/slam/pose_graph_2d/CMakeLists.txt
index 20af056..87943ec 100644
--- a/third_party/ceres/examples/slam/pose_graph_2d/CMakeLists.txt
+++ b/third_party/ceres/examples/slam/pose_graph_2d/CMakeLists.txt
@@ -1,5 +1,5 @@
# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2016 Google Inc. All rights reserved.
+# Copyright 2023 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
@@ -30,10 +30,10 @@
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
types.h)
- target_link_libraries(pose_graph_2d Ceres::ceres gflags)
+ target_link_libraries(pose_graph_2d PRIVATE Ceres::ceres gflags)
endif (GFLAGS)
diff --git a/third_party/ceres/examples/slam/pose_graph_2d/angle_local_parameterization.h b/third_party/ceres/examples/slam/pose_graph_2d/angle_manifold.h
similarity index 63%
rename from third_party/ceres/examples/slam/pose_graph_2d/angle_local_parameterization.h
rename to third_party/ceres/examples/slam/pose_graph_2d/angle_manifold.h
index a81637c..456d923 100644
--- a/third_party/ceres/examples/slam/pose_graph_2d/angle_local_parameterization.h
+++ b/third_party/ceres/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 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -27,38 +27,42 @@
// 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 {
+namespace ceres::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
+} // namespace ceres::examples
-#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
diff --git a/third_party/ceres/examples/slam/pose_graph_2d/normalize_angle.h b/third_party/ceres/examples/slam/pose_graph_2d/normalize_angle.h
index c215671..0602878 100644
--- a/third_party/ceres/examples/slam/pose_graph_2d/normalize_angle.h
+++ b/third_party/ceres/examples/slam/pose_graph_2d/normalize_angle.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -35,19 +35,17 @@
#include "ceres/ceres.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
// Normalizes the angle in radians between [-pi and pi).
template <typename T>
inline T NormalizeAngle(const T& angle_radians) {
// Use ceres::floor because it is specialized for double and Jet types.
- T two_pi(2.0 * M_PI);
+ T two_pi(2.0 * constants::pi);
return angle_radians -
- two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
+ two_pi * ceres::floor((angle_radians + T(constants::pi)) / two_pi);
}
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
diff --git a/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc b/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
index 1172123..3ebae3f 100644
--- a/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+++ b/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -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"
@@ -49,8 +49,7 @@
DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
namespace {
// Constructs the nonlinear least squares optimization problem from the pose
@@ -58,29 +57,21 @@
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();
- constraints_iter != constraints.end();
- ++constraints_iter) {
- const Constraint2d& constraint = *constraints_iter;
-
- std::map<int, Pose2d>::iterator pose_begin_iter =
- poses->find(constraint.id_begin);
+ for (const auto& constraint : constraints) {
+ auto pose_begin_iter = poses->find(constraint.id_begin);
CHECK(pose_begin_iter != poses->end())
<< "Pose with ID: " << constraint.id_begin << " not found.";
- std::map<int, Pose2d>::iterator pose_end_iter =
- poses->find(constraint.id_end);
+ auto pose_end_iter = poses->find(constraint.id_end);
CHECK(pose_end_iter != poses->end())
<< "Pose with ID: " << constraint.id_end << " not found.";
@@ -98,10 +89,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
@@ -111,7 +100,7 @@
// internal damping which mitigate this issue, but it is better to properly
// constrain the gauge freedom. This can be done by setting one of the poses
// as constant so the optimizer cannot change it.
- std::map<int, Pose2d>::iterator pose_start_iter = poses->begin();
+ auto pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(&pose_start_iter->second.x);
problem->SetParameterBlockConstant(&pose_start_iter->second.y);
@@ -120,7 +109,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;
@@ -143,10 +132,7 @@
std::cerr << "Error opening the file: " << filename << '\n';
return false;
}
- for (std::map<int, Pose2d>::const_iterator poses_iter = poses.begin();
- poses_iter != poses.end();
- ++poses_iter) {
- const std::map<int, Pose2d>::value_type& pair = *poses_iter;
+ for (const auto& pair : poses) {
outfile << pair.first << " " << pair.second.x << " " << pair.second.y << ' '
<< pair.second.yaw_radians << '\n';
}
@@ -154,8 +140,7 @@
}
} // namespace
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
diff --git a/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h b/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h
index 2df31f6..3d34f8d 100644
--- a/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h
+++ b/third_party/ceres/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -34,9 +34,9 @@
#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
@@ -96,10 +96,9 @@
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information) {
- return (new ceres::
- AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
- new PoseGraph2dErrorTerm(
- x_ab, y_ab, yaw_ab_radians, sqrt_information)));
+ return new ceres::
+ AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
+ x_ab, y_ab, yaw_ab_radians, sqrt_information);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -113,7 +112,6 @@
const Eigen::Matrix3d sqrt_information_;
};
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
diff --git a/third_party/ceres/examples/slam/pose_graph_2d/types.h b/third_party/ceres/examples/slam/pose_graph_2d/types.h
index 3c13824..caf4ccb 100644
--- a/third_party/ceres/examples/slam/pose_graph_2d/types.h
+++ b/third_party/ceres/examples/slam/pose_graph_2d/types.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -40,8 +40,7 @@
#include "Eigen/Core"
#include "normalize_angle.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
// The state for each vertex in the pose graph.
struct Pose2d {
@@ -95,7 +94,6 @@
return input;
}
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
diff --git a/third_party/ceres/examples/slam/pose_graph_3d/CMakeLists.txt b/third_party/ceres/examples/slam/pose_graph_3d/CMakeLists.txt
index b6421cc..544b00e 100644
--- a/third_party/ceres/examples/slam/pose_graph_3d/CMakeLists.txt
+++ b/third_party/ceres/examples/slam/pose_graph_3d/CMakeLists.txt
@@ -1,5 +1,5 @@
# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2016 Google Inc. All rights reserved.
+# Copyright 2023 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
@@ -30,5 +30,5 @@
if (GFLAGS)
add_executable(pose_graph_3d pose_graph_3d.cc)
- target_link_libraries(pose_graph_3d Ceres::ceres gflags)
+ target_link_libraries(pose_graph_3d PRIVATE Ceres::ceres gflags)
endif (GFLAGS)
diff --git a/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc b/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
index 2f8d6a4..522e2a1 100644
--- a/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
+++ b/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -41,8 +41,7 @@
DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
namespace {
// Constructs the nonlinear least squares optimization problem from the pose
@@ -50,27 +49,21 @@
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();
- constraints_iter != constraints.end();
- ++constraints_iter) {
- const Constraint3d& constraint = *constraints_iter;
-
- MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
+ for (const auto& constraint : constraints) {
+ auto pose_begin_iter = poses->find(constraint.id_begin);
CHECK(pose_begin_iter != poses->end())
<< "Pose with ID: " << constraint.id_begin << " not found.";
- MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
+ auto pose_end_iter = poses->find(constraint.id_end);
CHECK(pose_end_iter != poses->end())
<< "Pose with ID: " << constraint.id_end << " not found.";
@@ -87,10 +80,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
@@ -100,7 +93,7 @@
// internal damping which mitigates this issue, but it is better to properly
// constrain the gauge freedom. This can be done by setting one of the poses
// as constant so the optimizer cannot change it.
- MapOfPoses::iterator pose_start_iter = poses->begin();
+ auto pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
@@ -108,7 +101,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;
@@ -130,18 +123,7 @@
LOG(ERROR) << "Error opening the file: " << filename;
return false;
}
- for (std::map<int,
- Pose3d,
- std::less<int>,
- Eigen::aligned_allocator<std::pair<const int, Pose3d>>>::
- const_iterator poses_iter = poses.begin();
- poses_iter != poses.end();
- ++poses_iter) {
- const std::map<int,
- Pose3d,
- std::less<int>,
- Eigen::aligned_allocator<std::pair<const int, Pose3d>>>::
- value_type& pair = *poses_iter;
+ for (const auto& pair : poses) {
outfile << pair.first << " " << pair.second.p.transpose() << " "
<< pair.second.q.x() << " " << pair.second.q.y() << " "
<< pair.second.q.z() << " " << pair.second.q.w() << '\n';
@@ -150,8 +132,7 @@
}
} // namespace
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
diff --git a/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h b/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
index 1f3e8de..b1c0138 100644
--- a/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
+++ b/third_party/ceres/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -31,12 +31,13 @@
#ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+#include <utility>
+
#include "Eigen/Core"
#include "ceres/autodiff_cost_function.h"
#include "types.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement. We have two poses x_a
@@ -69,9 +70,10 @@
// where I is the information matrix which is the inverse of the covariance.
class PoseGraph3dErrorTerm {
public:
- PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
- const Eigen::Matrix<double, 6, 6>& sqrt_information)
- : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+ PoseGraph3dErrorTerm(Pose3d t_ab_measured,
+ Eigen::Matrix<double, 6, 6> sqrt_information)
+ : t_ab_measured_(std::move(t_ab_measured)),
+ sqrt_information_(std::move(sqrt_information)) {}
template <typename T>
bool operator()(const T* const p_a_ptr,
@@ -114,7 +116,7 @@
const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information) {
return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
- new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+ t_ab_measured, sqrt_information);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -126,7 +128,6 @@
const Eigen::Matrix<double, 6, 6> sqrt_information_;
};
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
diff --git a/third_party/ceres/examples/slam/pose_graph_3d/types.h b/third_party/ceres/examples/slam/pose_graph_3d/types.h
index d3f19ed..207dd5d 100644
--- a/third_party/ceres/examples/slam/pose_graph_3d/types.h
+++ b/third_party/ceres/examples/slam/pose_graph_3d/types.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -31,6 +31,7 @@
#ifndef EXAMPLES_CERES_TYPES_H_
#define EXAMPLES_CERES_TYPES_H_
+#include <functional>
#include <istream>
#include <map>
#include <string>
@@ -39,8 +40,7 @@
#include "Eigen/Core"
#include "Eigen/Geometry"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
struct Pose3d {
Eigen::Vector3d p;
@@ -61,11 +61,11 @@
return input;
}
-typedef std::map<int,
- Pose3d,
- std::less<int>,
- Eigen::aligned_allocator<std::pair<const int, Pose3d>>>
- MapOfPoses;
+using MapOfPoses =
+ std::map<int,
+ Pose3d,
+ std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d>>>;
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
@@ -103,10 +103,9 @@
return input;
}
-typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d>>
- VectorOfConstraints;
+using VectorOfConstraints =
+ std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d>>;
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // EXAMPLES_CERES_TYPES_H_
diff --git a/third_party/ceres/examples/snavely_reprojection_error.h b/third_party/ceres/examples/snavely_reprojection_error.h
index eb39d23..aaf0c6c 100644
--- a/third_party/ceres/examples/snavely_reprojection_error.h
+++ b/third_party/ceres/examples/snavely_reprojection_error.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -41,10 +41,10 @@
#ifndef CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
#define CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
+#include "ceres/autodiff_cost_function.h"
#include "ceres/rotation.h"
-namespace ceres {
-namespace examples {
+namespace ceres::examples {
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
@@ -95,8 +95,8 @@
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
- return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
- new SnavelyReprojectionError(observed_x, observed_y)));
+ return new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
+ observed_x, observed_y);
}
double observed_x;
@@ -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);
@@ -160,20 +160,15 @@
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
- return (
- new ceres::AutoDiffCostFunction<SnavelyReprojectionErrorWithQuaternions,
- 2,
- 10,
- 3>(
- new SnavelyReprojectionErrorWithQuaternions(observed_x,
- observed_y)));
+ return new ceres::
+ AutoDiffCostFunction<SnavelyReprojectionErrorWithQuaternions, 2, 10, 3>(
+ observed_x, observed_y);
}
double observed_x;
double observed_y;
};
-} // namespace examples
-} // namespace ceres
+} // namespace ceres::examples
#endif // CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_