tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(3rdparty): add eigen and ceres

Browse Source
This commit is contained in:
John Zhao
2019-01-03 16:25:18 +08:00
parent c6fd9db827
commit 6773d8eb7a
747 changed files with 375754 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
3rdparty/ceres-solver-1.11.0/examples/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,118 @@
# 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)
# 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)
add_executable(helloworld_numeric_diff helloworld_numeric_diff.cc)
target_link_libraries(helloworld_numeric_diff ceres)
add_executable(helloworld_analytic_diff helloworld_analytic_diff.cc)
target_link_libraries(helloworld_analytic_diff ceres)
add_executable(curve_fitting curve_fitting.cc)
target_link_libraries(curve_fitting ceres)
add_executable(rosenbrock rosenbrock.cc)
target_link_libraries(rosenbrock ceres)
add_executable(curve_fitting_c curve_fitting.c)
target_link_libraries(curve_fitting_c ceres)
# Force CMake to link curve_fitting_c using the C linker, this is important
# when Ceres was compiled using C++11 to ensure that -std=c++11 is not passed
# through.
set_target_properties(curve_fitting_c PROPERTIES LINKER_LANGUAGE C)
# 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)
add_executable(ellipse_approximation ellipse_approximation.cc)
target_link_libraries(ellipse_approximation ceres)
add_executable(robust_curve_fitting robust_curve_fitting.cc)
target_link_libraries(robust_curve_fitting ceres)
add_executable(simple_bundle_adjuster simple_bundle_adjuster.cc)
target_link_libraries(simple_bundle_adjuster ceres)
add_executable(sampled_function sampled_function.cc)
target_link_libraries(sampled_function ceres)
if (GFLAGS)
# The CERES_GFLAGS_NAMESPACE compile definition is NOT stored in
# CERES_COMPILE_OPTIONS (and thus config.h) as Ceres itself does not
# require gflags, only the tests and examples do.
add_definitions(-DCERES_GFLAGS_NAMESPACE=${GFLAGS_NAMESPACE})
add_executable(powell powell.cc)
target_link_libraries(powell ceres ${GFLAGS_LIBRARIES})
add_executable(nist nist.cc)
target_link_libraries(nist ceres ${GFLAGS_LIBRARIES})
add_executable(more_garbow_hillstrom more_garbow_hillstrom.cc)
target_link_libraries(more_garbow_hillstrom ceres ${GFLAGS_LIBRARIES})
add_executable(circle_fit circle_fit.cc)
target_link_libraries(circle_fit ceres ${GFLAGS_LIBRARIES})
add_executable(bundle_adjuster
bundle_adjuster.cc
bal_problem.cc)
target_link_libraries(bundle_adjuster ceres ${GFLAGS_LIBRARIES})
add_executable(libmv_bundle_adjuster
libmv_bundle_adjuster.cc)
target_link_libraries(libmv_bundle_adjuster ceres ${GFLAGS_LIBRARIES})
add_executable(libmv_homography
libmv_homography.cc)
target_link_libraries(libmv_homography ceres ${GFLAGS_LIBRARIES})
add_executable(denoising
denoising.cc
fields_of_experts.cc)
target_link_libraries(denoising ceres ${GFLAGS_LIBRARIES})
add_executable(robot_pose_mle
robot_pose_mle.cc)
target_link_libraries(robot_pose_mle ceres ${GFLAGS_LIBRARIES})
endif (GFLAGS)

82
3rdparty/ceres-solver-1.11.0/examples/Makefile.example vendored Normal file
View File

@@ -0,0 +1,82 @@
# 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)

339
3rdparty/ceres-solver-1.11.0/examples/bal_problem.cc vendored Normal file
View File

@@ -0,0 +1,339 @@
// 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)
#include "bal_problem.h"
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "ceres/rotation.h"
#include "glog/logging.h"
#include "random.h"
namespace ceres {
namespace examples {
namespace {
typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
template<typename T>
void FscanfOrDie(FILE* fptr, const char* format, T* value) {
int num_scanned = fscanf(fptr, format, value);
if (num_scanned != 1) {
LOG(FATAL) << "Invalid UW data file.";
}
}
void PerturbPoint3(const double sigma, double* point) {
for (int i = 0; i < 3; ++i) {
point[i] += RandNormal() * sigma;
}
}
double Median(std::vector<double>* data) {
int n = data->size();
std::vector<double>::iterator mid_point = data->begin() + n / 2;
std::nth_element(data->begin(), mid_point, data->end());
return *mid_point;
}
} // namespace
BALProblem::BALProblem(const std::string& filename, bool use_quaternions) {
FILE* fptr = fopen(filename.c_str(), "r");
if (fptr == NULL) {
LOG(FATAL) << "Error: unable to open file " << filename;
return;
};
// This wil die horribly on invalid files. Them's the breaks.
FscanfOrDie(fptr, "%d", &num_cameras_);
FscanfOrDie(fptr, "%d", &num_points_);
FscanfOrDie(fptr, "%d", &num_observations_);
VLOG(1) << "Header: " << num_cameras_
<< " " << num_points_
<< " " << num_observations_;
point_index_ = new int[num_observations_];
camera_index_ = new int[num_observations_];
observations_ = new double[2 * num_observations_];
num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
parameters_ = new double[num_parameters_];
for (int i = 0; i < num_observations_; ++i) {
FscanfOrDie(fptr, "%d", camera_index_ + i);
FscanfOrDie(fptr, "%d", point_index_ + i);
for (int j = 0; j < 2; ++j) {
FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
}
}
for (int i = 0; i < num_parameters_; ++i) {
FscanfOrDie(fptr, "%lf", parameters_ + i);
}
fclose(fptr);
use_quaternions_ = use_quaternions;
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_];
double* original_cursor = parameters_;
double* quaternion_cursor = quaternion_parameters;
for (int i = 0; i < num_cameras_; ++i) {
AngleAxisToQuaternion(original_cursor, quaternion_cursor);
quaternion_cursor += 4;
original_cursor += 3;
for (int j = 4; j < 10; ++j) {
*quaternion_cursor++ = *original_cursor++;
}
}
// Copy the rest of the points.
for (int i = 0; i < 3 * num_points_; ++i) {
*quaternion_cursor++ = *original_cursor++;
}
// Swap in the quaternion parameters.
delete []parameters_;
parameters_ = quaternion_parameters;
}
}
// This function writes the problem to a file in the same format that
// is read by the constructor.
void BALProblem::WriteToFile(const std::string& filename) const {
FILE* fptr = fopen(filename.c_str(), "w");
if (fptr == NULL) {
LOG(FATAL) << "Error: unable to open file " << filename;
return;
};
fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);
for (int i = 0; i < num_observations_; ++i) {
fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
for (int j = 0; j < 2; ++j) {
fprintf(fptr, " %g", observations_[2 * i + j]);
}
fprintf(fptr, "\n");
}
for (int i = 0; i < num_cameras(); ++i) {
double angleaxis[9];
if (use_quaternions_) {
// Output in angle-axis format.
QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
} else {
memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
}
for (int j = 0; j < 9; ++j) {
fprintf(fptr, "%.16g\n", angleaxis[j]);
}
}
const double* points = parameters_ + camera_block_size() * num_cameras_;
for (int i = 0; i < num_points(); ++i) {
const double* point = points + i * point_block_size();
for (int j = 0; j < point_block_size(); ++j) {
fprintf(fptr, "%.16g\n", point[j]);
}
}
fclose(fptr);
}
// Write the problem to a PLY file for inspection in Meshlab or CloudCompare.
void BALProblem::WriteToPLYFile(const std::string& filename) const {
std::ofstream of(filename.c_str());
of << "ply"
<< '\n' << "format ascii 1.0"
<< '\n' << "element vertex " << num_cameras_ + num_points_
<< '\n' << "property float x"
<< '\n' << "property float y"
<< '\n' << "property float z"
<< '\n' << "property uchar red"
<< '\n' << "property uchar green"
<< '\n' << "property uchar blue"
<< '\n' << "end_header" << std::endl;
// Export extrinsic data (i.e. camera centers) as green points.
double angle_axis[3];
double center[3];
for (int i = 0; i < num_cameras(); ++i) {
const double* camera = cameras() + camera_block_size() * i;
CameraToAngleAxisAndCenter(camera, angle_axis, center);
of << center[0] << ' ' << center[1] << ' ' << center[2]
<< " 0 255 0" << '\n';
}
// Export the structure (i.e. 3D Points) as white points.
const double* points = parameters_ + camera_block_size() * num_cameras_;
for (int i = 0; i < num_points(); ++i) {
const double* point = points + i * point_block_size();
for (int j = 0; j < point_block_size(); ++j) {
of << point[j] << ' ';
}
of << "255 255 255\n";
}
of.close();
}
void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
double* angle_axis,
double* center) const {
VectorRef angle_axis_ref(angle_axis, 3);
if (use_quaternions_) {
QuaternionToAngleAxis(camera, angle_axis);
} else {
angle_axis_ref = ConstVectorRef(camera, 3);
}
// c = -R't
Eigen::VectorXd inverse_rotation = -angle_axis_ref;
AngleAxisRotatePoint(inverse_rotation.data(),
camera + camera_block_size() - 6,
center);
VectorRef(center, 3) *= -1.0;
}
void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
const double* center,
double* camera) const {
ConstVectorRef angle_axis_ref(angle_axis, 3);
if (use_quaternions_) {
AngleAxisToQuaternion(angle_axis, camera);
} else {
VectorRef(camera, 3) = angle_axis_ref;
}
// t = -R * c
AngleAxisRotatePoint(angle_axis,
center,
camera + camera_block_size() - 6);
VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
}
void BALProblem::Normalize() {
// Compute the marginal median of the geometry.
std::vector<double> tmp(num_points_);
Eigen::Vector3d median;
double* points = mutable_points();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < num_points_; ++j) {
tmp[j] = points[3 * j + i];
}
median(i) = Median(&tmp);
}
for (int i = 0; i < num_points_; ++i) {
VectorRef point(points + 3 * i, 3);
tmp[i] = (point - median).lpNorm<1>();
}
const double median_absolute_deviation = Median(&tmp);
// Scale so that the median absolute deviation of the resulting
// reconstruction is 100.
const double scale = 100.0 / median_absolute_deviation;
VLOG(2) << "median: " << median.transpose();
VLOG(2) << "median absolute deviation: " << median_absolute_deviation;
VLOG(2) << "scale: " << scale;
// X = scale * (X - median)
for (int i = 0; i < num_points_; ++i) {
VectorRef point(points + 3 * i, 3);
point = scale * (point - median);
}
double* cameras = mutable_cameras();
double angle_axis[3];
double center[3];
for (int i = 0; i < num_cameras_; ++i) {
double* camera = cameras + camera_block_size() * i;
CameraToAngleAxisAndCenter(camera, angle_axis, center);
// center = scale * (center - median)
VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
AngleAxisAndCenterToCamera(angle_axis, center, camera);
}
}
void BALProblem::Perturb(const double rotation_sigma,
const double translation_sigma,
const double point_sigma) {
CHECK_GE(point_sigma, 0.0);
CHECK_GE(rotation_sigma, 0.0);
CHECK_GE(translation_sigma, 0.0);
double* points = mutable_points();
if (point_sigma > 0) {
for (int i = 0; i < num_points_; ++i) {
PerturbPoint3(point_sigma, points + 3 * i);
}
}
for (int i = 0; i < num_cameras_; ++i) {
double* camera = mutable_cameras() + camera_block_size() * i;
double angle_axis[3];
double center[3];
// Perturb in the rotation of the camera in the angle-axis
// representation.
CameraToAngleAxisAndCenter(camera, angle_axis, center);
if (rotation_sigma > 0.0) {
PerturbPoint3(rotation_sigma, angle_axis);
}
AngleAxisAndCenterToCamera(angle_axis, center, camera);
if (translation_sigma > 0.0) {
PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
}
}
}
BALProblem::~BALProblem() {
delete []point_index_;
delete []camera_index_;
delete []observations_;
delete []parameters_;
}
} // namespace examples
} // namespace ceres

109
3rdparty/ceres-solver-1.11.0/examples/bal_problem.h vendored Normal file
View File

@@ -0,0 +1,109 @@
// 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)
//
// Class for loading and holding in memory bundle adjustment problems
// from the BAL (Bundle Adjustment in the Large) dataset from the
// University of Washington.
//
// For more details see http://grail.cs.washington.edu/projects/bal/
#ifndef CERES_EXAMPLES_BAL_PROBLEM_H_
#define CERES_EXAMPLES_BAL_PROBLEM_H_
#include <string>
namespace ceres {
namespace examples {
class BALProblem {
public:
explicit BALProblem(const std::string& filename, bool use_quaternions);
~BALProblem();
void WriteToFile(const std::string& filename) const;
void WriteToPLYFile(const std::string& filename) const;
// Move the "center" of the reconstruction to the origin, where the
// center is determined by computing the marginal median of the
// points. The reconstruction is then scaled so that the median
// absolute deviation of the points measured from the origin is
// 100.0.
//
// The reprojection error of the problem remains the same.
void Normalize();
// Perturb the camera pose and the geometry with random normal
// numbers with corresponding standard deviations.
void Perturb(const double rotation_sigma,
const double translation_sigma,
const double point_sigma);
int camera_block_size() const { return use_quaternions_ ? 10 : 9; }
int point_block_size() const { return 3; }
int num_cameras() const { return num_cameras_; }
int num_points() const { return num_points_; }
int num_observations() const { return num_observations_; }
int num_parameters() const { return num_parameters_; }
const int* point_index() const { return point_index_; }
const int* camera_index() const { return camera_index_; }
const double* observations() const { return observations_; }
const double* parameters() const { return parameters_; }
const double* cameras() const { return parameters_; }
double* mutable_cameras() { return parameters_; }
double* mutable_points() {
return parameters_ + camera_block_size() * num_cameras_;
}
private:
void CameraToAngleAxisAndCenter(const double* camera,
double* angle_axis,
double* center) const;
void AngleAxisAndCenterToCamera(const double* angle_axis,
const double* center,
double* camera) const;
int num_cameras_;
int num_points_;
int num_observations_;
int num_parameters_;
bool use_quaternions_;
int* point_index_;
int* camera_index_;
double* observations_;
// The parameter vector is laid out as follows
// [camera_1, ..., camera_n, point_1, ..., point_m]
double* parameters_;
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_BAL_PROBLEM_H_

360
3rdparty/ceres-solver-1.11.0/examples/bundle_adjuster.cc vendored Normal file
View File

@@ -0,0 +1,360 @@
// 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)
//
// An example of solving a dynamically sized problem with various
// solvers and loss functions.
//
// For a simpler bare bones example of doing bundle adjustment with
// Ceres, please see simple_bundle_adjuster.cc.
//
// NOTE: This example will not compile without gflags and SuiteSparse.
//
// The problem being solved here is known as a Bundle Adjustment
// problem in computer vision. Given a set of 3d points X_1, ..., X_n,
// a set of cameras P_1, ..., P_m. If the point X_i is visible in
// image j, then there is a 2D observation u_ij that is the expected
// projection of X_i using P_j. The aim of this optimization is to
// find values of X_i and P_j such that the reprojection error
//
// E(X,P) = sum_ij |u_ij - P_j X_i|^2
//
// is minimized.
//
// The problem used here comes from a collection of bundle adjustment
// problems published at University of Washington.
// http://grail.cs.washington.edu/projects/bal
#include <algorithm>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <vector>
#include "bal_problem.h"
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "snavely_reprojection_error.h"
DEFINE_string(input, "", "Input File name");
DEFINE_string(trust_region_strategy, "levenberg_marquardt",
"Options are: levenberg_marquardt, dogleg.");
DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg,"
"subspace_dogleg.");
DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly "
"refine each successful trust region step.");
DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: "
"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.");
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, "
"cluster_tridiagonal.");
DEFINE_string(visibility_clustering, "canonical_views",
"single_linkage, canonical_views");
DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
"Options are: suite_sparse and cx_sparse.");
DEFINE_string(dense_linear_algebra_library, "eigen",
"Options are: eigen and lapack.");
DEFINE_string(ordering, "automatic", "Options are: automatic, 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(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_iterations, 5, "Number of iterations.");
DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds.");
DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
" nonmonotic steps.");
DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation "
"perturbation.");
DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera "
"translation perturbation.");
DEFINE_double(point_sigma, 0.0, "Standard deviation of the point "
"perturbation.");
DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
"of the pseudo random number generator used to generate "
"the pertubations.");
DEFINE_bool(line_search, false, "Use a line search instead of trust region "
"algorithm.");
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.");
namespace ceres {
namespace examples {
void SetLinearSolver(Solver::Options* options) {
CHECK(StringToLinearSolverType(FLAGS_linear_solver,
&options->linear_solver_type));
CHECK(StringToPreconditionerType(FLAGS_preconditioner,
&options->preconditioner_type));
CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
&options->visibility_clustering_type));
CHECK(StringToSparseLinearAlgebraLibraryType(
FLAGS_sparse_linear_algebra_library,
&options->sparse_linear_algebra_library_type));
CHECK(StringToDenseLinearAlgebraLibraryType(
FLAGS_dense_linear_algebra_library,
&options->dense_linear_algebra_library_type));
options->num_linear_solver_threads = FLAGS_num_threads;
options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;
}
void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
const int num_points = bal_problem->num_points();
const int point_block_size = bal_problem->point_block_size();
double* points = bal_problem->mutable_points();
const int num_cameras = bal_problem->num_cameras();
const int camera_block_size = bal_problem->camera_block_size();
double* cameras = bal_problem->mutable_cameras();
if (options->use_inner_iterations) {
if (FLAGS_blocks_for_inner_iterations == "cameras") {
LOG(INFO) << "Camera blocks for inner iterations";
options->inner_iteration_ordering.reset(new 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") {
LOG(INFO) << "Point blocks for inner iterations";
options->inner_iteration_ordering.reset(new 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") {
LOG(INFO) << "Camera followed by point blocks for inner iterations";
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
for (int i = 0; i < num_cameras; ++i) {
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
}
for (int i = 0; i < num_points; ++i) {
options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);
}
} else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
LOG(INFO) << "Point followed by camera blocks for inner iterations";
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
for (int i = 0; i < num_cameras; ++i) {
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
}
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 == "automatic") {
LOG(INFO) << "Choosing automatic blocks for inner iterations";
} else {
LOG(FATAL) << "Unknown block type for inner iterations: "
<< FLAGS_blocks_for_inner_iterations;
}
}
// Bundle adjustment problems have a sparsity structure that makes
// them amenable to more specialized and much more efficient
// solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and
// 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;
}
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);
// If quaternions are used, there are two blocks, so add the
// second block to the ordering.
if (FLAGS_use_quaternions) {
ordering->AddElementToGroup(cameras + camera_block_size * i + 4, 1);
}
}
options->linear_solver_ordering.reset(ordering);
}
void SetMinimizerOptions(Solver::Options* options) {
options->max_num_iterations = 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) {
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;
}
void SetSolverOptionsFromFlags(BALProblem* bal_problem,
Solver::Options* options) {
SetMinimizerOptions(options);
SetLinearSolver(options);
SetOrdering(bal_problem, options);
}
void BuildProblem(BALProblem* bal_problem, Problem* problem) {
const int point_block_size = bal_problem->point_block_size();
const int camera_block_size = bal_problem->camera_block_size();
double* points = bal_problem->mutable_points();
double* cameras = bal_problem->mutable_cameras();
// Observations is 2*num_observations long array observations =
// [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
// and y positions of the observation.
const double* observations = bal_problem->observations();
for (int i = 0; i < bal_problem->num_observations(); ++i) {
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)
? 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;
// Each observation correponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
double* camera =
cameras + camera_block_size * bal_problem->camera_index()[i];
double* point = points + point_block_size * bal_problem->point_index()[i];
if (FLAGS_use_quaternions) {
// When using quaternions, we split the camera into two
// parameter blocks. One of size 4 for the quaternion and the
// other of size 6 containing the translation, focal length and
// the radial distortion parameters.
problem->AddResidualBlock(cost_function,
loss_function,
camera,
camera + 4,
point);
} else {
problem->AddResidualBlock(cost_function, loss_function, camera, point);
}
}
if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
LocalParameterization* quaternion_parameterization =
new QuaternionParameterization;
for (int i = 0; i < bal_problem->num_cameras(); ++i) {
problem->SetParameterization(cameras + camera_block_size * i,
quaternion_parameterization);
}
}
}
void SolveProblem(const char* filename) {
BALProblem bal_problem(filename, FLAGS_use_quaternions);
if (!FLAGS_initial_ply.empty()) {
bal_problem.WriteToPLYFile(FLAGS_initial_ply);
}
Problem problem;
srand(FLAGS_random_seed);
bal_problem.Normalize();
bal_problem.Perturb(FLAGS_rotation_sigma,
FLAGS_translation_sigma,
FLAGS_point_sigma);
BuildProblem(&bal_problem, &problem);
Solver::Options options;
SetSolverOptionsFromFlags(&bal_problem, &options);
options.gradient_tolerance = 1e-16;
options.function_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);
}
}
} // namespace examples
} // namespace ceres
int main(int argc, char** argv) {
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
if (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());
return 0;
}

164
3rdparty/ceres-solver-1.11.0/examples/circle_fit.cc vendored Normal file
View File

@@ -0,0 +1,164 @@
// 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 fits circles to a collection of points, where the error is related to
// the distance of a point from the circle. This uses auto-differentiation to
// take the derivatives.
//
// The input format is simple text. Feed on standard in:
//
// x_initial y_initial r_initial
// x1 y1
// x2 y2
// y3 y3
// ...
//
// And the result after solving will be printed to stdout:
//
// x y r
//
// There are closed form solutions [1] to this problem which you may want to
// consider instead of using this one. If you already have a decent guess, Ceres
// can squeeze down the last bit of error.
//
// [1] http://www.mathworks.com/matlabcentral/fileexchange/5557-circle-fit/content/circfit.m
#include <cstdio>
#include <vector>
#include "ceres/ceres.h"
#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 robustification).");
// The cost for a single sample. The returned residual is related to the
// distance of the point from the circle (passed in as x, y, m parameters).
//
// Note that the radius is parameterized as r = m^2 to constrain the radius to
// positive values.
class DistanceFromCircleCost {
public:
DistanceFromCircleCost(double xx, double yy) : xx_(xx), yy_(yy) {}
template <typename T> bool operator()(const T* const x,
const T* const y,
const T* const m, // r = m^2
T* residual) const {
// Since the radius is parameterized as m^2, unpack m to get r.
T r = *m * *m;
// Get the position of the sample in the circle's coordinate system.
T xp = xx_ - *x;
T yp = yy_ - *y;
// It is tempting to use the following cost:
//
// residual[0] = r - sqrt(xp*xp + yp*yp);
//
// which is the distance of the sample from the circle. This works
// reasonably well, but the sqrt() adds strong nonlinearities to the cost
// function. Instead, a different cost is used, which while not strictly a
// distance in the metric sense (it has units distance^2) it produces more
// robust fits when there are outliers. This is because the cost surface is
// more convex.
residual[0] = r*r - xp*xp - yp*yp;
return true;
}
private:
// The measured x,y coordinate that should be on the circle.
double xx_, yy_;
};
int main(int argc, char** argv) {
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
double x, y, r;
if (scanf("%lg %lg %lg", &x, &y, &r) != 3) {
fprintf(stderr, "Couldn't read first line.\n");
return 1;
}
fprintf(stderr, "Got x, y, r %lg, %lg, %lg\n", x, y, r);
// Save initial values for comparison.
double initial_x = x;
double initial_y = y;
double initial_r = r;
// Parameterize r as m^2 so that it can't be negative.
double m = sqrt(r);
Problem problem;
// Configure the loss function.
LossFunction* loss = NULL;
if (FLAGS_robust_threshold) {
loss = new CauchyLoss(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));
problem.AddResidualBlock(cost, loss, &x, &y, &m);
num_points++;
}
std::cout << "Got " << num_points << " points.\n";
// Build and solve the problem.
Solver::Options options;
options.max_num_iterations = 500;
options.linear_solver_type = ceres::DENSE_QR;
Solver::Summary summary;
Solve(options, &problem, &summary);
// Recover r from m.
r = m * m;
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x << " -> " << x << "\n";
std::cout << "y : " << initial_y << " -> " << y << "\n";
std::cout << "r : " << initial_r << " -> " << r << "\n";
return 0;
}

187
3rdparty/ceres-solver-1.11.0/examples/curve_fitting.c vendored Normal file
View File

@@ -0,0 +1,187 @@
/* 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: mierle@gmail.com (Keir Mierle)
*
* This is a port of curve_fitting.cc to the minimal C API for Ceres.
*/
#include <math.h>
#include <stdio.h>
#include <string.h> // For NULL
#include "ceres/c_api.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'];
*
*/
int num_observations = 67;
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,
};
/* This is the equivalent of a use-defined CostFunction in the C++ Ceres API.
* This is passed as a callback to the Ceres C API, which internally converts
* the callback into a CostFunction. */
int exponential_residual(void* user_data,
double** parameters,
double* residuals,
double** jacobians) {
double* measurement = (double*) user_data;
double x = measurement[0];
double y = measurement[1];
double m = parameters[0][0];
double c = parameters[1][0];
residuals[0] = y - exp(m * x + c);
if (jacobians == NULL) {
return 1;
}
if (jacobians[0] != NULL) {
jacobians[0][0] = - x * exp(m * x + c); /* dr/dm */
}
if (jacobians[1] != NULL) {
jacobians[1][0] = - exp(m * x + c); /* dr/dc */
}
return 1;
}
int main(int argc, char** argv) {
/* Note: Typically it is better to compact m and c into one block,
* but in this case use separate blocks to illustrate the use of
* multiple parameter blocks. */
double m = 0.0;
double c = 0.0;
double *parameter_pointers[] = { &m, &c };
int parameter_sizes[] = { 1, 1 };
int i;
ceres_problem_t* problem;
/* Ceres has some internal stuff that needs to get initialized. */
ceres_init();
problem = ceres_create_problem();
/* Add all the residuals. */
for (i = 0; i < num_observations; ++i) {
ceres_problem_add_residual_block(
problem,
exponential_residual, /* Cost function */
&data[2 * i], /* Points to the (x,y) measurement */
NULL, /* No loss function */
NULL, /* No loss function user data */
1, /* Number of residuals */
2, /* Number of parameter blocks */
parameter_sizes,
parameter_pointers);
}
ceres_solve(problem);
ceres_free_problem(problem);
printf("Initial m: 0.0, c: 0.0\n");
printf("Final m: %g, c: %g\n", m, c);
return 0;
}

163
3rdparty/ceres-solver-1.11.0/examples/curve_fitting.cc vendored Normal file
View File

@@ -0,0 +1,163 @@
// 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)
#include "ceres/ceres.h"
#include "glog/logging.h"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
// 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;
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,
};
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] = T(y_) - exp(m[0] * T(x_) + c[0]);
return true;
}
private:
const double x_;
const double y_;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
double m = 0.0;
double c = 0.0;
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,
&m, &c);
}
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);
std::cout << summary.BriefReport() << "\n";
std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n";
std::cout << "Final m: " << m << " c: " << c << "\n";
return 0;
}

220
3rdparty/ceres-solver-1.11.0/examples/denoising.cc vendored Normal file
View File

@@ -0,0 +1,220 @@
// 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: strandmark@google.com (Petter Strandmark)
//
// Denoising using Fields of Experts and the Ceres minimizer.
//
// 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.
//
// [1] S. Roth and M.J. Black. "Fields of Experts." International Journal of
// Computer Vision, 82(2):205--229, 2009.
#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>
#include <sstream>
#include <string>
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "fields_of_experts.h"
#include "pgm_image.h"
DEFINE_string(input, "", "File to which the output image should be written");
DEFINE_string(foe_file, "", "FoE file to use");
DEFINE_string(output, "", "File to which the output image should be written");
DEFINE_double(sigma, 20.0, "Standard deviation of noise");
DEFINE_bool(verbose, false, "Prints information about the solver progress.");
DEFINE_bool(line_search, false, "Use a line search instead of trust region "
"algorithm.");
namespace ceres {
namespace examples {
// This cost function is used to build the data term.
//
// f_i(x) = a * (x_i - b)^2
//
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 {
const double x = parameters[0][0];
residuals[0] = sqrta_ * (x - b_);
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = sqrta_;
}
return true;
}
private:
double sqrta_, b_;
};
// Creates a Fields of Experts MAP inference problem.
void CreateProblem(const FieldsOfExperts& foe,
const PGMImage<double>& image,
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);
for (unsigned index = 0; index < image.NumPixels(); ++index) {
ceres::CostFunction* cost_function =
new QuadraticCostFunction(coefficient,
image.PixelFromLinearIndex(index));
problem->AddResidualBlock(cost_function,
NULL,
solution->MutablePixelFromLinearIndex(index));
}
// Create Ceres cost and loss functions for regularization. One is needed for
// each filter.
std::vector<ceres::LossFunction*> loss_function(foe.NumFilters());
std::vector<ceres::CostFunction*> cost_function(foe.NumFilters());
for (int alpha_index = 0; alpha_index < foe.NumFilters(); ++alpha_index) {
loss_function[alpha_index] = foe.NewLossFunction(alpha_index);
cost_function[alpha_index] = foe.NewCostFunction(alpha_index);
}
// Add FoE regularization for each patch in the image.
for (int x = 0; x < image.width() - (foe.Size() - 1); ++x) {
for (int y = 0; y < image.height() - (foe.Size() - 1); ++y) {
// Build a vector with the pixel indices of this patch.
std::vector<double*> pixels;
const std::vector<int>& x_delta_indices = foe.GetXDeltaIndices();
const std::vector<int>& y_delta_indices = foe.GetYDeltaIndices();
for (int i = 0; i < foe.NumVariables(); ++i) {
double* pixel = solution->MutablePixel(x + x_delta_indices[i],
y + y_delta_indices[i]);
pixels.push_back(pixel);
}
// For this patch with coordinates (x, y), we will add foe.NumFilters()
// terms to the objective function.
for (int alpha_index = 0; alpha_index < foe.NumFilters(); ++alpha_index) {
problem->AddResidualBlock(cost_function[alpha_index],
loss_function[alpha_index],
pixels);
}
}
}
}
// Solves the FoE problem using Ceres and post-processes it to make sure the
// solution stays within [0, 255].
void SolveProblem(Problem* problem, PGMImage<double>* solution) {
// These parameters may be experimented with. For example, ceres::DOGLEG tends
// to be faster for 2x2 filters, but gives solutions with slightly higher
// objective function value.
ceres::Solver::Options options;
options.max_num_iterations = 100;
if (FLAGS_verbose) {
options.minimizer_progress_to_stdout = true;
}
if (FLAGS_line_search) {
options.minimizer_type = ceres::LINE_SEARCH;
}
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.function_tolerance = 1e-3; // Enough for denoising.
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
if (FLAGS_verbose) {
std::cout << summary.FullReport() << "\n";
}
// Make the solution stay in [0, 255].
for (int x = 0; x < solution->width(); ++x) {
for (int y = 0; y < solution->height(); ++y) {
*solution->MutablePixel(x, y) =
std::min(255.0, std::max(0.0, solution->Pixel(x, y)));
}
}
}
} // namespace examples
} // namespace ceres
int main(int argc, char** argv) {
using namespace ceres::examples;
std::string
usage("This program denoises an image using Ceres. Sample usage:\n");
usage += argv[0];
usage += " --input=<noisy image PGM file> --foe_file=<FoE file name>";
CERES_GFLAGS_NAMESPACE::SetUsageMessage(usage);
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
if (FLAGS_input.empty()) {
std::cerr << "Please provide an image file name.\n";
return 1;
}
if (FLAGS_foe_file.empty()) {
std::cerr << "Please provide a Fields of Experts file name.\n";
return 1;
}
// Load the Fields of Experts filters from file.
FieldsOfExperts foe;
if (!foe.LoadFromFile(FLAGS_foe_file)) {
std::cerr << "Loading \"" << FLAGS_foe_file << "\" failed.\n";
return 2;
}
// Read the images
PGMImage<double> image(FLAGS_input);
if (image.width() == 0) {
std::cerr << "Reading \"" << FLAGS_input << "\" failed.\n";
return 3;
}
PGMImage<double> solution(image.width(), image.height());
solution.Set(0.0);
ceres::Problem problem;
CreateProblem(foe, image, &problem, &solution);
SolveProblem(&problem, &solution);
if (!FLAGS_output.empty()) {
CHECK(solution.WriteToFile(FLAGS_output))
<< "Writing \"" << FLAGS_output << "\" failed.";
}
return 0;
}

451
3rdparty/ceres-solver-1.11.0/examples/ellipse_approximation.cc vendored Normal file
View File

@@ -0,0 +1,451 @@
// 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: richie.stebbing@gmail.com (Richard Stebbing)
//
// This fits points randomly distributed on an ellipse with an approximate
// line segment contour. This is done by jointly optimizing the control points
// of the line segment contour along with the preimage positions for the data
// points. The purpose of this example is to show an example use case for
// dynamic_sparsity, and how it can benefit problems which are numerically
// dense but dynamically sparse.
#include <cmath>
#include <vector>
#include "ceres/ceres.h"
#include "glog/logging.h"
// Data generated with the following Python code.
// import numpy as np
// np.random.seed(1337)
// t = np.linspace(0.0, 2.0 * np.pi, 212, endpoint=False)
// t += 2.0 * np.pi * 0.01 * np.random.randn(t.size)
// theta = np.deg2rad(15)
// a, b = np.cos(theta), np.sin(theta)
// R = np.array([[a, -b],
// [b, a]])
// Y = np.dot(np.c_[4.0 * np.cos(t), np.sin(t)], R.T)
const int kYRows = 212;
const int kYCols = 2;
const double kYData[kYRows * kYCols] = {
+3.871364e+00, +9.916027e-01,
+3.864003e+00, +1.034148e+00,
+3.850651e+00, +1.072202e+00,
+3.868350e+00, +1.014408e+00,
+3.796381e+00, +1.153021e+00,
+3.857138e+00, +1.056102e+00,
+3.787532e+00, +1.162215e+00,
+3.704477e+00, +1.227272e+00,
+3.564711e+00, +1.294959e+00,
+3.754363e+00, +1.191948e+00,
+3.482098e+00, +1.322725e+00,
+3.602777e+00, +1.279658e+00,
+3.585433e+00, +1.286858e+00,
+3.347505e+00, +1.356415e+00,
+3.220855e+00, +1.378914e+00,
+3.558808e+00, +1.297174e+00,
+3.403618e+00, +1.343809e+00,
+3.179828e+00, +1.384721e+00,
+3.054789e+00, +1.398759e+00,
+3.294153e+00, +1.366808e+00,
+3.247312e+00, +1.374813e+00,
+2.988547e+00, +1.404247e+00,
+3.114508e+00, +1.392698e+00,
+2.899226e+00, +1.409802e+00,
+2.533256e+00, +1.414778e+00,
+2.654773e+00, +1.415909e+00,
+2.565100e+00, +1.415313e+00,
+2.976456e+00, +1.405118e+00,
+2.484200e+00, +1.413640e+00,
+2.324751e+00, +1.407476e+00,
+1.930468e+00, +1.378221e+00,
+2.329017e+00, +1.407688e+00,
+1.760640e+00, +1.360319e+00,
+2.147375e+00, +1.396603e+00,
+1.741989e+00, +1.358178e+00,
+1.743859e+00, +1.358394e+00,
+1.557372e+00, +1.335208e+00,
+1.280551e+00, +1.295087e+00,
+1.429880e+00, +1.317546e+00,
+1.213485e+00, +1.284400e+00,
+9.168172e-01, +1.232870e+00,
+1.311141e+00, +1.299839e+00,
+1.231969e+00, +1.287382e+00,
+7.453773e-01, +1.200049e+00,
+6.151587e-01, +1.173683e+00,
+5.935666e-01, +1.169193e+00,
+2.538707e-01, +1.094227e+00,
+6.806136e-01, +1.187089e+00,
+2.805447e-01, +1.100405e+00,
+6.184807e-01, +1.174371e+00,
+1.170550e-01, +1.061762e+00,
+2.890507e-01, +1.102365e+00,
+3.834234e-01, +1.123772e+00,
+3.980161e-04, +1.033061e+00,
-3.651680e-01, +9.370367e-01,
-8.386351e-01, +7.987201e-01,
-8.105704e-01, +8.073702e-01,
-8.735139e-01, +7.878886e-01,
-9.913836e-01, +7.506100e-01,
-8.784011e-01, +7.863636e-01,
-1.181440e+00, +6.882566e-01,
-1.229556e+00, +6.720191e-01,
-1.035839e+00, +7.362765e-01,
-8.031520e-01, +8.096470e-01,
-1.539136e+00, +5.629549e-01,
-1.755423e+00, +4.817306e-01,
-1.337589e+00, +6.348763e-01,
-1.836966e+00, +4.499485e-01,
-1.913367e+00, +4.195617e-01,
-2.126467e+00, +3.314900e-01,
-1.927625e+00, +4.138238e-01,
-2.339862e+00, +2.379074e-01,
-1.881736e+00, +4.322152e-01,
-2.116753e+00, +3.356163e-01,
-2.255733e+00, +2.754930e-01,
-2.555834e+00, +1.368473e-01,
-2.770277e+00, +2.895711e-02,
-2.563376e+00, +1.331890e-01,
-2.826715e+00, -9.000818e-04,
-2.978191e+00, -8.457804e-02,
-3.115855e+00, -1.658786e-01,
-2.982049e+00, -8.678322e-02,
-3.307892e+00, -2.902083e-01,
-3.038346e+00, -1.194222e-01,
-3.190057e+00, -2.122060e-01,
-3.279086e+00, -2.705777e-01,
-3.322028e+00, -2.999889e-01,
-3.122576e+00, -1.699965e-01,
-3.551973e+00, -4.768674e-01,
-3.581866e+00, -5.032175e-01,
-3.497799e+00, -4.315203e-01,
-3.565384e+00, -4.885602e-01,
-3.699493e+00, -6.199815e-01,
-3.585166e+00, -5.061925e-01,
-3.758914e+00, -6.918275e-01,
-3.741104e+00, -6.689131e-01,
-3.688331e+00, -6.077239e-01,
-3.810425e+00, -7.689015e-01,
-3.791829e+00, -7.386911e-01,
-3.789951e+00, -7.358189e-01,
-3.823100e+00, -7.918398e-01,
-3.857021e+00, -8.727074e-01,
-3.858250e+00, -8.767645e-01,
-3.872100e+00, -9.563174e-01,
-3.864397e+00, -1.032630e+00,
-3.846230e+00, -1.081669e+00,
-3.834799e+00, -1.102536e+00,
-3.866684e+00, -1.022901e+00,
-3.808643e+00, -1.139084e+00,
-3.868840e+00, -1.011569e+00,
-3.791071e+00, -1.158615e+00,
-3.797999e+00, -1.151267e+00,
-3.696278e+00, -1.232314e+00,
-3.779007e+00, -1.170504e+00,
-3.622855e+00, -1.270793e+00,
-3.647249e+00, -1.259166e+00,
-3.655412e+00, -1.255042e+00,
-3.573218e+00, -1.291696e+00,
-3.638019e+00, -1.263684e+00,
-3.498409e+00, -1.317750e+00,
-3.304143e+00, -1.364970e+00,
-3.183001e+00, -1.384295e+00,
-3.202456e+00, -1.381599e+00,
-3.244063e+00, -1.375332e+00,
-3.233308e+00, -1.377019e+00,
-3.060112e+00, -1.398264e+00,
-3.078187e+00, -1.396517e+00,
-2.689594e+00, -1.415761e+00,
-2.947662e+00, -1.407039e+00,
-2.854490e+00, -1.411860e+00,
-2.660499e+00, -1.415900e+00,
-2.875955e+00, -1.410930e+00,
-2.675385e+00, -1.415848e+00,
-2.813155e+00, -1.413363e+00,
-2.417673e+00, -1.411512e+00,
-2.725461e+00, -1.415373e+00,
-2.148334e+00, -1.396672e+00,
-2.108972e+00, -1.393738e+00,
-2.029905e+00, -1.387302e+00,
-2.046214e+00, -1.388687e+00,
-2.057402e+00, -1.389621e+00,
-1.650250e+00, -1.347160e+00,
-1.806764e+00, -1.365469e+00,
-1.206973e+00, -1.283343e+00,
-8.029259e-01, -1.211308e+00,
-1.229551e+00, -1.286993e+00,
-1.101507e+00, -1.265754e+00,
-9.110645e-01, -1.231804e+00,
-1.110046e+00, -1.267211e+00,
-8.465274e-01, -1.219677e+00,
-7.594163e-01, -1.202818e+00,
-8.023823e-01, -1.211203e+00,
-3.732519e-01, -1.121494e+00,
-1.918373e-01, -1.079668e+00,
-4.671988e-01, -1.142253e+00,
-4.033645e-01, -1.128215e+00,
-1.920740e-01, -1.079724e+00,
-3.022157e-01, -1.105389e+00,
-1.652831e-01, -1.073354e+00,
+4.671625e-01, -9.085886e-01,
+5.940178e-01, -8.721832e-01,
+3.147557e-01, -9.508290e-01,
+6.383631e-01, -8.591867e-01,
+9.888923e-01, -7.514088e-01,
+7.076339e-01, -8.386023e-01,
+1.326682e+00, -6.386698e-01,
+1.149834e+00, -6.988221e-01,
+1.257742e+00, -6.624207e-01,
+1.492352e+00, -5.799632e-01,
+1.595574e+00, -5.421766e-01,
+1.240173e+00, -6.684113e-01,
+1.706612e+00, -5.004442e-01,
+1.873984e+00, -4.353002e-01,
+1.985633e+00, -3.902561e-01,
+1.722880e+00, -4.942329e-01,
+2.095182e+00, -3.447402e-01,
+2.018118e+00, -3.768991e-01,
+2.422702e+00, -1.999563e-01,
+2.370611e+00, -2.239326e-01,
+2.152154e+00, -3.205250e-01,
+2.525121e+00, -1.516499e-01,
+2.422116e+00, -2.002280e-01,
+2.842806e+00, +9.536372e-03,
+3.030128e+00, +1.146027e-01,
+2.888424e+00, +3.433444e-02,
+2.991609e+00, +9.226409e-02,
+2.924807e+00, +5.445844e-02,
+3.007772e+00, +1.015875e-01,
+2.781973e+00, -2.282382e-02,
+3.164737e+00, +1.961781e-01,
+3.237671e+00, +2.430139e-01,
+3.046123e+00, +1.240014e-01,
+3.414834e+00, +3.669060e-01,
+3.436591e+00, +3.833600e-01,
+3.626207e+00, +5.444311e-01,
+3.223325e+00, +2.336361e-01,
+3.511963e+00, +4.431060e-01,
+3.698380e+00, +6.187442e-01,
+3.670244e+00, +5.884943e-01,
+3.558833e+00, +4.828230e-01,
+3.661807e+00, +5.797689e-01,
+3.767261e+00, +7.030893e-01,
+3.801065e+00, +7.532650e-01,
+3.828523e+00, +8.024454e-01,
+3.840719e+00, +8.287032e-01,
+3.848748e+00, +8.485921e-01,
+3.865801e+00, +9.066551e-01,
+3.870983e+00, +9.404873e-01,
+3.870263e+00, +1.001884e+00,
+3.864462e+00, +1.032374e+00,
+3.870542e+00, +9.996121e-01,
+3.865424e+00, +1.028474e+00
};
ceres::ConstMatrixRef kY(kYData, kYRows, kYCols);
class PointToLineSegmentContourCostFunction : public ceres::CostFunction {
public:
PointToLineSegmentContourCostFunction(const int num_segments,
const Eigen::Vector2d y)
: num_segments_(num_segments), y_(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.
for (int i = 0; i < num_segments_; ++i) {
mutable_parameter_block_sizes()->push_back(2);
}
set_num_residuals(2);
}
virtual bool Evaluate(const double* const* x,
double* residuals,
double** jacobians) const {
// 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.
const double t = ModuloNumSegments(*x[0]);
CHECK_GE(t, 0.0);
CHECK_LT(t, num_segments_);
const int i0 = floor(t), i1 = (i0 + 1) % num_segments_;
const double u = t - i0;
// Linearly interpolate between control points `i0` and `i1`.
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) {
return true;
}
if (jacobians[0] != NULL) {
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) {
ceres::MatrixRef(jacobians[i + 1], 2, 2).setZero();
if (i == i0) {
jacobians[i + 1][0] = -(1.0 - u);
jacobians[i + 1][3] = -(1.0 - u);
} else if (i == i1) {
jacobians[i + 1][0] = -u;
jacobians[i + 1][3] = -u;
}
}
}
return true;
}
static ceres::CostFunction* Create(const int num_segments,
const Eigen::Vector2d y) {
return new PointToLineSegmentContourCostFunction(num_segments, y);
}
private:
inline double ModuloNumSegments(const double& t) const {
return t - num_segments_ * floor(t / num_segments_);
}
const int num_segments_;
const Eigen::Vector2d y_;
};
struct EuclideanDistanceFunctor {
EuclideanDistanceFunctor(const double& sqrt_weight)
: sqrt_weight_(sqrt_weight) {}
template <typename T>
bool operator()(const T* x0, const T* x1, T* residuals) const {
residuals[0] = T(sqrt_weight_) * (x0[0] - x1[0]);
residuals[1] = T(sqrt_weight_) * (x0[1] - x1[1]);
return true;
}
static ceres::CostFunction* Create(const double& sqrt_weight) {
return new ceres::AutoDiffCostFunction<EuclideanDistanceFunctor, 2, 2, 2>(
new EuclideanDistanceFunctor(sqrt_weight));
}
private:
const double sqrt_weight_;
};
bool SolveWithFullReport(ceres::Solver::Options options,
ceres::Problem* problem,
bool dynamic_sparsity) {
options.dynamic_sparsity = dynamic_sparsity;
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
std::cout << "####################" << std::endl;
std::cout << "dynamic_sparsity = " << dynamic_sparsity << std::endl;
std::cout << "####################" << std::endl;
std::cout << summary.FullReport() << std::endl;
return summary.termination_type == ceres::CONVERGENCE;
}
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// Problem configuration.
const int num_segments = 151;
const double regularization_weight = 1e-2;
// 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 Eigen::VectorXd;
// `X` is the matrix of control points which make up the contour of line
// segments. The number of control points is equal to the number of line
// segments because the contour is closed.
//
// 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.conservativeResize(num_segments);
MatrixXd X(num_segments, 2);
X.col(0) = w.array().cos();
X.col(1) = w.array().sin();
// 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();
VectorXd t(num_observations);
for (int i = 0; i < num_observations; ++i) {
(X.rowwise() - kY.row(i)).rowwise().squaredNorm().minCoeff(&t[i]);
}
ceres::Problem problem;
// 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;
for (int i = 0; i < num_segments; ++i) {
parameter_blocks[i + 1] = X.data() + 2 * i;
}
for (int i = 0; i < num_observations; ++i) {
parameter_blocks[0] = &t[i];
problem.AddResidualBlock(
PointToLineSegmentContourCostFunction::Create(num_segments, kY.row(i)),
NULL,
parameter_blocks);
}
// Add regularization to minimize the length of the line segment contour.
for (int i = 0; i < num_segments; ++i) {
problem.AddResidualBlock(
EuclideanDistanceFunctor::Create(sqrt(regularization_weight)),
NULL,
X.data() + 2 * i,
X.data() + 2 * ((i + 1) % num_segments));
}
ceres::Solver::Options options;
options.max_num_iterations = 100;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
// First, solve `X` and `t` jointly with dynamic_sparsity = true.
MatrixXd X0 = X;
VectorXd t0 = t;
CHECK(SolveWithFullReport(options, &problem, true));
// Second, solve with dynamic_sparsity = false.
X = X0;
t = t0;
CHECK(SolveWithFullReport(options, &problem, false));
return 0;
}

152
3rdparty/ceres-solver-1.11.0/examples/fields_of_experts.cc vendored Normal file
View File

@@ -0,0 +1,152 @@
// 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: strandmark@google.com (Petter Strandmark)
//
// Class for loading the data required for descibing a Fields of Experts (FoE)
// model.
#include "fields_of_experts.h"
#include <fstream>
#include <cmath>
#include "pgm_image.h"
namespace ceres {
namespace examples {
FieldsOfExpertsCost::FieldsOfExpertsCost(const std::vector<double>& filter)
: filter_(filter) {
set_num_residuals(1);
for (int i = 0; i < filter_.size(); ++i) {
mutable_parameter_block_sizes()->push_back(1);
}
}
// This is a dot product between a the scalar parameters and a vector of filter
// coefficients.
bool FieldsOfExpertsCost::Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
int num_variables = filter_.size();
residuals[0] = 0;
for (int 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) {
jacobians[i][0] = filter_[i];
}
}
}
return true;
}
// This loss function builds the FoE terms and is equal to
//
// f(x) = alpha_i * log(1 + (1/2)s)
//
void FieldsOfExpertsLoss::Evaluate(double sq_norm, double rho[3]) const {
const double c = 0.5;
const double sum = 1.0 + sq_norm * c;
const double inv = 1.0 / sum;
// 'sum' and 'inv' are always positive, assuming that 's' is.
rho[0] = alpha_ * log(sum);
rho[1] = alpha_ * c * inv;
rho[2] = - alpha_ * c * c * inv * inv;
}
FieldsOfExperts::FieldsOfExperts()
: size_(0), num_filters_(0) {
}
bool FieldsOfExperts::LoadFromFile(const std::string& filename) {
std::ifstream foe_file(filename.c_str());
foe_file >> size_;
foe_file >> num_filters_;
if (size_ < 0 || num_filters_ < 0) {
return false;
}
const int num_variables = NumVariables();
x_delta_indices_.resize(num_variables);
for (int i = 0; i < num_variables; ++i) {
foe_file >> x_delta_indices_[i];
}
y_delta_indices_.resize(NumVariables());
for (int i = 0; i < num_variables; ++i) {
foe_file >> y_delta_indices_[i];
}
alpha_.resize(num_filters_);
for (int i = 0; i < num_filters_; ++i) {
foe_file >> alpha_[i];
}
filters_.resize(num_filters_);
for (int i = 0; i < num_filters_; ++i) {
filters_[i].resize(num_variables);
for (int j = 0; j < num_variables; ++j) {
foe_file >> filters_[i][j];
}
}
// If any read failed, return failure.
if (!foe_file) {
size_ = 0;
return false;
}
// There cannot be anything else in the file. Try reading another number and
// return failure if that succeeded.
double temp;
foe_file >> temp;
if (foe_file) {
size_ = 0;
return false;
}
return true;
}
ceres::CostFunction* FieldsOfExperts::NewCostFunction(int alpha_index) const {
return new FieldsOfExpertsCost(filters_[alpha_index]);
}
ceres::LossFunction* FieldsOfExperts::NewLossFunction(int alpha_index) const {
return new FieldsOfExpertsLoss(alpha_[alpha_index]);
}
} // namespace examples
} // namespace ceres

145
3rdparty/ceres-solver-1.11.0/examples/fields_of_experts.h vendored Normal file
View File

@@ -0,0 +1,145 @@
// 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: strandmark@google.com (Petter Strandmark)
//
// Class for loading the data required for descibing 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),
//
// where F is a d-by-d image patch and alpha is a constant. This is implemented
// by a FieldsOfExpertsSum object which represents the dot product between the
// image patches and a FieldsOfExpertsLoss which implements the log(1 + (1/2)s)
// part.
//
// [1] S. Roth and M.J. Black. "Fields of Experts." International Journal of
// Computer Vision, 82(2):205--229, 2009.
#ifndef CERES_EXAMPLES_FIELDS_OF_EXPERTS_H_
#define CERES_EXAMPLES_FIELDS_OF_EXPERTS_H_
#include <iostream>
#include <vector>
#include "ceres/loss_function.h"
#include "ceres/cost_function.h"
#include "ceres/sized_cost_function.h"
#include "pgm_image.h"
namespace ceres {
namespace 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
// coefficients given in the constructor and the scalar parameters passed to it.
class FieldsOfExpertsCost : public ceres::CostFunction {
public:
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;
private:
const std::vector<double>& filter_;
};
// The loss function used to build the correct regularization. See above.
//
// f(x) = alpha_i * log(1 + (1/2)s)
//
class FieldsOfExpertsLoss : public ceres::LossFunction {
public:
explicit FieldsOfExpertsLoss(double alpha) : alpha_(alpha) { }
virtual void Evaluate(double, double*) const;
private:
const double alpha_;
};
// This class loads a set of filters and coefficients from file. Then the users
// obtains the correct loss and cost functions through NewCostFunction and
// NewLossFunction.
class FieldsOfExperts {
public:
// Creates an empty object with size() == 0.
FieldsOfExperts();
// Attempts to load filters from a file. If unsuccessful it returns false and
// sets size() == 0.
bool LoadFromFile(const std::string& filename);
// Side length of a square filter in this FoE. They are all of the same size.
int Size() const {
return size_;
}
// Total number of pixels the filter covers.
int NumVariables() const {
return size_ * size_;
}
// Number of filters used by the FoE.
int NumFilters() const {
return num_filters_;
}
// Creates a new cost function. The caller is responsible for deallocating the
// memory. alpha_index specifies which filter is used in the cost function.
ceres::CostFunction* NewCostFunction(int alpha_index) const;
// Creates a new loss function. The caller is responsible for deallocating the
// memory. alpha_index specifies which filter this loss function is for.
ceres::LossFunction* NewLossFunction(int alpha_index) const;
// Gets the delta pixel indices for all pixels in a patch.
const std::vector<int>& GetXDeltaIndices() const {
return x_delta_indices_;
}
const std::vector<int>& GetYDeltaIndices() const {
return y_delta_indices_;
}
private:
// The side length of a square filter.
int size_;
// The number of different filters used.
int num_filters_;
// Pixel offsets for all variables.
std::vector<int> x_delta_indices_, y_delta_indices_;
// The coefficients in front of each term.
std::vector<double> alpha_;
// The filters used for the dot product with image patches.
std::vector<std::vector<double> > filters_;
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_FIELDS_OF_EXPERTS_H_

83
3rdparty/ceres-solver-1.11.0/examples/helloworld.cc vendored Normal file
View File

@@ -0,0 +1,83 @@
// 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)
//
// A simple example of using the Ceres minimizer.
//
// Minimize 0.5 (10 - x)^2 using jacobian matrix computed using
// automatic differentiation.
#include "ceres/ceres.h"
#include "glog/logging.h"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
// 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
// derivatives.
struct CostFunctor {
template <typename T> bool operator()(const T* const x, T* residual) const {
residual[0] = T(10.0) - x[0];
return true;
}
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// The variable to solve for with its initial value. It will be
// mutated in place by the solver.
double x = 0.5;
const double initial_x = x;
// Build the problem.
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);
problem.AddResidualBlock(cost_function, NULL, &x);
// Run the solver!
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}

107
3rdparty/ceres-solver-1.11.0/examples/helloworld_analytic_diff.cc vendored Normal file
View File

@@ -0,0 +1,107 @@
// 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)
//
// A simple example of using the Ceres minimizer.
//
// Minimize 0.5 (10 - x)^2 using analytic jacobian matrix.
#include <vector>
#include "ceres/ceres.h"
#include "glog/logging.h"
using ceres::CostFunction;
using ceres::SizedCostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
// 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:
virtual ~QuadraticCostFunction() {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
double x = parameters[0][0];
// f(x) = 10 - x.
residuals[0] = 10 - x;
// f'(x) = -1. Since there's only 1 parameter and that parameter
// has 1 dimension, there is only 1 element to fill in the
// jacobians.
//
// Since the Evaluate function can be called with the jacobians
// pointer equal to NULL, 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
// 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) {
jacobians[0][0] = -1;
}
return true;
}
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// The variable to solve for with its initial value. It will be
// mutated in place by the solver.
double x = 0.5;
const double initial_x = x;
// Build the problem.
Problem problem;
// Set up the only cost function (also known as residual).
CostFunction* cost_function = new QuadraticCostFunction;
problem.AddResidualBlock(cost_function, NULL, &x);
// Run the solver!
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}

79
3rdparty/ceres-solver-1.11.0/examples/helloworld_numeric_diff.cc vendored Normal file
View File

@@ -0,0 +1,79 @@
// 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)
//
// Minimize 0.5 (10 - x)^2 using jacobian matrix computed using
// numeric differentiation.
#include "ceres/ceres.h"
#include "glog/logging.h"
using ceres::NumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
// A cost functor that implements the residual r = 10 - x.
struct CostFunctor {
bool operator()(const double* const x, double* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// The variable to solve for with its initial value. It will be
// mutated in place by the solver.
double x = 0.5;
const double initial_x = x;
// Build the problem.
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);
// Run the solver!
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}

843
3rdparty/ceres-solver-1.11.0/examples/libmv_bundle_adjuster.cc vendored Normal file
View File

@@ -0,0 +1,843 @@
// Copyright (c) 2013 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
//
// Author: mierle@gmail.com (Keir Mierle)
// sergey.vfx@gmail.com (Sergey Sharybin)
//
// This is an example application which contains bundle adjustment code used
// in the Libmv library and Blender. It reads problems from files passed via
// the command line and runs the bundle adjuster on the problem.
//
// File with problem a binary file, for which it is crucial to know in which
// order bytes of float values are stored in. This information is provided
// by a single character in the beginning of the file. There're two possible
// values of this byte:
// - V, which means values in the file are stored with big endian type
// - v, which means values in the file are stored with little endian type
//
// The rest of the file contains data in the following order:
// - Space in which markers' coordinates are stored in
// - Camera intrinsics
// - Number of cameras
// - Cameras
// - Number of 3D points
// - 3D points
// - Number of markers
// - Markers
//
// Markers' space could either be normalized or image (pixels). This is defined
// by the single character in the file. P means markers in the file is in image
// space, and N means markers are in normalized space.
//
// Camera intrinsics are 8 described by 8 float 8.
// This values goes in the following order:
//
// - Focal length, principal point X, principal point Y, k1, k2, k3, p1, p2
//
// Every camera is described by:
//
// - Image for which camera belongs to (single 4 bytes integer value).
// - Column-major camera rotation matrix, 9 float values.
// - Camera translation, 3-component vector of float values.
//
// 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:
//
// - Track number point belongs to (single 4 bytes integer value).
// - 3D position vector, 3-component vector of float values.
//
// Track number shall be greater or equal to zero. Order of tracks does not
// matter and gaps are possible.
//
// Finally every marker is described by:
//
// - Image marker belongs to single 4 bytes integer value).
// - Track marker belongs to single 4 bytes integer value).
// - 2D marker position vector, (two float values).
//
// Marker's space is used a default value for refine_intrinsics command line
// flag. This means if there's no refine_intrinsics flag passed via command
// line, camera intrinsics will be refined if markers in the problem are
// stored in image space and camera intrinsics will not be refined if markers
// are in normalized space.
//
// Passing refine_intrinsics command line flag defines explicitly whether
// refinement of intrinsics will happen. Currently, only none and all
// intrinsics refinement is supported.
//
// There're existing problem files dumped from blender stored in folder
// ../data/libmv-ba-problems.
#include <cstdio>
#include <fcntl.h>
#include <sstream>
#include <string>
#include <vector>
#ifdef _MSC_VER
# include <io.h>
# define open _open
# define close _close
typedef unsigned __int32 uint32_t;
#else
#include <stdint.h>
#include <unistd.h>
// O_BINARY is not defined on unix like platforms, as there is no
// difference between binary and text files.
#define O_BINARY 0
#endif
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#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;
DEFINE_string(input, "", "Input File name");
DEFINE_string(refine_intrinsics, "", "Camera intrinsics to be refined. "
"Options are: none, radial.");
namespace {
// A EuclideanCamera is the location and rotation of the camera
// viewing an image.
//
// image identifies which image this camera represents.
// 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) {}
int image;
Mat3 R;
Vec3 t;
};
// A Point is the 3D location of a track.
//
// 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;
Vec3 X;
};
// A Marker is the 2D location of a tracked point in an image.
//
// x and y is the position of the marker in pixels from the top left corner
// in the image identified by an image. All markers for to the same target
// form a track identified by a common track number.
struct Marker {
int image;
int track;
double x, y;
};
// Cameras intrinsics to be bundled.
//
// BUNDLE_RADIAL actually implies bundling of k1 and k2 coefficients only,
// no bundling of k3 is possible at this moment.
enum BundleIntrinsics {
BUNDLE_NO_INTRINSICS = 0,
BUNDLE_FOCAL_LENGTH = 1,
BUNDLE_PRINCIPAL_POINT = 2,
BUNDLE_RADIAL_K1 = 4,
BUNDLE_RADIAL_K2 = 8,
BUNDLE_RADIAL = 12,
BUNDLE_TANGENTIAL_P1 = 16,
BUNDLE_TANGENTIAL_P2 = 32,
BUNDLE_TANGENTIAL = 48,
};
// Denotes which blocks to keep constant during bundling.
// For example it is useful to keep camera translations constant
// when bundling tripod motions.
enum BundleConstraints {
BUNDLE_NO_CONSTRAINTS = 0,
BUNDLE_NO_TRANSLATION = 1,
};
// The intrinsics need to get combined into a single parameter block; use these
// enums to index instead of numeric constants.
enum {
OFFSET_FOCAL_LENGTH,
OFFSET_PRINCIPAL_POINT_X,
OFFSET_PRINCIPAL_POINT_Y,
OFFSET_K1,
OFFSET_K2,
OFFSET_K3,
OFFSET_P1,
OFFSET_P2,
};
// Returns a pointer to the camera corresponding to a image.
EuclideanCamera *CameraForImage(vector<EuclideanCamera> *all_cameras,
const int image) {
if (image < 0 || image >= all_cameras->size()) {
return NULL;
}
EuclideanCamera *camera = &(*all_cameras)[image];
if (camera->image == -1) {
return NULL;
}
return camera;
}
const EuclideanCamera *CameraForImage(
const vector<EuclideanCamera> &all_cameras,
const int image) {
if (image < 0 || image >= all_cameras.size()) {
return NULL;
}
const EuclideanCamera *camera = &all_cameras[image];
if (camera->image == -1) {
return NULL;
}
return camera;
}
// Returns maximal image number at which marker exists.
int MaxImage(const vector<Marker> &all_markers) {
if (all_markers.size() == 0) {
return -1;
}
int max_image = all_markers[0].image;
for (int i = 1; i < all_markers.size(); i++) {
max_image = std::max(max_image, all_markers[i].image);
}
return max_image;
}
// Returns a pointer to the point corresponding to a track.
EuclideanPoint *PointForTrack(vector<EuclideanPoint> *all_points,
const int track) {
if (track < 0 || track >= all_points->size()) {
return NULL;
}
EuclideanPoint *point = &(*all_points)[track];
if (point->track == -1) {
return NULL;
}
return point;
}
// Reader of binary file which makes sure possibly needed endian
// conversion happens when loading values like floats and integers.
//
// File's endian type is reading from a first character of file, which
// could either be V for big endian or v for little endian. This
// means you need to design file format assuming first character
// denotes file endianness in this way.
class EndianAwareFileReader {
public:
EndianAwareFileReader(void) : file_descriptor_(-1) {
// Get an endian type of the host machine.
union {
unsigned char bytes[4];
uint32_t value;
} endian_test = { { 0, 1, 2, 3 } };
host_endian_type_ = endian_test.value;
file_endian_type_ = host_endian_type_;
}
~EndianAwareFileReader(void) {
if (file_descriptor_ > 0) {
close(file_descriptor_);
}
}
bool OpenFile(const std::string &file_name) {
file_descriptor_ = open(file_name.c_str(), O_RDONLY | O_BINARY);
if (file_descriptor_ < 0) {
return false;
}
// Get an endian tpye of data in the file.
unsigned char file_endian_type_flag = Read<unsigned char>();
if (file_endian_type_flag == 'V') {
file_endian_type_ = kBigEndian;
} else if (file_endian_type_flag == 'v') {
file_endian_type_ = kLittleEndian;
} else {
LOG(FATAL) << "Problem file is stored in unknown endian type.";
}
return true;
}
// Read value from the file, will switch endian if needed.
template <typename T>
T Read(void) const {
T value;
CHECK_GT(read(file_descriptor_, &value, sizeof(value)), 0);
// Switch endian type if file contains data in different type
// that current machine.
if (file_endian_type_ != host_endian_type_) {
value = SwitchEndian<T>(value);
}
return value;
}
private:
static const long int kLittleEndian = 0x03020100ul;
static const long int kBigEndian = 0x00010203ul;
// Switch endian type between big to little.
template <typename T>
T SwitchEndian(const T value) const {
if (sizeof(T) == 4) {
unsigned int temp_value = static_cast<unsigned int>(value);
return ((temp_value >> 24)) |
((temp_value << 8) & 0x00ff0000) |
((temp_value >> 8) & 0x0000ff00) |
((temp_value << 24));
} else if (sizeof(T) == 1) {
return value;
} else {
LOG(FATAL) << "Entered non-implemented part of endian switching function.";
}
}
int host_endian_type_;
int file_endian_type_;
int file_descriptor_;
};
// Read 3x3 column-major matrix from the file
void ReadMatrix3x3(const EndianAwareFileReader &file_reader,
Mat3 *matrix) {
for (int i = 0; i < 9; i++) {
(*matrix)(i % 3, i / 3) = file_reader.Read<float>();
}
}
// Read 3-vector from file
void ReadVector3(const EndianAwareFileReader &file_reader,
Vec3 *vector) {
for (int i = 0; i < 3; i++) {
(*vector)(i) = file_reader.Read<float>();
}
}
// Reads a bundle adjustment problem from the file.
//
// file_name denotes from which file to read the problem.
// camera_intrinsics will contain initial camera intrinsics values.
//
// all_cameras is a vector of all reconstructed cameras to be optimized,
// vector element with number i will contain camera for image i.
//
// all_points is a vector of all reconstructed 3D points to be optimized,
// vector element with number i will contain point for track i.
//
// all_markers is a vector of all tracked markers existing in
// the problem. Only used for reprojection error calculation, stay
// unchanged during optimization.
//
// Returns false if any kind of error happened during
// reading.
bool ReadProblemFromFile(const std::string &file_name,
double camera_intrinsics[8],
vector<EuclideanCamera> *all_cameras,
vector<EuclideanPoint> *all_points,
bool *is_image_space,
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>();
if (is_image_space_flag == 'P') {
*is_image_space = true;
} else if (is_image_space_flag == 'N') {
*is_image_space = false;
} else {
LOG(FATAL) << "Problem file contains markers stored in unknown space.";
}
// Read camera intrinsics.
for (int i = 0; i < 8; i++) {
camera_intrinsics[i] = file_reader.Read<float>();
}
// Read all cameras.
int number_of_cameras = file_reader.Read<int>();
for (int i = 0; i < number_of_cameras; i++) {
EuclideanCamera camera;
camera.image = file_reader.Read<int>();
ReadMatrix3x3(file_reader, &camera.R);
ReadVector3(file_reader, &camera.t);
if (camera.image >= all_cameras->size()) {
all_cameras->resize(camera.image + 1);
}
(*all_cameras)[camera.image].image = camera.image;
(*all_cameras)[camera.image].R = camera.R;
(*all_cameras)[camera.image].t = camera.t;
}
LOG(INFO) << "Read " << number_of_cameras << " cameras.";
// Read all reconstructed 3D points.
int number_of_points = file_reader.Read<int>();
for (int i = 0; i < number_of_points; i++) {
EuclideanPoint point;
point.track = file_reader.Read<int>();
ReadVector3(file_reader, &point.X);
if (point.track >= all_points->size()) {
all_points->resize(point.track + 1);
}
(*all_points)[point.track].track = point.track;
(*all_points)[point.track].X = point.X;
}
LOG(INFO) << "Read " << number_of_points << " points.";
// And finally read all markers.
int number_of_markers = file_reader.Read<int>();
for (int i = 0; i < number_of_markers; i++) {
Marker marker;
marker.image = file_reader.Read<int>();
marker.track = file_reader.Read<int>();
marker.x = file_reader.Read<float>();
marker.y = file_reader.Read<float>();
all_markers->push_back(marker);
}
LOG(INFO) << "Read " << number_of_markers << " markers.";
return true;
}
// Apply camera intrinsics to the normalized point to get image coordinates.
// This applies the radial lens distortion to a point which is in normalized
// camera coordinates (i.e. the principal point is at (0, 0)) to get image
// coordinates in pixels. Templated for use with autodifferentiation.
template <typename T>
inline void ApplyRadialDistortionCameraIntrinsics(const T &focal_length_x,
const T &focal_length_y,
const T &principal_point_x,
const T &principal_point_y,
const T &k1,
const T &k2,
const T &k3,
const T &p1,
const T &p2,
const T &normalized_x,
const T &normalized_y,
T *image_x,
T *image_y) {
T x = normalized_x;
T y = normalized_y;
// Apply distortion to the normalized points to get (xd, yd).
T r2 = x*x + y*y;
T r4 = r2 * r2;
T r6 = r4 * r2;
T r_coeff = (T(1) + k1*r2 + k2*r4 + k3*r6);
T xd = x * r_coeff + T(2)*p1*x*y + p2*(r2 + T(2)*x*x);
T yd = y * r_coeff + T(2)*p2*x*y + p1*(r2 + T(2)*y*y);
// Apply focal length and principal point to get the final image coordinates.
*image_x = focal_length_x * xd + principal_point_x;
*image_y = focal_length_y * yd + principal_point_y;
}
// Cost functor which computes reprojection error of 3D point X
// on camera defined by angle-axis rotation and it's translation
// (which are in the same block due to optimization reasons).
//
// This functor uses a radial distortion model.
struct OpenCVReprojectionError {
OpenCVReprojectionError(const double observed_x, const double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T* const intrinsics,
const T* const R_t, // Rotation denoted by angle axis
// followed with translation
const T* const X, // Point coordinates 3x1.
T* residuals) const {
// Unpack the intrinsics.
const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH];
const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X];
const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y];
const T& k1 = intrinsics[OFFSET_K1];
const T& k2 = intrinsics[OFFSET_K2];
const T& k3 = intrinsics[OFFSET_K3];
const T& p1 = intrinsics[OFFSET_P1];
const T& p2 = intrinsics[OFFSET_P2];
// Compute projective coordinates: x = RX + t.
T x[3];
ceres::AngleAxisRotatePoint(R_t, X, x);
x[0] += R_t[3];
x[1] += R_t[4];
x[2] += R_t[5];
// Compute normalized coordinates: x /= x[2].
T xn = x[0] / x[2];
T yn = x[1] / x[2];
T predicted_x, predicted_y;
// Apply distortion to the normalized points to get (xd, yd).
// TODO(keir): Do early bailouts for zero distortion; these are expensive
// jet operations.
ApplyRadialDistortionCameraIntrinsics(focal_length,
focal_length,
principal_point_x,
principal_point_y,
k1, k2, k3,
p1, p2,
xn, yn,
&predicted_x,
&predicted_y);
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
const double observed_x;
const double observed_y;
};
// Print a message to the log which camera intrinsics are gonna to be optimized.
void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
LOG(INFO) << "Bundling only camera positions.";
} else {
std::string bundling_message = "";
#define APPEND_BUNDLING_INTRINSICS(name, flag) \
if (bundle_intrinsics & flag) { \
if (!bundling_message.empty()) { \
bundling_message += ", "; \
} \
bundling_message += name; \
} (void)0
APPEND_BUNDLING_INTRINSICS("f", BUNDLE_FOCAL_LENGTH);
APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT);
APPEND_BUNDLING_INTRINSICS("k1", BUNDLE_RADIAL_K1);
APPEND_BUNDLING_INTRINSICS("k2", BUNDLE_RADIAL_K2);
APPEND_BUNDLING_INTRINSICS("p1", BUNDLE_TANGENTIAL_P1);
APPEND_BUNDLING_INTRINSICS("p2", BUNDLE_TANGENTIAL_P2);
LOG(INFO) << "Bundling " << bundling_message << ".";
}
}
// Print a message to the log containing all the camera intriniscs values.
void PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) {
std::ostringstream intrinsics_output;
intrinsics_output << "f=" << camera_intrinsics[OFFSET_FOCAL_LENGTH];
intrinsics_output <<
" cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] <<
" cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y];
#define APPEND_DISTORTION_COEFFICIENT(name, offset) \
{ \
if (camera_intrinsics[offset] != 0.0) { \
intrinsics_output << " " name "=" << camera_intrinsics[offset]; \
} \
} (void)0
APPEND_DISTORTION_COEFFICIENT("k1", OFFSET_K1);
APPEND_DISTORTION_COEFFICIENT("k2", OFFSET_K2);
APPEND_DISTORTION_COEFFICIENT("k3", OFFSET_K3);
APPEND_DISTORTION_COEFFICIENT("p1", OFFSET_P1);
APPEND_DISTORTION_COEFFICIENT("p2", OFFSET_P2);
#undef APPEND_DISTORTION_COEFFICIENT
LOG(INFO) << text << intrinsics_output.str();
}
// Get a vector of camera's rotations denoted by angle axis
// conjuncted with translations into single block
//
// 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;
int max_image = MaxImage(all_markers);
all_cameras_R_t.resize(max_image + 1);
for (int i = 0; i <= max_image; i++) {
const EuclideanCamera *camera = CameraForImage(all_cameras, i);
if (!camera) {
continue;
}
ceres::RotationMatrixToAngleAxis(&camera->R(0, 0),
&all_cameras_R_t[i](0));
all_cameras_R_t[i].tail<3>() = camera->t;
}
return all_cameras_R_t;
}
// 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) {
int max_image = MaxImage(all_markers);
for (int i = 0; i <= max_image; i++) {
EuclideanCamera *camera = CameraForImage(all_cameras, i);
if (!camera) {
continue;
}
ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0),
&camera->R(0, 0));
camera->t = all_cameras_R_t[i].tail<3>();
}
}
void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers,
const int bundle_intrinsics,
const int bundle_constraints,
double *camera_intrinsics,
vector<EuclideanCamera> *all_cameras,
vector<EuclideanPoint> *all_points) {
PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics);
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// Convert cameras rotations to angle axis and merge with translation
// into single parameter block for maximal minimization speed
//
// Block for minimization has got the following structure:
// <3 elements for angle-axis> <3 elements for translation>
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;
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
std::vector<int> constant_translation;
// First three elements are rotation, last three are translation.
constant_translation.push_back(3);
constant_translation.push_back(4);
constant_translation.push_back(5);
constant_transform_parameterization =
new ceres::SubsetParameterization(6, constant_translation);
}
int num_residuals = 0;
bool have_locked_camera = false;
for (int i = 0; i < all_markers.size(); ++i) {
const Marker &marker = all_markers[i];
EuclideanCamera *camera = CameraForImage(all_cameras, marker.image);
EuclideanPoint *point = PointForTrack(all_points, marker.track);
if (camera == NULL || point == NULL) {
continue;
}
// Rotation of camera denoted in angle axis followed with
// camera translaiton.
double *current_camera_R_t = &all_cameras_R_t[camera->image](0);
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
OpenCVReprojectionError, 2, 8, 6, 3>(
new OpenCVReprojectionError(
marker.x,
marker.y)),
NULL,
camera_intrinsics,
current_camera_R_t,
&point->X(0));
// We lock the first camera to better deal with scene orientation ambiguity.
if (!have_locked_camera) {
problem.SetParameterBlockConstant(current_camera_R_t);
have_locked_camera = true;
}
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
problem.SetParameterization(current_camera_R_t,
constant_transform_parameterization);
}
num_residuals++;
}
LOG(INFO) << "Number of residuals: " << num_residuals;
if (!num_residuals) {
LOG(INFO) << "Skipping running minimizer with zero residuals";
return;
}
BundleIntrinsicsLogMessage(bundle_intrinsics);
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
// No camera intrinsics are being refined,
// set the whole parameter block as constant for best performance.
problem.SetParameterBlockConstant(camera_intrinsics);
} else {
// Set the camera intrinsics that are not to be bundled as
// constant using some macro trickery.
std::vector<int> constant_intrinsics;
#define MAYBE_SET_CONSTANT(bundle_enum, offset) \
if (!(bundle_intrinsics & bundle_enum)) { \
constant_intrinsics.push_back(offset); \
}
MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH);
MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X);
MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y);
MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1);
MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2);
MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1);
MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2);
#undef MAYBE_SET_CONSTANT
// 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);
}
// Configure the solver.
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.preconditioner_type = ceres::SCHUR_JACOBI;
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
options.use_inner_iterations = true;
options.max_num_iterations = 100;
options.minimizer_progress_to_stdout = true;
// Solve!
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << "Final report:\n" << summary.FullReport();
// Copy rotations and translations back.
UnpackCamerasRotationAndTranslation(all_markers,
all_cameras_R_t,
all_cameras);
PrintCameraIntrinsics("Final intrinsics: ", camera_intrinsics);
}
} // namespace
int main(int argc, char **argv) {
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
if (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;
bool is_image_space;
vector<Marker> all_markers;
if (!ReadProblemFromFile(FLAGS_input,
camera_intrinsics,
&all_cameras,
&all_points,
&is_image_space,
&all_markers)) {
LOG(ERROR) << "Error reading problem file";
return EXIT_FAILURE;
}
// If there's no refine_intrinsics passed via command line
// (in this case FLAGS_refine_intrinsics will be an empty string)
// we use problem's settings to detect whether intrinsics
// shall be refined or not.
//
// Namely, if problem has got markers stored in image (pixel)
// space, we do full intrinsics refinement. If markers are
// stored in normalized space, and refine_intrinsics is not
// set, no refining will happen.
//
// Using command line argument refine_intrinsics will explicitly
// 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 (is_image_space) {
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
}
} else {
if (FLAGS_refine_intrinsics == "radial") {
bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
} else if (FLAGS_refine_intrinsics != "none") {
LOG(ERROR) << "Unsupported value for refine-intrinsics";
return EXIT_FAILURE;
}
}
// Run the bundler.
EuclideanBundleCommonIntrinsics(all_markers,
bundle_intrinsics,
BUNDLE_NO_CONSTRAINTS,
camera_intrinsics,
&all_cameras,
&all_points);
return EXIT_SUCCESS;
}

414
3rdparty/ceres-solver-1.11.0/examples/libmv_homography.cc vendored Normal file
View File

@@ -0,0 +1,414 @@
// 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.
//
// Copyright (c) 2014 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
//
// Author: sergey.vfx@gmail.com (Sergey Sharybin)
//
// This file demonstrates solving for a homography between two sets of points.
// A homography describes a transformation between a sets of points on a plane,
// perspectively projected into two images. The first step is to solve a
// homogeneous system of equations via singular value decompposition, giving an
// algebraic solution for the homography, then solving for a final solution by
// minimizing the symmetric transfer error in image space with Ceres (called the
// Gold Standard Solution in "Multiple View Geometry"). The routines are based on
// the routines from the Libmv library.
//
// This example demonstrates custom exit criterion by having a callback check
// for image-space error.
#include "ceres/ceres.h"
#include "glog/logging.h"
typedef Eigen::NumTraits<double> EigenDouble;
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;
namespace {
// This structure contains options that controls how the homography
// estimation operates.
//
// Defaults should be suitable for a wide range of use cases, but
// better performance and accuracy might require tweaking.
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) {}
// Maximal number of iterations for the refinement step.
int max_num_iterations;
// Expected average of symmetric geometric distance between
// actual destination points and original ones transformed by
// estimated homography matrix.
//
// Refinement will finish as soon as average of symmetric
// 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;
};
// Calculate symmetric geometric cost terms:
//
// forward_error = D(H * x1, x2)
// backward_error = D(H^-1 * x2, x1)
//
// Templated to be used with autodifferenciation.
template <typename T>
void SymmetricGeometricDistanceTerms(const Eigen::Matrix<T, 3, 3> &H,
const Eigen::Matrix<T, 2, 1> &x1,
const Eigen::Matrix<T, 2, 1> &x2,
T forward_error[2],
T backward_error[2]) {
typedef Eigen::Matrix<T, 3, 1> Vec3;
Vec3 x(x1(0), x1(1), T(1.0));
Vec3 y(x2(0), x2(1), T(1.0));
Vec3 H_x = H * x;
Vec3 Hinv_y = H.inverse() * y;
H_x /= H_x(2);
Hinv_y /= Hinv_y(2);
forward_error[0] = H_x(0) - y(0);
forward_error[1] = H_x(1) - y(1);
backward_error[0] = Hinv_y(0) - x(0);
backward_error[1] = Hinv_y(1) - x(1);
}
// Calculate symmetric geometric cost:
//
// D(H * x1, x2)^2 + D(H^-1 * x2, x1)^2
//
double SymmetricGeometricDistance(const Mat3 &H,
const Vec2 &x1,
const Vec2 &x2) {
Vec2 forward_error, backward_error;
SymmetricGeometricDistanceTerms<double>(H,
x1,
x2,
forward_error.data(),
backward_error.data());
return forward_error.squaredNorm() +
backward_error.squaredNorm();
}
// A parameterization of the 2D homography matrix that uses 8 parameters so
// that the matrix is normalized (H(2,2) == 1).
// The homography matrix H is built from a list of 8 parameters (a, b,...g, h)
// as follows
//
// |a b c|
// H = |d e f|
// |g h 1|
//
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
// Convert from the 8 parameters to a H matrix.
static void To(const Parameters &p, Parameterized *h) {
*h << p(0), p(1), p(2),
p(3), p(4), p(5),
p(6), p(7), 1.0;
}
// Convert from a H matrix to the 8 parameters.
static void From(const Parameterized &h, Parameters *p) {
*p << h(0, 0), h(0, 1), h(0, 2),
h(1, 0), h(1, 1), h(1, 2),
h(2, 0), h(2, 1);
}
};
// 2D Homography transformation estimation in the case that points are in
// euclidean coordinates.
//
// x = H y
//
// x and y vector must have the same direction, we could write
//
// crossproduct(|x|, * H * |y| ) = |0|
//
// | 0 -1 x2| |a b c| |y1| |0|
// | 1 0 -x1| * |d e f| * |y2| = |0|
// |-x2 x1 0| |g h 1| |1 | |0|
//
// That gives:
//
// (-d+x2*g)*y1 + (-e+x2*h)*y2 + -f+x2 |0|
// (a-x1*g)*y1 + (b-x1*h)*y2 + c-x1 = |0|
// (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f |0|
//
bool Homography2DFromCorrespondencesLinearEuc(
const Mat &x1,
const Mat &x2,
Mat3 *H,
double expected_precision) {
assert(2 == x1.rows());
assert(4 <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
int 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;
L(j, 0) = x1(0, i); // a
L(j, 1) = x1(1, i); // b
L(j, 2) = 1.0; // c
L(j, 6) = -x2(0, i) * x1(0, i); // g
L(j, 7) = -x2(0, i) * x1(1, i); // h
b(j, 0) = x2(0, i); // i
++j;
L(j, 3) = x1(0, i); // d
L(j, 4) = x1(1, i); // e
L(j, 5) = 1.0; // f
L(j, 6) = -x2(1, i) * x1(0, i); // g
L(j, 7) = -x2(1, i) * x1(1, i); // h
b(j, 0) = x2(1, i); // i
// This ensures better stability
// TODO(julien) make a lite version without this 3rd set
++j;
L(j, 0) = x2(1, i) * x1(0, i); // a
L(j, 1) = x2(1, i) * x1(1, i); // b
L(j, 2) = x2(1, i); // c
L(j, 3) = -x2(0, i) * x1(0, i); // d
L(j, 4) = -x2(0, i) * x1(1, i); // e
L(j, 5) = -x2(0, i); // f
}
// Solve Lx=B
const Vec h = L.fullPivLu().solve(b);
Homography2DNormalizedParameterization<double>::To(h, H);
return (L * h).isApprox(b, expected_precision);
}
// Cost functor which computes symmetric geometric distance
// used for homography matrix refinement.
class HomographySymmetricGeometricCostFunctor {
public:
HomographySymmetricGeometricCostFunctor(const Vec2 &x,
const Vec2 &y)
: x_(x), y_(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;
Mat3 H(homography_parameters);
Vec2 x(T(x_(0)), T(x_(1)));
Vec2 y(T(y_(0)), T(y_(1)));
SymmetricGeometricDistanceTerms<T>(H,
x,
y,
&residuals[0],
&residuals[2]);
return true;
}
const Vec2 x_;
const Vec2 y_;
};
// Termination checking callback. This is needed to finish the
// optimization when an absolute error threshold is met, as opposed
// to Ceres's function_tolerance, which provides for finishing when
// successful steps reduce the cost function by a fractional amount.
// In this case, the callback checks for the absolute average reprojection
// error and terminates when it's below a threshold (for example all
// points < 0.5px error).
class TerminationCheckingCallback : public ceres::IterationCallback {
public:
TerminationCheckingCallback(const Mat &x1, const Mat &x2,
const EstimateHomographyOptions &options,
Mat3 *H)
: options_(options), x1_(x1), x2_(x2), H_(H) {}
virtual ceres::CallbackReturnType operator()(
const ceres::IterationSummary& summary) {
// If the step wasn't successful, there's nothing to do.
if (!summary.step_is_successful) {
return ceres::SOLVER_CONTINUE;
}
// Calculate average of symmetric geometric distance.
double average_distance = 0.0;
for (int i = 0; i < x1_.cols(); i++) {
average_distance += SymmetricGeometricDistance(*H_,
x1_.col(i),
x2_.col(i));
}
average_distance /= x1_.cols();
if (average_distance <= options_.expected_average_symmetric_distance) {
return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
}
return ceres::SOLVER_CONTINUE;
}
private:
const EstimateHomographyOptions &options_;
const Mat &x1_;
const Mat &x2_;
Mat3 *H_;
};
bool EstimateHomography2DFromCorrespondences(
const Mat &x1,
const Mat &x2,
const EstimateHomographyOptions &options,
Mat3 *H) {
assert(2 == x1.rows());
assert(4 <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
// Step 1: Algebraic homography estimation.
// Assume algebraic estimation always succeeds.
Homography2DFromCorrespondencesLinearEuc(x1,
x2,
H,
EigenDouble::dummy_precision());
LOG(INFO) << "Estimated matrix after algebraic estimation:\n" << *H;
// 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,
H->data());
}
// Configure the solve.
ceres::Solver::Options solver_options;
solver_options.linear_solver_type = ceres::DENSE_QR;
solver_options.max_num_iterations = options.max_num_iterations;
solver_options.update_state_every_iteration = true;
// Terminate if the average symmetric distance is good enough.
TerminationCheckingCallback callback(x1, x2, options, H);
solver_options.callbacks.push_back(&callback);
// Run the solve.
ceres::Solver::Summary summary;
ceres::Solve(solver_options, &problem, &summary);
LOG(INFO) << "Summary:\n" << summary.FullReport();
LOG(INFO) << "Final refined matrix:\n" << *H;
return summary.IsSolutionUsable();
}
} // namespace libmv
int main(int argc, char **argv) {
google::InitGoogleLogging(argv[0]);
Mat x1(2, 100);
for (int i = 0; i < x1.cols(); ++i) {
x1(0, i) = rand() % 1024;
x1(1, i) = rand() % 1024;
}
Mat3 homography_matrix;
// This matrix has been dumped from a Blender test file of plane tracking.
homography_matrix << 1.243715, -0.461057, -111.964454,
0.0, 0.617589, -192.379252,
0.0, -0.000983, 1.0;
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);
// Apply some noise so algebraic estimation is not good enough.
x2(0, i) += static_cast<double>(rand() % 1000) / 5000.0;
x2(1, i) += static_cast<double>(rand() % 1000) / 5000.0;
}
Mat3 estimated_matrix;
EstimateHomographyOptions options;
options.expected_average_symmetric_distance = 0.02;
EstimateHomography2DFromCorrespondences(x1, x2, options, &estimated_matrix);
// Normalize the matrix for easier comparison.
estimated_matrix /= estimated_matrix(2 ,2);
std::cout << "Original matrix:\n" << homography_matrix << "\n";
std::cout << "Estimated matrix:\n" << estimated_matrix << "\n";
return EXIT_SUCCESS;
}

678
3rdparty/ceres-solver-1.11.0/examples/more_garbow_hillstrom.cc vendored Normal file
View File

@@ -0,0 +1,678 @@
// 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)
//
// Test problems from the paper
//
// Testing Unconstrained Optimization Software
// Jorge J. More, Burton S. Garbow and Kenneth E. Hillstrom
// ACM Transactions on Mathematical Software, 7(1), pp. 17-41, 1981
//
// A subset of these problems were augmented with bounds and used for
// testing bounds constrained optimization algorithms by
//
// A Trust Region Approach to Linearly Constrained Optimization
// David M. Gay
// Numerical Analysis (Griffiths, D.F., ed.), pp. 72-105
// Lecture Notes in Mathematics 1066, Springer Verlag, 1984.
//
// The latter paper is behind a paywall. We obtained the bounds on the
// variables and the function values at the global minimums from
//
// http://www.mat.univie.ac.at/~neum/glopt/bounds.html
//
// A problem is considered solved if of the log relative error of its
// objective function is at least 4.
#include <cmath>
#include <iostream> // NOLINT
#include <sstream> // NOLINT
#include <string>
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
DEFINE_string(problem, "all", "Which problem to solve");
DEFINE_bool(use_numeric_diff, false,
"Use numeric differentiation instead of automatic "
"differentiation.");
DEFINE_string(numeric_diff_method, "ridders", "When using numeric "
"differentiation, selects algorithm. Options are: central, "
"forward, ridders.");
DEFINE_int32(ridders_extrapolations, 3, "Maximal number of extrapolations in "
"Ridders' method.");
namespace ceres {
namespace examples {
const double kDoubleMax = std::numeric_limits<double>::max();
static void SetNumericDiffOptions(ceres::NumericDiffOptions* options) {
options->max_num_ridders_extrapolations = FLAGS_ridders_extrapolations;
}
#define BEGIN_MGH_PROBLEM(name, num_parameters, num_residuals) \
struct name { \
static const 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> \
bool operator()(const T* const x, T* residual) const {
#define END_MGH_PROBLEM return true; } }; // NOLINT
// Rosenbrock function.
BEGIN_MGH_PROBLEM(TestProblem1, 2, 2)
const T x1 = x[0];
const T x2 = x[1];
residual[0] = T(10.0) * (x2 - x1 * x1);
residual[1] = T(1.0) - x1;
END_MGH_PROBLEM;
const double TestProblem1::initial_x[] = {-1.2, 1.0};
const double TestProblem1::lower_bounds[] = {-kDoubleMax, -kDoubleMax};
const double TestProblem1::upper_bounds[] = {kDoubleMax, kDoubleMax};
const double TestProblem1::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem1::unconstrained_optimal_cost = 0.0;
// Freudenstein and Roth function.
BEGIN_MGH_PROBLEM(TestProblem2, 2, 2)
const T x1 = x[0];
const T x2 = x[1];
residual[0] = T(-13.0) + x1 + ((T(5.0) - x2) * x2 - T(2.0)) * x2;
residual[1] = T(-29.0) + x1 + ((x2 + T(1.0)) * x2 - T(14.0)) * x2;
END_MGH_PROBLEM;
const double TestProblem2::initial_x[] = {0.5, -2.0};
const double TestProblem2::lower_bounds[] = {-kDoubleMax, -kDoubleMax};
const double TestProblem2::upper_bounds[] = {kDoubleMax, kDoubleMax};
const double TestProblem2::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem2::unconstrained_optimal_cost = 0.0;
// Powell badly scaled function.
BEGIN_MGH_PROBLEM(TestProblem3, 2, 2)
const T x1 = x[0];
const T x2 = x[1];
residual[0] = T(10000.0) * x1 * x2 - T(1.0);
residual[1] = exp(-x1) + exp(-x2) - T(1.0001);
END_MGH_PROBLEM;
const double TestProblem3::initial_x[] = {0.0, 1.0};
const double TestProblem3::lower_bounds[] = {0.0, 1.0};
const double TestProblem3::upper_bounds[] = {1.0, 9.0};
const double TestProblem3::constrained_optimal_cost = 0.15125900e-9;
const double TestProblem3::unconstrained_optimal_cost = 0.0;
// Brown badly scaled function.
BEGIN_MGH_PROBLEM(TestProblem4, 2, 3)
const T x1 = x[0];
const T x2 = x[1];
residual[0] = x1 - T(1000000.0);
residual[1] = x2 - T(0.000002);
residual[2] = x1 * x2 - T(2.0);
END_MGH_PROBLEM;
const double TestProblem4::initial_x[] = {1.0, 1.0};
const double TestProblem4::lower_bounds[] = {0.0, 0.00003};
const double TestProblem4::upper_bounds[] = {1000000.0, 100.0};
const double TestProblem4::constrained_optimal_cost = 0.78400000e3;
const double TestProblem4::unconstrained_optimal_cost = 0.0;
// Beale function.
BEGIN_MGH_PROBLEM(TestProblem5, 2, 3)
const T x1 = x[0];
const T x2 = x[1];
residual[0] = T(1.5) - x1 * (T(1.0) - x2);
residual[1] = T(2.25) - x1 * (T(1.0) - x2 * x2);
residual[2] = T(2.625) - x1 * (T(1.0) - x2 * x2 * x2);
END_MGH_PROBLEM;
const double TestProblem5::initial_x[] = {1.0, 1.0};
const double TestProblem5::lower_bounds[] = {0.6, 0.5};
const double TestProblem5::upper_bounds[] = {10.0, 100.0};
const double TestProblem5::constrained_optimal_cost = 0.0;
const double TestProblem5::unconstrained_optimal_cost = 0.0;
// Jennrich and Sampson function.
BEGIN_MGH_PROBLEM(TestProblem6, 2, 10)
const T x1 = x[0];
const T x2 = x[1];
for (int i = 1; i <= 10; ++i) {
residual[i - 1] = T(2.0) + T(2.0 * i) -
(exp(T(static_cast<double>(i)) * x1) +
exp(T(static_cast<double>(i) * x2)));
}
END_MGH_PROBLEM;
const double TestProblem6::initial_x[] = {1.0, 1.0};
const double TestProblem6::lower_bounds[] = {-kDoubleMax, -kDoubleMax};
const double TestProblem6::upper_bounds[] = {kDoubleMax, kDoubleMax};
const double TestProblem6::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem6::unconstrained_optimal_cost = 124.362;
// Helical valley function.
BEGIN_MGH_PROBLEM(TestProblem7, 3, 3)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T theta = T(0.5 / M_PI) * atan(x2 / x1) + (x1 > 0.0 ? T(0.0) : T(0.5));
residual[0] = T(10.0) * (x3 - T(10.0) * theta);
residual[1] = T(10.0) * (sqrt(x1 * x1 + x2 * x2) - T(1.0));
residual[2] = x3;
END_MGH_PROBLEM;
const double TestProblem7::initial_x[] = {-1.0, 0.0, 0.0};
const double TestProblem7::lower_bounds[] = {-100.0, -1.0, -1.0};
const double TestProblem7::upper_bounds[] = {0.8, 1.0, 1.0};
const double TestProblem7::constrained_optimal_cost = 0.99042212;
const double TestProblem7::unconstrained_optimal_cost = 0.0;
// Bard function
BEGIN_MGH_PROBLEM(TestProblem8, 3, 15)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
double y[] = {0.14, 0.18, 0.22, 0.25,
0.29, 0.32, 0.35, 0.39, 0.37, 0.58,
0.73, 0.96, 1.34, 2.10, 4.39};
for (int i = 1; i <=15; ++i) {
const T u = T(static_cast<double>(i));
const T v = T(static_cast<double>(16 - i));
const T w = T(static_cast<double>(std::min(i, 16 - i)));
residual[i - 1] = T(y[i - 1]) - (x1 + u / (v * x2 + w * x3));
}
END_MGH_PROBLEM;
const double TestProblem8::initial_x[] = {1.0, 1.0, 1.0};
const double TestProblem8::lower_bounds[] = {
-kDoubleMax, -kDoubleMax, -kDoubleMax};
const double TestProblem8::upper_bounds[] = {
kDoubleMax, kDoubleMax, kDoubleMax};
const double TestProblem8::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem8::unconstrained_optimal_cost = 8.21487e-3;
// Gaussian function.
BEGIN_MGH_PROBLEM(TestProblem9, 3, 15)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const double y[] = {0.0009, 0.0044, 0.0175, 0.0540, 0.1295, 0.2420, 0.3521,
0.3989,
0.3521, 0.2420, 0.1295, 0.0540, 0.0175, 0.0044, 0.0009};
for (int i = 0; i < 15; ++i) {
const T t_i = T((8.0 - i - 1.0) / 2.0);
const T y_i = T(y[i]);
residual[i] = x1 * exp(-x2 * (t_i - x3) * (t_i - x3) / T(2.0)) - y_i;
}
END_MGH_PROBLEM;
const double TestProblem9::initial_x[] = {0.4, 1.0, 0.0};
const double TestProblem9::lower_bounds[] = {0.398, 1.0, -0.5};
const double TestProblem9::upper_bounds[] = {4.2, 2.0, 0.1};
const double TestProblem9::constrained_optimal_cost = 0.11279300e-7;
const double TestProblem9::unconstrained_optimal_cost = 0.112793e-7;
// Meyer function.
BEGIN_MGH_PROBLEM(TestProblem10, 3, 16)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const double y[] = {34780, 28610, 23650, 19630, 16370, 13720, 11540, 9744,
8261, 7030, 6005, 5147, 4427, 3820, 3307, 2872};
for (int i = 0; i < 16; ++i) {
const T ti = T(45 + 5.0 * (i + 1));
const T yi = T(y[i]);
residual[i] = x1 * exp(x2 / (ti + x3)) - yi;
}
END_MGH_PROBLEM
const double TestProblem10::initial_x[] = {0.02, 4000, 250};
const double TestProblem10::lower_bounds[] = {
-kDoubleMax, -kDoubleMax, -kDoubleMax};
const double TestProblem10::upper_bounds[] = {
kDoubleMax, kDoubleMax, kDoubleMax};
const double TestProblem10::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem10::unconstrained_optimal_cost = 87.9458;
// Gulf research and development function
BEGIN_MGH_PROBLEM(TestProblem11, 3, 100)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
for (int i = 1; i <= 100; ++i) {
const double ti = static_cast<double>(i) / 100.0;
const double yi = 25.0 + pow(-50.0 * log(ti), 2.0 / 3.0);
residual[i - 1] = exp(-pow(abs(T(yi * 100.0 * i) * x2), x3) / x1) - T(ti);
}
END_MGH_PROBLEM
const double TestProblem11::initial_x[] = {5.0, 2.5, 0.15};
const double TestProblem11::lower_bounds[] = {1e-16, 0.0, 0.0};
const double TestProblem11::upper_bounds[] = {10.0, 10.0, 10.0};
const double TestProblem11::constrained_optimal_cost = 0.58281431e-4;
const double TestProblem11::unconstrained_optimal_cost = 0.0;
// Box three-dimensional function.
BEGIN_MGH_PROBLEM(TestProblem12, 3, 3)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T t1 = T(0.1);
const T t2 = T(0.2);
const T t3 = T(0.3);
residual[0] = exp(-t1 * x1) - exp(-t1 * x2) - x3 * (exp(-t1) - exp(-T(10.0) * t1));
residual[1] = exp(-t2 * x1) - exp(-t2 * x2) - x3 * (exp(-t2) - exp(-T(10.0) * t2));
residual[2] = exp(-t3 * x1) - exp(-t3 * x2) - x3 * (exp(-t3) - exp(-T(10.0) * t3));
END_MGH_PROBLEM
const double TestProblem12::initial_x[] = {0.0, 10.0, 20.0};
const double TestProblem12::lower_bounds[] = {0.0, 5.0, 0.0};
const double TestProblem12::upper_bounds[] = {2.0, 9.5, 20.0};
const double TestProblem12::constrained_optimal_cost = 0.30998153e-5;
const double TestProblem12::unconstrained_optimal_cost = 0.0;
// Powell Singular function.
BEGIN_MGH_PROBLEM(TestProblem13, 4, 4)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T x4 = x[3];
residual[0] = x1 + T(10.0) * x2;
residual[1] = T(sqrt(5.0)) * (x3 - x4);
residual[2] = (x2 - T(2.0) * x3) * (x2 - T(2.0) * x3);
residual[3] = sqrt(10.0) * (x1 - x4) * (x1 - x4);
END_MGH_PROBLEM
const double TestProblem13::initial_x[] = {3.0, -1.0, 0.0, 1.0};
const double TestProblem13::lower_bounds[] = {
-kDoubleMax, -kDoubleMax, -kDoubleMax};
const double TestProblem13::upper_bounds[] = {
kDoubleMax, kDoubleMax, kDoubleMax};
const double TestProblem13::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem13::unconstrained_optimal_cost = 0.0;
// Wood function.
BEGIN_MGH_PROBLEM(TestProblem14, 4, 6)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T x4 = x[3];
residual[0] = T(10.0) * (x2 - x1 * x1);
residual[1] = T(1.0) - x1;
residual[2] = T(sqrt(90.0)) * (x4 - x3 * x3);
residual[3] = T(1.0) - x3;
residual[4] = T(sqrt(10.0)) * (x2 + x4 - T(2.0));
residual[5] = T(1.0/sqrt(10.0)) * (x2 - x4);
END_MGH_PROBLEM;
const double TestProblem14::initial_x[] = {-3.0, -1.0, -3.0, -1.0};
const double TestProblem14::lower_bounds[] = {-100.0, -100.0, -100.0, -100.0};
const double TestProblem14::upper_bounds[] = {0.0, 10.0, 100.0, 100.0};
const double TestProblem14::constrained_optimal_cost = 0.15567008e1;
const double TestProblem14::unconstrained_optimal_cost = 0.0;
// Kowalik and Osborne function.
BEGIN_MGH_PROBLEM(TestProblem15, 4, 11)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T x4 = x[3];
const double y[] = {0.1957, 0.1947, 0.1735, 0.1600, 0.0844, 0.0627,
0.0456, 0.0342, 0.0323, 0.0235, 0.0246};
const double u[] = {4.0, 2.0, 1.0, 0.5, 0.25, 0.167, 0.125, 0.1,
0.0833, 0.0714, 0.0625};
for (int i = 0; i < 11; ++i) {
const T yi = T(y[i]);
const T ui = T(u[i]);
residual[i] = yi - x1 * (ui * ui + ui * x2) / (ui * ui + ui * x3 + x4);
}
END_MGH_PROBLEM;
const double TestProblem15::initial_x[] = {0.25, 0.39, 0.415, 0.39};
const double TestProblem15::lower_bounds[] = {
-kDoubleMax, -kDoubleMax, -kDoubleMax, -kDoubleMax};
const double TestProblem15::upper_bounds[] = {
kDoubleMax, kDoubleMax, kDoubleMax, kDoubleMax};
const double TestProblem15::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem15::unconstrained_optimal_cost = 3.07505e-4;
// Brown and Dennis function.
BEGIN_MGH_PROBLEM(TestProblem16, 4, 20)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T x4 = x[3];
for (int i = 0; i < 20; ++i) {
const T ti = T(static_cast<double>(i + 1) / 5.0);
residual[i] = (x1 + ti * x2 - exp(ti)) * (x1 + ti * x2 - exp(ti)) +
(x3 + x4 * sin(ti) - cos(ti)) * (x3 + x4 * sin(ti) - cos(ti));
}
END_MGH_PROBLEM;
const double TestProblem16::initial_x[] = {25.0, 5.0, -5.0, -1.0};
const double TestProblem16::lower_bounds[] = {-10.0, 0.0, -100.0, -20.0};
const double TestProblem16::upper_bounds[] = {100.0, 15.0, 0.0, 0.2};
const double TestProblem16::constrained_optimal_cost = 0.88860479e5;
const double TestProblem16::unconstrained_optimal_cost = 85822.2;
// Osborne 1 function.
BEGIN_MGH_PROBLEM(TestProblem17, 5, 33)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T x4 = x[3];
const T x5 = x[4];
const double y[] = {0.844, 0.908, 0.932, 0.936, 0.925, 0.908, 0.881, 0.850, 0.818,
0.784, 0.751, 0.718, 0.685, 0.658, 0.628, 0.603, 0.580, 0.558,
0.538, 0.522, 0.506, 0.490, 0.478, 0.467, 0.457, 0.448, 0.438,
0.431, 0.424, 0.420, 0.414, 0.411, 0.406};
for (int i = 0; i < 33; ++i) {
const T yi = T(y[i]);
const T ti = T(10.0 * i);
residual[i] = yi - (x1 + x2 * exp(-ti * x4) + x3 * exp(-ti * x5));
}
END_MGH_PROBLEM;
const double TestProblem17::initial_x[] = {0.5, 1.5, -1.0, 0.01, 0.02};
const double TestProblem17::lower_bounds[] = {
-kDoubleMax, -kDoubleMax, -kDoubleMax, -kDoubleMax};
const double TestProblem17::upper_bounds[] = {
kDoubleMax, kDoubleMax, kDoubleMax, kDoubleMax};
const double TestProblem17::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem17::unconstrained_optimal_cost = 5.46489e-5;
// Biggs EXP6 function.
BEGIN_MGH_PROBLEM(TestProblem18, 6, 13)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T x4 = x[3];
const T x5 = x[4];
const T x6 = x[5];
for (int i = 0; i < 13; ++i) {
const double ti = 0.1 * (i + 1.0);
const double yi = exp(-ti) - 5.0 * exp(-10.0 * ti) + 3.0 * exp(-4.0 * ti);
const T si = T(ti);
residual[i] =x3 * exp(-si * x1) - x4 * exp(-si * x2) + x6 * exp(-si * x5) - T(yi);
}
END_MGH_PROBLEM
const double TestProblem18::initial_x[] = {1.0, 2.0, 1.0, 1.0, 1.0, 1.0};
const double TestProblem18::lower_bounds[] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0};
const double TestProblem18::upper_bounds[] = {2.0, 8.0, 1.0, 7.0, 5.0, 5.0};
const double TestProblem18::constrained_optimal_cost = 0.53209865e-3;
const double TestProblem18::unconstrained_optimal_cost = 0.0;
// Osborne 2 function.
BEGIN_MGH_PROBLEM(TestProblem19, 11, 65)
const T x1 = x[0];
const T x2 = x[1];
const T x3 = x[2];
const T x4 = x[3];
const T x5 = x[4];
const T x6 = x[5];
const T x7 = x[6];
const T x8 = x[7];
const T x9 = x[8];
const T x10 = x[9];
const T x11 = x[10];
const double y[] = {1.366, 1.191, 1.112, 1.013, 0.991,
0.885, 0.831, 0.847, 0.786, 0.725,
0.746, 0.679, 0.608, 0.655, 0.616,
0.606, 0.602, 0.626, 0.651, 0.724,
0.649, 0.649, 0.694, 0.644, 0.624,
0.661, 0.612, 0.558, 0.533, 0.495,
0.500, 0.423, 0.395, 0.375, 0.372,
0.391, 0.396, 0.405, 0.428, 0.429,
0.523, 0.562, 0.607, 0.653, 0.672,
0.708, 0.633, 0.668, 0.645, 0.632,
0.591, 0.559, 0.597, 0.625, 0.739,
0.710, 0.729, 0.720, 0.636, 0.581,
0.428, 0.292, 0.162, 0.098, 0.054};
for (int i = 0; i < 65; ++i) {
const T ti = T(static_cast<double>(i) / 10.0);
residual[i] = T(y[i]) - (x1 * exp(-(ti * x5)) +
x2 * exp(-(ti - x9) * (ti - x9) * x6) +
x3 * exp(-(ti - x10) * (ti - x10) * x7) +
x4 * exp(-(ti - x11) * (ti - x11) * x8));
}
END_MGH_PROBLEM;
const double TestProblem19::initial_x[] = {1.3, 0.65, 0.65, 0.7, 0.6,
3.0, 5.0, 7.0, 2.0, 4.5, 5.5};
const double TestProblem19::lower_bounds[] = {
-kDoubleMax, -kDoubleMax, -kDoubleMax, -kDoubleMax};
const double TestProblem19::upper_bounds[] = {
kDoubleMax, kDoubleMax, kDoubleMax, kDoubleMax};
const double TestProblem19::constrained_optimal_cost =
std::numeric_limits<double>::quiet_NaN();
const double TestProblem19::unconstrained_optimal_cost = 4.01377e-2;
#undef BEGIN_MGH_PROBLEM
#undef END_MGH_PROBLEM
template<typename TestProblem> bool Solve(bool is_constrained, int trial) {
double x[TestProblem::kNumParameters];
for (int i = 0; i < TestProblem::kNumParameters; ++i) {
x[i] = pow(10, trial) * TestProblem::initial_x[i];
}
Problem problem;
problem.AddResidualBlock(TestProblem::Create(), NULL, x);
double optimal_cost = TestProblem::unconstrained_optimal_cost;
if (is_constrained) {
for (int i = 0; i < TestProblem::kNumParameters; ++i) {
problem.SetParameterLowerBound(x, i, TestProblem::lower_bounds[i]);
problem.SetParameterUpperBound(x, i, TestProblem::upper_bounds[i]);
}
optimal_cost = TestProblem::constrained_optimal_cost;
}
Solver::Options options;
options.parameter_tolerance = 1e-18;
options.function_tolerance = 1e-18;
options.gradient_tolerance = 1e-18;
options.max_num_iterations = 1000;
options.linear_solver_type = DENSE_QR;
Solver::Summary summary;
Solve(options, &problem, &summary);
const double kMinLogRelativeError = 4.0;
const double log_relative_error = -std::log10(
std::abs(2.0 * summary.final_cost - optimal_cost) /
(optimal_cost > 0.0 ? optimal_cost : 1.0));
const bool success = log_relative_error >= kMinLogRelativeError;
LOG(INFO) << "Expected : " << optimal_cost
<< " actual: " << 2.0 * summary.final_cost
<< " " << success
<< " in " << summary.total_time_in_seconds
<< " seconds";
return success;
}
} // namespace examples
} // namespace ceres
int main(int argc, char** argv) {
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
using ceres::examples::Solve;
int unconstrained_problems = 0;
int unconstrained_successes = 0;
int constrained_problems = 0;
int constrained_successes = 0;
std::stringstream ss;
#define UNCONSTRAINED_SOLVE(n) \
ss << "Unconstrained Problem " << n << " : "; \
if (FLAGS_problem == #n || FLAGS_problem == "all") { \
unconstrained_problems += 3; \
if (Solve<ceres::examples::TestProblem##n>(false, 0)) { \
unconstrained_successes += 1; \
ss << "Yes "; \
} else { \
ss << "No "; \
} \
if (Solve<ceres::examples::TestProblem##n>(false, 1)) { \
unconstrained_successes += 1; \
ss << "Yes "; \
} else { \
ss << "No "; \
} \
if (Solve<ceres::examples::TestProblem##n>(false, 2)) { \
unconstrained_successes += 1; \
ss << "Yes "; \
} else { \
ss << "No "; \
} \
} \
ss << std::endl;
UNCONSTRAINED_SOLVE(1);
UNCONSTRAINED_SOLVE(2);
UNCONSTRAINED_SOLVE(3);
UNCONSTRAINED_SOLVE(4);
UNCONSTRAINED_SOLVE(5);
UNCONSTRAINED_SOLVE(6);
UNCONSTRAINED_SOLVE(7);
UNCONSTRAINED_SOLVE(8);
UNCONSTRAINED_SOLVE(9);
UNCONSTRAINED_SOLVE(10);
UNCONSTRAINED_SOLVE(11);
UNCONSTRAINED_SOLVE(12);
UNCONSTRAINED_SOLVE(13);
UNCONSTRAINED_SOLVE(14);
UNCONSTRAINED_SOLVE(15);
UNCONSTRAINED_SOLVE(16);
UNCONSTRAINED_SOLVE(17);
UNCONSTRAINED_SOLVE(18);
UNCONSTRAINED_SOLVE(19);
ss << "Unconstrained : "
<< unconstrained_successes
<< "/"
<< unconstrained_problems << std::endl;
#define CONSTRAINED_SOLVE(n) \
ss << "Constrained Problem " << n << " : "; \
if (FLAGS_problem == #n || FLAGS_problem == "all") { \
constrained_problems += 1; \
if (Solve<ceres::examples::TestProblem##n>(true, 0)) { \
constrained_successes += 1; \
ss << "Yes "; \
} else { \
ss << "No "; \
} \
} \
ss << std::endl;
CONSTRAINED_SOLVE(3);
CONSTRAINED_SOLVE(4);
CONSTRAINED_SOLVE(5);
CONSTRAINED_SOLVE(7);
CONSTRAINED_SOLVE(9);
CONSTRAINED_SOLVE(11);
CONSTRAINED_SOLVE(12);
CONSTRAINED_SOLVE(14);
CONSTRAINED_SOLVE(16);
CONSTRAINED_SOLVE(18);
ss << "Constrained : "
<< constrained_successes
<< "/"
<< constrained_problems << std::endl;
std::cout << ss.str();
return 0;
}

629
3rdparty/ceres-solver-1.11.0/examples/nist.cc vendored Normal file
View File

@@ -0,0 +1,629 @@
// 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)
//
// The National Institute of Standards and Technology has released a
// set of problems to test non-linear least squares solvers.
//
// More information about the background on these problems and
// suggested evaluation methodology can be found at:
//
// http://www.itl.nist.gov/div898/strd/nls/nls_info.shtml
//
// The problem data themselves can be found at
//
// http://www.itl.nist.gov/div898/strd/nls/nls_main.shtml
//
// The problems are divided into three levels of difficulty, Easy,
// Medium and Hard. For each problem there are two starting guesses,
// the first one far away from the global minimum and the second
// closer to it.
//
// A problem is considered successfully solved, if every components of
// the solution matches the globally optimal solution in at least 4
// digits or more.
//
// This dataset was used for an evaluation of Non-linear least squares
// solvers:
//
// P. F. Mondragon & B. Borchers, A Comparison of Nonlinear Regression
// Codes, Journal of Modern Applied Statistical Methods, 4(1):343-351,
// 2005.
//
// The results from Mondragon & Borchers can be summarized as
// Excel Gnuplot GaussFit HBN MinPack
// Average LRE 2.3 4.3 4.0 6.8 4.4
// Winner 1 5 12 29 12
//
// Where the row Winner counts, the number of problems for which the
// solver had the highest LRE.
// In this file, we implement the same evaluation methodology using
// Ceres. Currently using Levenberg-Marquardt with DENSE_QR, we get
//
// Excel Gnuplot GaussFit HBN MinPack Ceres
// Average LRE 2.3 4.3 4.0 6.8 4.4 9.4
// Winner 0 0 5 11 2 41
#include <iostream>
#include <iterator>
#include <fstream>
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "Eigen/Core"
DEFINE_string(nist_data_dir, "", "Directory containing the NIST non-linear"
"regression examples");
DEFINE_string(minimizer, "trust_region",
"Minimizer type to use, choices are: line_search & trust_region");
DEFINE_string(trust_region_strategy, "levenberg_marquardt",
"Options are: levenberg_marquardt, dogleg");
DEFINE_string(dogleg, "traditional_dogleg",
"Options are: traditional_dogleg, subspace_dogleg");
DEFINE_string(linear_solver, "dense_qr", "Options are: "
"sparse_cholesky, dense_qr, dense_normal_cholesky and"
"cgnr");
DEFINE_string(preconditioner, "jacobi", "Options are: "
"identity, jacobi");
DEFINE_string(line_search, "wolfe",
"Line search algorithm to use, choices are: armijo and wolfe.");
DEFINE_string(line_search_direction, "lbfgs",
"Line search direction algorithm to use, choices: lbfgs, bfgs");
DEFINE_int32(max_line_search_iterations, 20,
"Maximum number of iterations for each line search.");
DEFINE_int32(max_line_search_restarts, 10,
"Maximum number of restarts of line search direction algorithm.");
DEFINE_string(line_search_interpolation, "cubic",
"Degree of polynomial aproximation in line search, "
"choices are: bisection, quadratic & cubic.");
DEFINE_int32(lbfgs_rank, 20,
"Rank of L-BFGS inverse Hessian approximation in line search.");
DEFINE_bool(approximate_eigenvalue_bfgs_scaling, false,
"Use approximate eigenvalue scaling in (L)BFGS line search.");
DEFINE_double(sufficient_decrease, 1.0e-4,
"Line search Armijo sufficient (function) decrease factor.");
DEFINE_double(sufficient_curvature_decrease, 0.9,
"Line search Wolfe sufficient curvature decrease factor.");
DEFINE_int32(num_iterations, 10000, "Number of iterations");
DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
" nonmonotic steps");
DEFINE_double(initial_trust_region_radius, 1e4, "Initial trust region radius");
DEFINE_bool(use_numeric_diff, false,
"Use numeric differentiation instead of automatic "
"differentiation.");
DEFINE_string(numeric_diff_method, "ridders", "When using numeric "
"differentiation, selects algorithm. Options are: central, "
"forward, ridders.");
DEFINE_double(ridders_step_size, 1e-9, "Initial step size for Ridders "
"numeric differentiation.");
DEFINE_int32(ridders_extrapolations, 3, "Maximal number of Ridders "
"extrapolations.");
namespace ceres {
namespace examples {
using Eigen::Dynamic;
using Eigen::RowMajor;
typedef Eigen::Matrix<double, Dynamic, 1> Vector;
typedef Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> Matrix;
using std::atof;
using std::atoi;
using std::cout;
using std::ifstream;
using std::string;
using std::vector;
void SplitStringUsingChar(const string& full,
const char delim,
vector<string>* result) {
std::back_insert_iterator< vector<string> > it(*result);
const char* p = full.data();
const char* end = p + full.size();
while (p != end) {
if (*p == delim) {
++p;
} else {
const char* start = p;
while (++p != end && *p != delim) {
// Skip to the next occurence of the delimiter.
}
*it++ = string(start, p - start);
}
}
}
bool GetAndSplitLine(ifstream& ifs, vector<string>* pieces) {
pieces->clear();
char buf[256];
ifs.getline(buf, 256);
SplitStringUsingChar(string(buf), ' ', pieces);
return true;
}
void SkipLines(ifstream& ifs, int num_lines) {
char buf[256];
for (int i = 0; i < num_lines; ++i) {
ifs.getline(buf, 256);
}
}
class NISTProblem {
public:
explicit NISTProblem(const string& filename) {
ifstream ifs(filename.c_str(), ifstream::in);
vector<string> pieces;
SkipLines(ifs, 24);
GetAndSplitLine(ifs, &pieces);
const int kNumResponses = atoi(pieces[1].c_str());
GetAndSplitLine(ifs, &pieces);
const int kNumPredictors = atoi(pieces[0].c_str());
GetAndSplitLine(ifs, &pieces);
const int kNumObservations = atoi(pieces[0].c_str());
SkipLines(ifs, 4);
GetAndSplitLine(ifs, &pieces);
const int kNumParameters = atoi(pieces[0].c_str());
SkipLines(ifs, 8);
// Get the first line of initial and final parameter values to
// determine the number of tries.
GetAndSplitLine(ifs, &pieces);
const int kNumTries = pieces.size() - 4;
predictor_.resize(kNumObservations, kNumPredictors);
response_.resize(kNumObservations, kNumResponses);
initial_parameters_.resize(kNumTries, kNumParameters);
final_parameters_.resize(1, kNumParameters);
// 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());
}
final_parameters_(0, parameter_id) = 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());
}
final_parameters_(0, parameter_id) = atof(pieces[2 + kNumTries].c_str());
}
// Certfied cost
SkipLines(ifs, 1);
GetAndSplitLine(ifs, &pieces);
certified_cost_ = atof(pieces[4].c_str()) / 2.0;
// Read the observations.
SkipLines(ifs, 18 - kNumParameters);
for (int i = 0; i < kNumObservations; ++i) {
GetAndSplitLine(ifs, &pieces);
// Response.
for (int j = 0; j < kNumResponses; ++j) {
response_(i, j) = atof(pieces[j].c_str());
}
// Predictor variables.
for (int j = 0; j < kNumPredictors; ++j) {
predictor_(i, j) = atof(pieces[j + kNumResponses].c_str());
}
}
}
Matrix initial_parameters(int start) const { return initial_parameters_.row(start); } // NOLINT
Matrix final_parameters() const { return final_parameters_; }
Matrix predictor() const { return predictor_; }
Matrix response() const { return response_; }
int predictor_size() const { return predictor_.cols(); }
int num_observations() const { return predictor_.rows(); }
int response_size() const { return response_.cols(); }
int num_parameters() const { return initial_parameters_.cols(); }
int num_starts() const { return initial_parameters_.rows(); }
double certified_cost() const { return certified_cost_; }
private:
Matrix predictor_;
Matrix response_;
Matrix initial_parameters_;
Matrix final_parameters_;
double certified_cost_;
};
#define NIST_BEGIN(CostFunctionName) \
struct CostFunctionName { \
CostFunctionName(const double* const x, \
const double* const y) \
: x_(*x), y_(*y) {} \
double x_; \
double y_; \
template <typename T> \
bool operator()(const T* const b, T* residual) const { \
const T y(y_); \
const T x(x_); \
residual[0] = y - (
#define NIST_END ); return true; }};
// y = b1 * (b2+x)**(-1/b3) + e
NIST_BEGIN(Bennet5)
b[0] * pow(b[1] + x, T(-1.0) / b[2])
NIST_END
// y = b1*(1-exp[-b2*x]) + e
NIST_BEGIN(BoxBOD)
b[0] * (T(1.0) - exp(-b[1] * x))
NIST_END
// y = exp[-b1*x]/(b2+b3*x) + e
NIST_BEGIN(Chwirut)
exp(-b[0] * x) / (b[1] + b[2] * x)
NIST_END
// y = b1*x**b2 + e
NIST_BEGIN(DanWood)
b[0] * pow(x, b[1])
NIST_END
// y = b1*exp( -b2*x ) + b3*exp( -(x-b4)**2 / b5**2 )
// + b6*exp( -(x-b7)**2 / b8**2 ) + e
NIST_BEGIN(Gauss)
b[0] * exp(-b[1] * x) +
b[2] * exp(-pow((x - b[3])/b[4], 2)) +
b[5] * exp(-pow((x - b[6])/b[7], 2))
NIST_END
// y = b1*exp(-b2*x) + b3*exp(-b4*x) + b5*exp(-b6*x) + e
NIST_BEGIN(Lanczos)
b[0] * exp(-b[1] * x) + b[2] * exp(-b[3] * x) + b[4] * exp(-b[5] * x)
NIST_END
// y = (b1+b2*x+b3*x**2+b4*x**3) /
// (1+b5*x+b6*x**2+b7*x**3) + e
NIST_BEGIN(Hahn1)
(b[0] + b[1] * x + b[2] * x * x + b[3] * x * x * x) /
(T(1.0) + b[4] * x + b[5] * x * x + b[6] * x * x * x)
NIST_END
// y = (b1 + b2*x + b3*x**2) /
// (1 + b4*x + b5*x**2) + e
NIST_BEGIN(Kirby2)
(b[0] + b[1] * x + b[2] * x * x) /
(T(1.0) + b[3] * x + b[4] * x * x)
NIST_END
// y = b1*(x**2+x*b2) / (x**2+x*b3+b4) + e
NIST_BEGIN(MGH09)
b[0] * (x * x + x * b[1]) / (x * x + x * b[2] + b[3])
NIST_END
// y = b1 * exp[b2/(x+b3)] + e
NIST_BEGIN(MGH10)
b[0] * exp(b[1] / (x + b[2]))
NIST_END
// y = b1 + b2*exp[-x*b4] + b3*exp[-x*b5]
NIST_BEGIN(MGH17)
b[0] + b[1] * exp(-x * b[3]) + b[2] * exp(-x * b[4])
NIST_END
// y = b1*(1-exp[-b2*x]) + e
NIST_BEGIN(Misra1a)
b[0] * (T(1.0) - exp(-b[1] * x))
NIST_END
// y = b1 * (1-(1+b2*x/2)**(-2)) + e
NIST_BEGIN(Misra1b)
b[0] * (T(1.0) - T(1.0)/ ((T(1.0) + b[1] * x / 2.0) * (T(1.0) + b[1] * x / 2.0))) // NOLINT
NIST_END
// y = b1 * (1-(1+2*b2*x)**(-.5)) + e
NIST_BEGIN(Misra1c)
b[0] * (T(1.0) - pow(T(1.0) + T(2.0) * b[1] * x, -0.5))
NIST_END
// y = b1*b2*x*((1+b2*x)**(-1)) + e
NIST_BEGIN(Misra1d)
b[0] * b[1] * x / (T(1.0) + b[1] * x)
NIST_END
const double kPi = 3.141592653589793238462643383279;
// pi = 3.141592653589793238462643383279E0
// y = b1 - b2*x - arctan[b3/(x-b4)]/pi + e
NIST_BEGIN(Roszman1)
b[0] - b[1] * x - atan2(b[2], (x - b[3]))/T(kPi)
NIST_END
// y = b1 / (1+exp[b2-b3*x]) + e
NIST_BEGIN(Rat42)
b[0] / (T(1.0) + exp(b[1] - b[2] * x))
NIST_END
// y = b1 / ((1+exp[b2-b3*x])**(1/b4)) + e
NIST_BEGIN(Rat43)
b[0] / pow(T(1.0) + exp(b[1] - b[2] * x), T(1.0) / b[3])
NIST_END
// y = (b1 + b2*x + b3*x**2 + b4*x**3) /
// (1 + b5*x + b6*x**2 + b7*x**3) + e
NIST_BEGIN(Thurber)
(b[0] + b[1] * x + b[2] * x * x + b[3] * x * x * x) /
(T(1.0) + b[4] * x + b[5] * x * x + b[6] * x * x * x)
NIST_END
// y = b1 + b2*cos( 2*pi*x/12 ) + b3*sin( 2*pi*x/12 )
// + b5*cos( 2*pi*x/b4 ) + b6*sin( 2*pi*x/b4 )
// + b8*cos( 2*pi*x/b7 ) + b9*sin( 2*pi*x/b7 ) + e
NIST_BEGIN(ENSO)
b[0] + b[1] * cos(T(2.0 * kPi) * x / T(12.0)) +
b[2] * sin(T(2.0 * kPi) * x / T(12.0)) +
b[4] * cos(T(2.0 * kPi) * x / b[3]) +
b[5] * sin(T(2.0 * kPi) * x / b[3]) +
b[7] * cos(T(2.0 * kPi) * x / b[6]) +
b[8] * sin(T(2.0 * kPi) * x / b[6])
NIST_END
// y = (b1/b2) * exp[-0.5*((x-b3)/b2)**2] + e
NIST_BEGIN(Eckerle4)
b[0] / b[1] * exp(T(-0.5) * pow((x - b[2])/b[1], 2))
NIST_END
struct Nelson {
public:
Nelson(const double* const x, const double* const y)
: x1_(x[0]), x2_(x[1]), y_(y[0]) {}
template <typename T>
bool operator()(const T* const b, T* residual) const {
// log[y] = b1 - b2*x1 * exp[-b3*x2] + e
residual[0] = T(log(y_)) - (b[0] - b[1] * T(x1_) * exp(-b[2] * T(x2_)));
return true;
}
private:
double x1_;
double x2_;
double y_;
};
static void SetNumericDiffOptions(ceres::NumericDiffOptions* options) {
options->max_num_ridders_extrapolations = FLAGS_ridders_extrapolations;
options->ridders_relative_initial_step_size = FLAGS_ridders_step_size;
}
template <typename Model, int num_residuals, int num_parameters>
int RegressionDriver(const string& filename,
const ceres::Solver::Options& options) {
NISTProblem nist_problem(FLAGS_nist_data_dir + filename);
CHECK_EQ(num_residuals, nist_problem.response_size());
CHECK_EQ(num_parameters, nist_problem.num_parameters());
Matrix predictor = nist_problem.predictor();
Matrix response = nist_problem.response();
Matrix final_parameters = nist_problem.final_parameters();
printf("%s\n", filename.c_str());
// Each NIST problem comes with multiple starting points, so we
// construct the problem from scratch for each case and solve it.
int num_success = 0;
for (int start = 0; start < nist_problem.num_starts(); ++start) {
Matrix initial_parameters = nist_problem.initial_parameters(start);
ceres::Problem problem;
for (int i = 0; i < nist_problem.num_observations(); ++i) {
Model* model = new Model(
predictor.data() + nist_problem.predictor_size() * i,
response.data() + nist_problem.response_size() * i);
ceres::CostFunction* cost_function = NULL;
if (FLAGS_use_numeric_diff) {
ceres::NumericDiffOptions options;
SetNumericDiffOptions(&options);
if (FLAGS_numeric_diff_method == "central") {
cost_function = new NumericDiffCostFunction<Model,
ceres::CENTRAL,
num_residuals,
num_parameters>(
model, ceres::TAKE_OWNERSHIP, num_residuals, options);
} else if (FLAGS_numeric_diff_method == "forward") {
cost_function = new NumericDiffCostFunction<Model,
ceres::FORWARD,
num_residuals,
num_parameters>(
model, ceres::TAKE_OWNERSHIP, num_residuals, options);
} else if (FLAGS_numeric_diff_method == "ridders") {
cost_function = new NumericDiffCostFunction<Model,
ceres::RIDDERS,
num_residuals,
num_parameters>(
model, ceres::TAKE_OWNERSHIP, num_residuals, options);
} else {
LOG(ERROR) << "Invalid numeric diff method specified";
return 0;
}
} else {
cost_function =
new ceres::AutoDiffCostFunction<Model,
num_residuals,
num_parameters>(model);
}
problem.AddResidualBlock(cost_function,
NULL,
initial_parameters.data());
}
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
// Compute the LRE by comparing each component of the solution
// with the ground truth, and taking the minimum.
Matrix final_parameters = nist_problem.final_parameters();
const double kMaxNumSignificantDigits = 11;
double log_relative_error = kMaxNumSignificantDigits + 1;
for (int i = 0; i < num_parameters; ++i) {
const double tmp_lre =
-std::log10(std::fabs(final_parameters(i) - initial_parameters(i)) /
std::fabs(final_parameters(i)));
// The maximum LRE is capped at 11 - the precision at which the
// ground truth is known.
//
// The minimum LRE is capped at 0 - no digits match between the
// computed solution and the ground truth.
log_relative_error =
std::min(log_relative_error,
std::max(0.0, std::min(kMaxNumSignificantDigits, tmp_lre)));
}
const int kMinNumMatchingDigits = 4;
if (log_relative_error >= kMinNumMatchingDigits) {
++num_success;
}
printf("start: %d status: %s lre: %4.1f initial cost: %e final cost:%e "
"certified cost: %e total iterations: %d\n",
start + 1,
log_relative_error < kMinNumMatchingDigits ? "FAILURE" : "SUCCESS",
log_relative_error,
summary.initial_cost,
summary.final_cost,
nist_problem.certified_cost(),
(summary.num_successful_steps + summary.num_unsuccessful_steps));
}
return num_success;
}
void SetMinimizerOptions(ceres::Solver::Options* options) {
CHECK(ceres::StringToMinimizerType(FLAGS_minimizer,
&options->minimizer_type));
CHECK(ceres::StringToLinearSolverType(FLAGS_linear_solver,
&options->linear_solver_type));
CHECK(ceres::StringToPreconditionerType(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));
CHECK(ceres::StringToLineSearchDirectionType(
FLAGS_line_search_direction,
&options->line_search_direction_type));
CHECK(ceres::StringToLineSearchType(FLAGS_line_search,
&options->line_search_type));
CHECK(ceres::StringToLineSearchInterpolationType(
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->line_search_sufficient_curvature_decrease =
FLAGS_sufficient_curvature_decrease;
options->max_num_line_search_step_size_iterations =
FLAGS_max_line_search_iterations;
options->max_num_line_search_direction_restarts =
FLAGS_max_line_search_restarts;
options->use_approximate_eigenvalue_bfgs_scaling =
FLAGS_approximate_eigenvalue_bfgs_scaling;
options->function_tolerance = 1e-18;
options->gradient_tolerance = 1e-18;
options->parameter_tolerance = 1e-18;
}
void SolveNISTProblems() {
if (FLAGS_nist_data_dir.empty()) {
LOG(FATAL) << "Must specify the directory containing the NIST problems";
}
ceres::Solver::Options options;
SetMinimizerOptions(&options);
cout << "Lower Difficulty\n";
int easy_success = 0;
easy_success += RegressionDriver<Misra1a, 1, 2>("Misra1a.dat", options);
easy_success += RegressionDriver<Chwirut, 1, 3>("Chwirut1.dat", options);
easy_success += RegressionDriver<Chwirut, 1, 3>("Chwirut2.dat", options);
easy_success += RegressionDriver<Lanczos, 1, 6>("Lanczos3.dat", options);
easy_success += RegressionDriver<Gauss, 1, 8>("Gauss1.dat", options);
easy_success += RegressionDriver<Gauss, 1, 8>("Gauss2.dat", options);
easy_success += RegressionDriver<DanWood, 1, 2>("DanWood.dat", options);
easy_success += RegressionDriver<Misra1b, 1, 2>("Misra1b.dat", options);
cout << "\nMedium Difficulty\n";
int medium_success = 0;
medium_success += RegressionDriver<Kirby2, 1, 5>("Kirby2.dat", options);
medium_success += RegressionDriver<Hahn1, 1, 7>("Hahn1.dat", options);
medium_success += RegressionDriver<Nelson, 1, 3>("Nelson.dat", options);
medium_success += RegressionDriver<MGH17, 1, 5>("MGH17.dat", options);
medium_success += RegressionDriver<Lanczos, 1, 6>("Lanczos1.dat", options);
medium_success += RegressionDriver<Lanczos, 1, 6>("Lanczos2.dat", options);
medium_success += RegressionDriver<Gauss, 1, 8>("Gauss3.dat", options);
medium_success += RegressionDriver<Misra1c, 1, 2>("Misra1c.dat", options);
medium_success += RegressionDriver<Misra1d, 1, 2>("Misra1d.dat", options);
medium_success += RegressionDriver<Roszman1, 1, 4>("Roszman1.dat", options);
medium_success += RegressionDriver<ENSO, 1, 9>("ENSO.dat", options);
cout << "\nHigher Difficulty\n";
int hard_success = 0;
hard_success += RegressionDriver<MGH09, 1, 4>("MGH09.dat", options);
hard_success += RegressionDriver<Thurber, 1, 7>("Thurber.dat", options);
hard_success += RegressionDriver<BoxBOD, 1, 2>("BoxBOD.dat", options);
hard_success += RegressionDriver<Rat42, 1, 3>("Rat42.dat", options);
hard_success += RegressionDriver<MGH10, 1, 3>("MGH10.dat", options);
hard_success += RegressionDriver<Eckerle4, 1, 3>("Eckerle4.dat", options);
hard_success += RegressionDriver<Rat43, 1, 4>("Rat43.dat", options);
hard_success += RegressionDriver<Bennet5, 1, 3>("Bennett5.dat", options);
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";
}
} // namespace examples
} // namespace ceres
int main(int argc, char** argv) {
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
ceres::examples::SolveNISTProblems();
return 0;
}

319
3rdparty/ceres-solver-1.11.0/examples/pgm_image.h vendored Normal file
View File

@@ -0,0 +1,319 @@
// 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: strandmark@google.com (Petter Strandmark)
//
// Simple class for accessing PGM images.
#ifndef CERES_EXAMPLES_PGM_IMAGE_H_
#define CERES_EXAMPLES_PGM_IMAGE_H_
#include <algorithm>
#include <cstring>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include "glog/logging.h"
namespace ceres {
namespace examples {
template<typename Real>
class PGMImage {
public:
// Create an empty image
PGMImage(int width, int height);
// Load an image from file
explicit PGMImage(std::string filename);
// Sets an image to a constant
void Set(double constant);
// Reading dimensions
int width() const;
int height() const;
int NumPixels() const;
// Get individual pixels
Real* MutablePixel(int x, int y);
Real Pixel(int x, int y) const;
Real* MutablePixelFromLinearIndex(int index);
Real PixelFromLinearIndex(int index) const;
int LinearIndex(int x, int y) const;
// Adds an image to another
void operator+=(const PGMImage& image);
// Adds a constant to an image
void operator+=(Real a);
// Multiplies an image by a constant
void operator*=(Real a);
// File access
bool WriteToFile(std::string filename) const;
bool ReadFromFile(std::string filename);
// Accessing the image data directly
bool SetData(const std::vector<Real>& new_data);
const std::vector<Real>& data() const;
protected:
int height_, width_;
std::vector<Real> data_;
};
// --- IMPLEMENTATION
template<typename Real>
PGMImage<Real>::PGMImage(int width, int height)
: height_(height), width_(width), data_(width*height, 0.0) {
}
template<typename Real>
PGMImage<Real>::PGMImage(std::string filename) {
if (!ReadFromFile(filename)) {
height_ = 0;
width_ = 0;
}
}
template<typename Real>
void PGMImage<Real>::Set(double constant) {
for (int i = 0; i < data_.size(); ++i) {
data_[i] = constant;
}
}
template<typename Real>
int PGMImage<Real>::width() const {
return width_;
}
template<typename Real>
int PGMImage<Real>::height() const {
return height_;
}
template<typename Real>
int PGMImage<Real>::NumPixels() const {
return width_ * height_;
}
template<typename Real>
Real* PGMImage<Real>::MutablePixel(int x, int y) {
return MutablePixelFromLinearIndex(LinearIndex(x, y));
}
template<typename Real>
Real PGMImage<Real>::Pixel(int x, int y) const {
return PixelFromLinearIndex(LinearIndex(x, y));
}
template<typename Real>
Real* PGMImage<Real>::MutablePixelFromLinearIndex(int index) {
CHECK(index >= 0);
CHECK(index < width_ * height_);
CHECK(index < data_.size());
return &data_[index];
}
template<typename Real>
Real PGMImage<Real>::PixelFromLinearIndex(int index) const {
CHECK(index >= 0);
CHECK(index < width_ * height_);
CHECK(index < data_.size());
return data_[index];
}
template<typename Real>
int PGMImage<Real>::LinearIndex(int x, int y) const {
return x + width_*y;
}
// Adds an image to another
template<typename Real>
void PGMImage<Real>::operator+= (const PGMImage<Real>& image) {
CHECK(data_.size() == image.data_.size());
for (int i = 0; i < data_.size(); ++i) {
data_[i] += image.data_[i];
}
}
// Adds a constant to an image
template<typename Real>
void PGMImage<Real>::operator+= (Real a) {
for (int i = 0; i < data_.size(); ++i) {
data_[i] += a;
}
}
// Multiplies an image by a constant
template<typename Real>
void PGMImage<Real>::operator*= (Real a) {
for (int i = 0; i < data_.size(); ++i) {
data_[i] *= a;
}
}
template<typename Real>
bool PGMImage<Real>::WriteToFile(std::string filename) const {
std::ofstream outputfile(filename.c_str());
outputfile << "P2" << std::endl;
outputfile << "# PGM format" << std::endl;
outputfile << " # <width> <height> <levels> " << std::endl;
outputfile << " # <data> ... " << std::endl;
outputfile << width_ << ' ' << height_ << " 255 " << std::endl;
// Write data
int num_pixels = width_*height_;
for (int i = 0; i < num_pixels; ++i) {
// Convert to integer by rounding when writing file
outputfile << static_cast<int>(data_[i] + 0.5) << ' ';
}
return bool(outputfile); // Returns true/false
}
namespace {
// Helper function to read data from a text file, ignoring "#" comments.
template<typename T>
bool GetIgnoreComment(std::istream* in, T& t) {
std::string word;
bool ok;
do {
ok = true;
(*in) >> word;
if (word.length() > 0 && word[0] == '#') {
// Comment; read the whole line
ok = false;
std::getline(*in, word);
}
} while (!ok);
// Convert the string
std::stringstream sin(word);
sin >> t;
// Check for success
if (!in || !sin) {
return false;
}
return true;
}
} // namespace
template<typename Real>
bool PGMImage<Real>::ReadFromFile(std::string filename) {
std::ifstream inputfile(filename.c_str());
// File must start with "P2"
char ch1, ch2;
inputfile >> ch1 >> ch2;
if (!inputfile || ch1 != 'P' || (ch2 != '2' && ch2 != '5')) {
return false;
}
// Read the image header
int two_fifty_five;
if (!GetIgnoreComment(&inputfile, width_) ||
!GetIgnoreComment(&inputfile, height_) ||
!GetIgnoreComment(&inputfile, two_fifty_five) ) {
return false;
}
// Assert that the number of grey levels is 255.
if (two_fifty_five != 255) {
return false;
}
// Now read the data
int num_pixels = width_*height_;
data_.resize(num_pixels);
if (ch2 == '2') {
// Ascii file
for (int i = 0; i < num_pixels; ++i) {
int pixel_data;
bool res = GetIgnoreComment(&inputfile, pixel_data);
if (!res) {
return false;
}
data_[i] = pixel_data;
}
// There cannot be anything else in the file (except comments). Try reading
// another number and return failure if that succeeded.
int temp;
bool res = GetIgnoreComment(&inputfile, temp);
if (res) {
return false;
}
} else {
// Read the line feed character
if (inputfile.get() != '\n') {
return false;
}
// Binary file
// TODO(strandmark): Will not work on Windows (linebreak conversion).
for (int i = 0; i < num_pixels; ++i) {
unsigned char pixel_data = inputfile.get();
if (!inputfile) {
return false;
}
data_[i] = pixel_data;
}
// There cannot be anything else in the file. Try reading another byte
// and return failure if that succeeded.
inputfile.get();
if (inputfile) {
return false;
}
}
return true;
}
template<typename Real>
bool PGMImage<Real>::SetData(const std::vector<Real>& new_data) {
// This function cannot change the dimensions
if (new_data.size() != data_.size()) {
return false;
}
std::copy(new_data.begin(), new_data.end(), data_.begin());
return true;
}
template<typename Real>
const std::vector<Real>& PGMImage<Real>::data() const {
return data_;
}
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_PGM_IMAGE_H_

154
3rdparty/ceres-solver-1.11.0/examples/powell.cc vendored Normal file
View File

@@ -0,0 +1,154 @@
// 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)
//
// An example program that minimizes Powell's singular function.
//
// F = 1/2 (f1^2 + f2^2 + f3^2 + f4^2)
//
// f1 = x1 + 10*x2;
// f2 = sqrt(5) * (x3 - x4)
// f3 = (x2 - 2*x3)^2
// f4 = sqrt(10) * (x1 - x4)^2
//
// The starting values are x1 = 3, x2 = -1, x3 = 0, x4 = 1.
// The minimum is 0 at (x1, x2, x3, x4) = 0.
//
// From: Testing Unconstrained Optimization Software by Jorge J. More, Burton S.
// Garbow and Kenneth E. Hillstrom in ACM Transactions on Mathematical Software,
// Vol 7(1), March 1981.
#include <vector>
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
struct F1 {
template <typename T> bool operator()(const T* const x1,
const T* const x2,
T* residual) const {
// f1 = x1 + 10 * x2;
residual[0] = x1[0] + T(10.0) * x2[0];
return true;
}
};
struct F2 {
template <typename T> bool operator()(const T* const x3,
const T* const x4,
T* residual) const {
// f2 = sqrt(5) (x3 - x4)
residual[0] = T(sqrt(5.0)) * (x3[0] - x4[0]);
return true;
}
};
struct F3 {
template <typename T> bool operator()(const T* const x2,
const T* const x4,
T* residual) const {
// f3 = (x2 - 2 x3)^2
residual[0] = (x2[0] - T(2.0) * x4[0]) * (x2[0] - T(2.0) * x4[0]);
return true;
}
};
struct F4 {
template <typename T> bool operator()(const T* const x1,
const T* const x4,
T* residual) const {
// f4 = sqrt(10) (x1 - x4)^2
residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
return true;
}
};
DEFINE_string(minimizer, "trust_region",
"Minimizer type to use, choices are: line_search & trust_region");
int main(int argc, char** argv) {
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
double x1 = 3.0;
double x2 = -1.0;
double x3 = 0.0;
double x4 = 1.0;
Problem problem;
// Add residual terms to the problem using the 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);
problem.AddResidualBlock(new AutoDiffCostFunction<F2, 1, 1, 1>(new F2),
NULL,
&x3, &x4);
problem.AddResidualBlock(new AutoDiffCostFunction<F3, 1, 1, 1>(new F3),
NULL,
&x2, &x3);
problem.AddResidualBlock(new AutoDiffCostFunction<F4, 1, 1, 1>(new F4),
NULL,
&x1, &x4);
Solver::Options options;
LOG_IF(FATAL, !ceres::StringToMinimizerType(FLAGS_minimizer,
&options.minimizer_type))
<< "Invalid minimizer: " << FLAGS_minimizer
<< ", valid options are: trust_region and line_search.";
options.max_num_iterations = 100;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
std::cout << "Initial x1 = " << x1
<< ", x2 = " << x2
<< ", x3 = " << x3
<< ", x4 = " << x4
<< "\n";
// Run the solver!
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
std::cout << "Final x1 = " << x1
<< ", x2 = " << x2
<< ", x3 = " << x3
<< ", x4 = " << x4
<< "\n";
return 0;
}

64
3rdparty/ceres-solver-1.11.0/examples/random.h vendored Normal file
View File

@@ -0,0 +1,64 @@
// 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_

316
3rdparty/ceres-solver-1.11.0/examples/robot_pose_mle.cc vendored Normal file
View File

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

165
3rdparty/ceres-solver-1.11.0/examples/robust_curve_fitting.cc vendored Normal file
View File

@@ -0,0 +1,165 @@
// 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)
#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;
// outlier_noise = rand(size(x)) < 0.05;
// y_observed = y + noise + outlier_noise;
// data = [x', y_observed'];
const int kNumObservations = 67;
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, 5.543078e+00, // Outlier point
1.500000e+00, 5.664015e+00, // Outlier point
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
};
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::CauchyLoss;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
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] = T(y_) - exp(m[0] * T(x_) + c[0]);
return true;
}
private:
const double x_;
const double y_;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
double m = 0.0;
double c = 0.0;
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);
}
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n";
std::cout << "Final m: " << m << " c: " << c << "\n";
return 0;
}

74
3rdparty/ceres-solver-1.11.0/examples/rosenbrock.cc vendored Normal file
View File

@@ -0,0 +1,74 @@
// 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)
#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];
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; }
};
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;
}

94
3rdparty/ceres-solver-1.11.0/examples/sampled_function.cc vendored Normal file
View File

@@ -0,0 +1,94 @@
// 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)
//
// A simple example of optimizing a sampled function by using cubic
// interpolation.
#include "ceres/ceres.h"
#include "ceres/cubic_interpolation.h"
#include "glog/logging.h"
using ceres::Grid1D;
using ceres::CubicInterpolator;
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
// 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) {
}
template<typename T> bool operator()(const T* x, T* residuals) const {
interpolator_.Evaluate(*x, residuals);
return true;
}
static CostFunction* Create(
const CubicInterpolator<Grid1D<double> >& interpolator) {
return new AutoDiffCostFunction<InterpolatedCostFunctor, 1, 1>(
new InterpolatedCostFunctor(interpolator));
}
private:
const CubicInterpolator<Grid1D<double> >& interpolator_;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// Evaluate the function f(x) = (x - 4.5)^2;
const int kNumSamples = 10;
double values[kNumSamples];
for (int i = 0; i < kNumSamples; ++i) {
values[i] = (i - 4.5) * (i - 4.5);
}
Grid1D<double> array(values, 0, kNumSamples);
CubicInterpolator<Grid1D<double> > interpolator(array);
double x = 1.0;
Problem problem;
CostFunction* cost_function = InterpolatedCostFunctor::Create(interpolator);
problem.AddResidualBlock(cost_function, NULL, &x);
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "Expected x: 4.5. Actual x : " << x << std::endl;
return 0;
}

218
3rdparty/ceres-solver-1.11.0/examples/simple_bundle_adjuster.cc vendored Normal file
View File

@@ -0,0 +1,218 @@
// 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)
//
// A minimal, self-contained bundle adjuster using Ceres, that reads
// files from University of Washington' Bundle Adjustment in the Large dataset:
// http://grail.cs.washington.edu/projects/bal
//
// This does not use the best configuration for solving; see the more involved
// bundle_adjuster.cc file for details.
#include <cmath>
#include <cstdio>
#include <iostream>
#include "ceres/ceres.h"
#include "ceres/rotation.h"
// Read a Bundle Adjustment in the Large dataset.
class BALProblem {
public:
~BALProblem() {
delete[] point_index_;
delete[] camera_index_;
delete[] observations_;
delete[] parameters_;
}
int num_observations() const { return num_observations_; }
const double* observations() const { return observations_; }
double* mutable_cameras() { return parameters_; }
double* mutable_points() { return parameters_ + 9 * num_cameras_; }
double* mutable_camera_for_observation(int i) {
return mutable_cameras() + camera_index_[i] * 9;
}
double* mutable_point_for_observation(int i) {
return mutable_points() + point_index_[i] * 3;
}
bool LoadFile(const char* filename) {
FILE* fptr = fopen(filename, "r");
if (fptr == NULL) {
return false;
};
FscanfOrDie(fptr, "%d", &num_cameras_);
FscanfOrDie(fptr, "%d", &num_points_);
FscanfOrDie(fptr, "%d", &num_observations_);
point_index_ = new int[num_observations_];
camera_index_ = new int[num_observations_];
observations_ = new double[2 * num_observations_];
num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
parameters_ = new double[num_parameters_];
for (int i = 0; i < num_observations_; ++i) {
FscanfOrDie(fptr, "%d", camera_index_ + i);
FscanfOrDie(fptr, "%d", point_index_ + i);
for (int j = 0; j < 2; ++j) {
FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
}
}
for (int i = 0; i < num_parameters_; ++i) {
FscanfOrDie(fptr, "%lf", parameters_ + i);
}
return true;
}
private:
template<typename T>
void FscanfOrDie(FILE *fptr, const char *format, T *value) {
int num_scanned = fscanf(fptr, format, value);
if (num_scanned != 1) {
LOG(FATAL) << "Invalid UW data file.";
}
}
int num_cameras_;
int num_points_;
int num_observations_;
int num_parameters_;
int* point_index_;
int* camera_index_;
double* observations_;
double* parameters_;
};
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
// Factory to hide the construction of the CostFunction object from
// 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)));
}
double observed_x;
double observed_y;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
if (argc != 2) {
std::cerr << "usage: simple_bundle_adjuster <bal_problem>\n";
return 1;
}
BALProblem bal_problem;
if (!bal_problem.LoadFile(argv[1])) {
std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
return 1;
}
const double* observations = bal_problem.observations();
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(observations[2 * i + 0],
observations[2 * i + 1]);
problem.AddResidualBlock(cost_function,
NULL /* squared loss */,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
}
// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
return 0;
}

174
3rdparty/ceres-solver-1.11.0/examples/snavely_reprojection_error.h vendored Normal file
View File

@@ -0,0 +1,174 @@
// 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)
//
// Templated struct implementing the camera model and residual
// computation for bundle adjustment used by Noah Snavely's Bundler
// SfM system. This is also the camera model/residual for the bundle
// adjustment problems in the BAL dataset. It is templated so that we
// can use Ceres's automatic differentiation to compute analytic
// jacobians.
//
// For details see: http://phototour.cs.washington.edu/bundler/
// and http://grail.cs.washington.edu/projects/bal/
#ifndef CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
#define CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
#include "ceres/rotation.h"
namespace ceres {
namespace examples {
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
const T& focal = camera[6];
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
// Factory to hide the construction of the CostFunction object from
// 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)));
}
double observed_x;
double observed_y;
};
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 10 parameters. 4 for rotation, 3 for
// translation, 1 for focal length and 2 for radial distortion. The
// principal point is not modeled (i.e. it is assumed be located at
// the image center).
struct SnavelyReprojectionErrorWithQuaternions {
// (u, v): the position of the observation with respect to the image
// center point.
SnavelyReprojectionErrorWithQuaternions(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T* const camera_rotation,
const T* const camera_translation_and_intrinsics,
const T* const point,
T* residuals) const {
const T& focal = camera_translation_and_intrinsics[3];
const T& l1 = camera_translation_and_intrinsics[4];
const T& l2 = camera_translation_and_intrinsics[5];
// Use a quaternion rotation that doesn't assume the quaternion is
// normalized, since one of the ways to run the bundler is to let Ceres
// optimize all 4 quaternion parameters unconstrained.
T p[3];
QuaternionRotatePoint(camera_rotation, point, p);
p[0] += camera_translation_and_intrinsics[0];
p[1] += camera_translation_and_intrinsics[1];
p[2] += camera_translation_and_intrinsics[2];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
T r2 = xp*xp + yp*yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
return (new ceres::AutoDiffCostFunction<
SnavelyReprojectionErrorWithQuaternions, 2, 4, 6, 3>(
new SnavelyReprojectionErrorWithQuaternions(observed_x,
observed_y)));
}
double observed_x;
double observed_y;
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_