Update ceres to the latest version in g3
Please pay special attention to the changes in Android.mk.
They are the only real changes I had to make.
Bug: 16953678
Change-Id: I44a644358e779aaff99a2ea822387fe49ac26888
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 610e816..4d4f873 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -34,8 +34,8 @@
block_evaluate_preparer.cc
block_jacobi_preconditioner.cc
block_jacobian_writer.cc
- block_random_access_crs_matrix.cc
block_random_access_dense_matrix.cc
+ block_random_access_diagonal_matrix.cc
block_random_access_matrix.cc
block_random_access_sparse_matrix.cc
block_sparse_matrix.cc
@@ -43,6 +43,7 @@
c_api.cc
canonical_views_clustering.cc
cgnr_solver.cc
+ callbacks.cc
compressed_col_sparse_matrix_utils.cc
compressed_row_jacobian_writer.cc
compressed_row_sparse_matrix.cc
@@ -58,6 +59,8 @@
dense_sparse_matrix.cc
detect_structure.cc
dogleg_strategy.cc
+ dynamic_compressed_row_jacobian_writer.cc
+ dynamic_compressed_row_sparse_matrix.cc
evaluator.cc
file.cc
gradient_checking_cost_function.cc
@@ -84,13 +87,14 @@
problem.cc
problem_impl.cc
program.cc
+ reorder_program.cc
residual_block.cc
residual_block_utils.cc
- runtime_numeric_diff_cost_function.cc
schur_complement_solver.cc
schur_eliminator.cc
schur_jacobi_preconditioner.cc
scratch_evaluate_preparer.cc
+ single_linkage_clustering.cc
solver.cc
solver_impl.cc
sparse_matrix.cc
@@ -98,6 +102,7 @@
split.cc
stringprintf.cc
suitesparse.cc
+ summary_utils.cc
triplet_sparse_matrix.cc
trust_region_minimizer.cc
trust_region_strategy.cc
@@ -128,62 +133,31 @@
FILE(GLOB CERES_INTERNAL_SCHUR_FILES generated/*.cc)
ELSE (SCHUR_SPECIALIZATIONS)
# Only the fully dynamic solver. The build is much faster this way.
- FILE(GLOB CERES_INTERNAL_SCHUR_FILES generated/schur_eliminator_d_d_d.cc)
+ FILE(GLOB CERES_INTERNAL_SCHUR_FILES generated/*_d_d_d.cc)
ENDIF (SCHUR_SPECIALIZATIONS)
-# For Android, use the internal Glog implementation.
-IF (MINIGLOG)
- ADD_LIBRARY(miniglog STATIC miniglog/glog/logging.cc)
- INSTALL(TARGETS miniglog
- EXPORT CeresExport
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib${LIB_SUFFIX}
- ARCHIVE DESTINATION lib${LIB_SUFFIX})
-ENDIF (MINIGLOG)
+# Build the list of dependencies for Ceres based on the current configuration.
+IF (NOT MINIGLOG AND GLOG_FOUND)
+ LIST(APPEND CERES_LIBRARY_PUBLIC_DEPENDENCIES ${GLOG_LIBRARIES})
+ENDIF (NOT MINIGLOG AND GLOG_FOUND)
-SET(CERES_LIBRARY_DEPENDENCIES ${GLOG_LIB})
+IF (SUITESPARSE AND SUITESPARSE_FOUND)
+ LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${SUITESPARSE_LIBRARIES})
+ENDIF (SUITESPARSE AND SUITESPARSE_FOUND)
-IF (GFLAGS)
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${GFLAGS_LIB})
-ENDIF (GFLAGS)
+IF (CXSPARSE AND CXSPARSE_FOUND)
+ LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${CXSPARSE_LIBRARIES})
+ENDIF (CXSPARSE AND CXSPARSE_FOUND)
-IF (SUITESPARSE_FOUND)
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${SUITESPARSEQR_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CHOLMOD_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CCOLAMD_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CAMD_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${COLAMD_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${AMD_LIB})
- IF (EXISTS ${SUITESPARSE_CONFIG_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${SUITESPARSE_CONFIG_LIB})
- ENDIF (EXISTS ${SUITESPARSE_CONFIG_LIB})
-
- IF (EXISTS ${METIS_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${METIS_LIB})
- ENDIF (EXISTS ${METIS_LIB})
-
- IF (TBB_FOUND)
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${TBB_LIB})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${TBB_MALLOC_LIB})
- ENDIF (TBB_FOUND)
-ENDIF (SUITESPARSE_FOUND)
-
-IF (CXSPARSE_FOUND)
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CXSPARSE_LIB})
-ENDIF (CXSPARSE_FOUND)
-
-IF (BLAS_AND_LAPACK_FOUND)
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${LAPACK_LIBRARIES})
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${BLAS_LIBRARIES})
-ENDIF (BLAS_AND_LAPACK_FOUND)
-
-IF (CXSPARSE_FOUND)
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CXSPARSE_LIB})
-ENDIF (CXSPARSE_FOUND)
+IF (BLAS_FOUND AND LAPACK_FOUND)
+ LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${LAPACK_LIBRARIES})
+ LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${BLAS_LIBRARIES})
+ENDIF (BLAS_FOUND AND LAPACK_FOUND)
IF (OPENMP_FOUND)
IF (NOT MSVC)
- LIST(APPEND CERES_LIBRARY_DEPENDENCIES gomp)
+ LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES gomp)
+ LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${CMAKE_THREAD_LIBS_INIT})
ENDIF (NOT MSVC)
ENDIF (OPENMP_FOUND)
@@ -192,12 +166,33 @@
${CERES_INTERNAL_HDRS}
${CERES_INTERNAL_SCHUR_FILES})
+# Primarily for Android, but optionally for others, compile the minimal
+# glog implementation into Ceres.
+IF (MINIGLOG)
+ LIST(APPEND CERES_LIBRARY_SOURCE miniglog/glog/logging.cc)
+ENDIF (MINIGLOG)
+
ADD_LIBRARY(ceres ${CERES_LIBRARY_SOURCE})
SET_TARGET_PROPERTIES(ceres PROPERTIES
VERSION ${CERES_VERSION}
SOVERSION ${CERES_VERSION_MAJOR}
)
-TARGET_LINK_LIBRARIES(ceres ${CERES_LIBRARY_DEPENDENCIES})
+
+IF (BUILD_SHARED_LIBS)
+ # When building a shared library, mark all external libraries as
+ # PRIVATE so they don't show up as a dependency.
+ TARGET_LINK_LIBRARIES(ceres
+ LINK_PUBLIC ${CERES_LIBRARY_PUBLIC_DEPENDENCIES}
+ LINK_PRIVATE ${CERES_LIBRARY_PRIVATE_DEPENDENCIES})
+ELSE (BUILD_SHARED_LIBS)
+ # When building a static library, all external libraries are
+ # PUBLIC(default) since the user needs to link to them.
+ # They will be listed in CeresTargets.cmake.
+ SET(CERES_LIBRARY_DEPENDENCIES
+ ${CERES_LIBRARY_PUBLIC_DEPENDENCIES}
+ ${CERES_LIBRARY_PRIVATE_DEPENDENCIES})
+ TARGET_LINK_LIBRARIES(ceres ${CERES_LIBRARY_DEPENDENCIES})
+ENDIF (BUILD_SHARED_LIBS)
INSTALL(TARGETS ceres
EXPORT CeresExport
@@ -212,8 +207,15 @@
numeric_diff_test_utils.cc
test_util.cc)
- TARGET_LINK_LIBRARIES(gtest ${GFLAGS_LIB} ${GLOG_LIB})
- TARGET_LINK_LIBRARIES(test_util ceres gtest ${GLOG_LIB})
+ IF (MINIGLOG)
+ # When using miniglog, it is compiled into Ceres, thus Ceres becomes
+ # the library against which other libraries should link for logging.
+ TARGET_LINK_LIBRARIES(gtest ${GFLAGS_LIBRARIES} ceres)
+ TARGET_LINK_LIBRARIES(test_util ceres gtest)
+ ELSE (MINIGLOG)
+ TARGET_LINK_LIBRARIES(gtest ${GFLAGS_LIBRARIES} ${GLOG_LIBRARIES})
+ TARGET_LINK_LIBRARIES(test_util ceres gtest ${GLOG_LIBRARIES})
+ ENDIF (MINIGLOG)
MACRO (CERES_TEST NAME)
ADD_EXECUTABLE(${NAME}_test ${NAME}_test.cc)
@@ -228,8 +230,8 @@
CERES_TEST(autodiff)
CERES_TEST(autodiff_cost_function)
CERES_TEST(autodiff_local_parameterization)
- CERES_TEST(block_random_access_crs_matrix)
CERES_TEST(block_random_access_dense_matrix)
+ CERES_TEST(block_random_access_diagonal_matrix)
CERES_TEST(block_random_access_sparse_matrix)
CERES_TEST(block_sparse_matrix)
CERES_TEST(c_api)
@@ -241,6 +243,8 @@
CERES_TEST(covariance)
CERES_TEST(dense_sparse_matrix)
CERES_TEST(dynamic_autodiff_cost_function)
+ CERES_TEST(dynamic_compressed_row_sparse_matrix)
+ CERES_TEST(dynamic_numeric_diff_cost_function)
CERES_TEST(evaluator)
CERES_TEST(gradient_checker)
CERES_TEST(gradient_checking_cost_function)
@@ -264,20 +268,23 @@
CERES_TEST(partitioned_matrix_view)
CERES_TEST(polynomial)
CERES_TEST(problem)
+ CERES_TEST(program)
+ CERES_TEST(reorder_program)
CERES_TEST(residual_block)
CERES_TEST(residual_block_utils)
CERES_TEST(rotation)
- CERES_TEST(runtime_numeric_diff_cost_function)
CERES_TEST(schur_complement_solver)
CERES_TEST(schur_eliminator)
+ CERES_TEST(single_linkage_clustering)
CERES_TEST(small_blas)
+ CERES_TEST(solver)
CERES_TEST(solver_impl)
# TODO(sameeragarwal): This test should ultimately be made
# independent of SuiteSparse.
- IF (SUITESPARSE_FOUND)
+ IF (SUITESPARSE AND SUITESPARSE_FOUND)
CERES_TEST(compressed_col_sparse_matrix_utils)
- ENDIF (SUITESPARSE_FOUND)
+ ENDIF (SUITESPARSE AND SUITESPARSE_FOUND)
CERES_TEST(symmetric_linear_solver)
CERES_TEST(triplet_sparse_matrix)
diff --git a/internal/ceres/array_utils.cc b/internal/ceres/array_utils.cc
index 673baa4..205ddaf 100644
--- a/internal/ceres/array_utils.cc
+++ b/internal/ceres/array_utils.cc
@@ -30,9 +30,13 @@
#include "ceres/array_utils.h"
+#include <algorithm>
#include <cmath>
#include <cstddef>
+#include <string>
+#include <vector>
#include "ceres/fpclassify.h"
+#include "ceres/stringprintf.h"
namespace ceres {
namespace internal {
@@ -55,6 +59,20 @@
return true;
}
+int FindInvalidValue(const int size, const double* x) {
+ if (x == NULL) {
+ return size;
+ }
+
+ for (int i = 0; i < size; ++i) {
+ if (!IsFinite(x[i]) || (x[i] == kImpossibleValue)) {
+ return i;
+ }
+ }
+
+ return size;
+};
+
void InvalidateArray(const int size, double* x) {
if (x != NULL) {
for (int i = 0; i < size; ++i) {
@@ -63,5 +81,33 @@
}
}
+void AppendArrayToString(const int size, const double* x, string* result) {
+ for (int i = 0; i < size; ++i) {
+ if (x == NULL) {
+ StringAppendF(result, "Not Computed ");
+ } else {
+ if (x[i] == kImpossibleValue) {
+ StringAppendF(result, "Uninitialized ");
+ } else {
+ StringAppendF(result, "%12g ", x[i]);
+ }
+ }
+ }
+}
+
+void MapValuesToContiguousRange(const int size, int* array) {
+ std::vector<int> unique_values(array, array + size);
+ std::sort(unique_values.begin(), unique_values.end());
+ unique_values.erase(std::unique(unique_values.begin(),
+ unique_values.end()),
+ unique_values.end());
+
+ for (int i = 0; i < size; ++i) {
+ array[i] = std::lower_bound(unique_values.begin(),
+ unique_values.end(),
+ array[i]) - unique_values.begin();
+ }
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/array_utils.h b/internal/ceres/array_utils.h
index 742f439..7f56947 100644
--- a/internal/ceres/array_utils.h
+++ b/internal/ceres/array_utils.h
@@ -57,8 +57,31 @@
// equal to the "impossible" value used by InvalidateArray.
bool IsArrayValid(int size, const double* x);
+// If the array contains an invalid value, return the index for it,
+// otherwise return size.
+int FindInvalidValue(const int size, const double* x);
+
+// Utility routine to print an array of doubles to a string. If the
+// array pointer is NULL, it is treated as an array of zeros.
+void AppendArrayToString(const int size, const double* x, string* result);
+
extern const double kImpossibleValue;
+// This routine takes an array of integer values, sorts and uniques
+// them and then maps each value in the array to its position in the
+// sorted+uniqued array. By doing this, if there are are k unique
+// values in the array, each value is replaced by an integer in the
+// range [0, k-1], while preserving their relative order.
+//
+// For example
+//
+// [1 0 3 5 0 1 5]
+//
+// gets mapped to
+//
+// [1 0 2 3 0 1 3]
+void MapValuesToContiguousRange(int size, int* array);
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/array_utils_test.cc b/internal/ceres/array_utils_test.cc
index c19a44a..203a301 100644
--- a/internal/ceres/array_utils_test.cc
+++ b/internal/ceres/array_utils_test.cc
@@ -32,6 +32,7 @@
#include <limits>
#include <cmath>
+#include <vector>
#include "gtest/gtest.h"
namespace ceres {
@@ -54,5 +55,68 @@
EXPECT_FALSE(IsArrayValid(3, x));
}
+TEST(ArrayUtils, FindInvalidIndex) {
+ double x[3];
+ x[0] = 0.0;
+ x[1] = 1.0;
+ x[2] = 2.0;
+ EXPECT_EQ(FindInvalidValue(3, x), 3);
+ x[1] = std::numeric_limits<double>::infinity();
+ EXPECT_EQ(FindInvalidValue(3, x), 1);
+ x[1] = std::numeric_limits<double>::quiet_NaN();
+ EXPECT_EQ(FindInvalidValue(3, x), 1);
+ x[1] = std::numeric_limits<double>::signaling_NaN();
+ EXPECT_EQ(FindInvalidValue(3, x), 1);
+ EXPECT_EQ(FindInvalidValue(1, NULL), 1);
+ InvalidateArray(3, x);
+ EXPECT_EQ(FindInvalidValue(3, x), 0);
+}
+
+TEST(MapValuesToContiguousRange, ContiguousEntries) {
+ vector<int> array;
+ array.push_back(0);
+ array.push_back(1);
+ vector<int> expected = array;
+ MapValuesToContiguousRange(array.size(), &array[0]);
+ EXPECT_EQ(array, expected);
+ array.clear();
+
+ array.push_back(1);
+ array.push_back(0);
+ expected = array;
+ MapValuesToContiguousRange(array.size(), &array[0]);
+ EXPECT_EQ(array, expected);
+}
+
+TEST(MapValuesToContiguousRange, NonContiguousEntries) {
+ vector<int> array;
+ array.push_back(0);
+ array.push_back(2);
+ vector<int> expected;
+ expected.push_back(0);
+ expected.push_back(1);
+ MapValuesToContiguousRange(array.size(), &array[0]);
+ EXPECT_EQ(array, expected);
+}
+
+TEST(MapValuesToContiguousRange, NonContiguousRepeatingEntries) {
+ vector<int> array;
+ array.push_back(3);
+ array.push_back(1);
+ array.push_back(0);
+ array.push_back(0);
+ array.push_back(0);
+ array.push_back(5);
+ vector<int> expected;
+ expected.push_back(2);
+ expected.push_back(1);
+ expected.push_back(0);
+ expected.push_back(0);
+ expected.push_back(0);
+ expected.push_back(3);
+ MapValuesToContiguousRange(array.size(), &array[0]);
+ EXPECT_EQ(array, expected);
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/autodiff_local_parameterization_test.cc b/internal/ceres/autodiff_local_parameterization_test.cc
index 7e90177..a0f705d 100644
--- a/internal/ceres/autodiff_local_parameterization_test.cc
+++ b/internal/ceres/autodiff_local_parameterization_test.cc
@@ -48,7 +48,6 @@
}
};
-
TEST(AutoDiffLocalParameterizationTest, IdentityParameterization) {
AutoDiffLocalParameterization<IdentityPlus, 3, 3>
parameterization;
@@ -72,6 +71,47 @@
}
}
+struct ScaledPlus {
+ ScaledPlus(const double &scale_factor)
+ : scale_factor_(scale_factor)
+ {}
+
+ template <typename T>
+ bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
+ for (int i = 0; i < 3; ++i) {
+ x_plus_delta[i] = x[i] + T(scale_factor_) * delta[i];
+ }
+ return true;
+ }
+
+ const double scale_factor_;
+};
+
+TEST(AutoDiffLocalParameterizationTest, ScaledParameterization) {
+ const double kTolerance = 1e-14;
+
+ AutoDiffLocalParameterization<ScaledPlus, 3, 3>
+ parameterization(new ScaledPlus(1.2345));
+
+ double x[3] = {1.0, 2.0, 3.0};
+ double delta[3] = {0.0, 1.0, 2.0};
+ double x_plus_delta[3] = {0.0, 0.0, 0.0};
+ parameterization.Plus(x, delta, x_plus_delta);
+
+ EXPECT_NEAR(x_plus_delta[0], 1.0, kTolerance);
+ EXPECT_NEAR(x_plus_delta[1], 3.2345, kTolerance);
+ EXPECT_NEAR(x_plus_delta[2], 5.469, kTolerance);
+
+ double jacobian[9];
+ parameterization.ComputeJacobian(x, jacobian);
+ int k = 0;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j, ++k) {
+ EXPECT_NEAR(jacobian[k], (i == j) ? 1.2345 : 0.0, kTolerance);
+ }
+ }
+}
+
struct QuaternionPlus {
template<typename T>
bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
diff --git a/internal/ceres/blas.cc b/internal/ceres/blas.cc
index f79b1eb..b919e13 100644
--- a/internal/ceres/blas.cc
+++ b/internal/ceres/blas.cc
@@ -29,6 +29,7 @@
// Author: sameeragarwal@google.com (Sameer Agarwal)
#include "ceres/blas.h"
+#include "ceres/internal/port.h"
#include "glog/logging.h"
extern "C" void dsyrk_(char* uplo,
diff --git a/internal/ceres/blas_test.cc b/internal/ceres/blas_test.cc
deleted file mode 100644
index efa7e7b..0000000
--- a/internal/ceres/blas_test.cc
+++ /dev/null
@@ -1,303 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2013 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: keir@google.com (Keir Mierle)
-
-#include "ceres/blas.h"
-
-#include "gtest/gtest.h"
-#include "ceres/internal/eigen.h"
-
-namespace ceres {
-namespace internal {
-
-TEST(BLAS, MatrixMatrixMultiply) {
- const double kTolerance = 1e-16;
- const int kRowA = 3;
- const int kColA = 5;
- Matrix A(kRowA, kColA);
- A.setOnes();
-
- const int kRowB = 5;
- const int kColB = 7;
- Matrix B(kRowB, kColB);
- B.setOnes();
-
- for (int row_stride_c = kRowA; row_stride_c < 3 * kRowA; ++row_stride_c) {
- for (int col_stride_c = kColB; col_stride_c < 3 * kColB; ++col_stride_c) {
- Matrix C(row_stride_c, col_stride_c);
- C.setOnes();
-
- Matrix C_plus = C;
- Matrix C_minus = C;
- Matrix C_assign = C;
-
- Matrix C_plus_ref = C;
- Matrix C_minus_ref = C;
- Matrix C_assign_ref = C;
- for (int start_row_c = 0; start_row_c + kRowA < row_stride_c; ++start_row_c) {
- for (int start_col_c = 0; start_col_c + kColB < col_stride_c; ++start_col_c) {
- C_plus_ref.block(start_row_c, start_col_c, kRowA, kColB) +=
- A * B;
-
- MatrixMatrixMultiply<kRowA, kColA, kRowB, kColB, 1>(
- A.data(), kRowA, kColA,
- B.data(), kRowB, kColB,
- C_plus.data(), start_row_c, start_col_c, row_stride_c, col_stride_c);
-
- EXPECT_NEAR((C_plus_ref - C_plus).norm(), 0.0, kTolerance)
- << "C += A * B \n"
- << "row_stride_c : " << row_stride_c << "\n"
- << "col_stride_c : " << col_stride_c << "\n"
- << "start_row_c : " << start_row_c << "\n"
- << "start_col_c : " << start_col_c << "\n"
- << "Cref : \n" << C_plus_ref << "\n"
- << "C: \n" << C_plus;
-
-
- C_minus_ref.block(start_row_c, start_col_c, kRowA, kColB) -=
- A * B;
-
- MatrixMatrixMultiply<kRowA, kColA, kRowB, kColB, -1>(
- A.data(), kRowA, kColA,
- B.data(), kRowB, kColB,
- C_minus.data(), start_row_c, start_col_c, row_stride_c, col_stride_c);
-
- EXPECT_NEAR((C_minus_ref - C_minus).norm(), 0.0, kTolerance)
- << "C -= A * B \n"
- << "row_stride_c : " << row_stride_c << "\n"
- << "col_stride_c : " << col_stride_c << "\n"
- << "start_row_c : " << start_row_c << "\n"
- << "start_col_c : " << start_col_c << "\n"
- << "Cref : \n" << C_minus_ref << "\n"
- << "C: \n" << C_minus;
-
- C_assign_ref.block(start_row_c, start_col_c, kRowA, kColB) =
- A * B;
-
- MatrixMatrixMultiply<kRowA, kColA, kRowB, kColB, 0>(
- A.data(), kRowA, kColA,
- B.data(), kRowB, kColB,
- C_assign.data(), start_row_c, start_col_c, row_stride_c, col_stride_c);
-
- EXPECT_NEAR((C_assign_ref - C_assign).norm(), 0.0, kTolerance)
- << "C = A * B \n"
- << "row_stride_c : " << row_stride_c << "\n"
- << "col_stride_c : " << col_stride_c << "\n"
- << "start_row_c : " << start_row_c << "\n"
- << "start_col_c : " << start_col_c << "\n"
- << "Cref : \n" << C_assign_ref << "\n"
- << "C: \n" << C_assign;
- }
- }
- }
- }
-}
-
-TEST(BLAS, MatrixTransposeMatrixMultiply) {
- const double kTolerance = 1e-16;
- const int kRowA = 5;
- const int kColA = 3;
- Matrix A(kRowA, kColA);
- A.setOnes();
-
- const int kRowB = 5;
- const int kColB = 7;
- Matrix B(kRowB, kColB);
- B.setOnes();
-
- for (int row_stride_c = kColA; row_stride_c < 3 * kColA; ++row_stride_c) {
- for (int col_stride_c = kColB; col_stride_c < 3 * kColB; ++col_stride_c) {
- Matrix C(row_stride_c, col_stride_c);
- C.setOnes();
-
- Matrix C_plus = C;
- Matrix C_minus = C;
- Matrix C_assign = C;
-
- Matrix C_plus_ref = C;
- Matrix C_minus_ref = C;
- Matrix C_assign_ref = C;
- for (int start_row_c = 0; start_row_c + kColA < row_stride_c; ++start_row_c) {
- for (int start_col_c = 0; start_col_c + kColB < col_stride_c; ++start_col_c) {
- C_plus_ref.block(start_row_c, start_col_c, kColA, kColB) +=
- A.transpose() * B;
-
- MatrixTransposeMatrixMultiply<kRowA, kColA, kRowB, kColB, 1>(
- A.data(), kRowA, kColA,
- B.data(), kRowB, kColB,
- C_plus.data(), start_row_c, start_col_c, row_stride_c, col_stride_c);
-
- EXPECT_NEAR((C_plus_ref - C_plus).norm(), 0.0, kTolerance)
- << "C += A' * B \n"
- << "row_stride_c : " << row_stride_c << "\n"
- << "col_stride_c : " << col_stride_c << "\n"
- << "start_row_c : " << start_row_c << "\n"
- << "start_col_c : " << start_col_c << "\n"
- << "Cref : \n" << C_plus_ref << "\n"
- << "C: \n" << C_plus;
-
- C_minus_ref.block(start_row_c, start_col_c, kColA, kColB) -=
- A.transpose() * B;
-
- MatrixTransposeMatrixMultiply<kRowA, kColA, kRowB, kColB, -1>(
- A.data(), kRowA, kColA,
- B.data(), kRowB, kColB,
- C_minus.data(), start_row_c, start_col_c, row_stride_c, col_stride_c);
-
- EXPECT_NEAR((C_minus_ref - C_minus).norm(), 0.0, kTolerance)
- << "C -= A' * B \n"
- << "row_stride_c : " << row_stride_c << "\n"
- << "col_stride_c : " << col_stride_c << "\n"
- << "start_row_c : " << start_row_c << "\n"
- << "start_col_c : " << start_col_c << "\n"
- << "Cref : \n" << C_minus_ref << "\n"
- << "C: \n" << C_minus;
-
- C_assign_ref.block(start_row_c, start_col_c, kColA, kColB) =
- A.transpose() * B;
-
- MatrixTransposeMatrixMultiply<kRowA, kColA, kRowB, kColB, 0>(
- A.data(), kRowA, kColA,
- B.data(), kRowB, kColB,
- C_assign.data(), start_row_c, start_col_c, row_stride_c, col_stride_c);
-
- EXPECT_NEAR((C_assign_ref - C_assign).norm(), 0.0, kTolerance)
- << "C = A' * B \n"
- << "row_stride_c : " << row_stride_c << "\n"
- << "col_stride_c : " << col_stride_c << "\n"
- << "start_row_c : " << start_row_c << "\n"
- << "start_col_c : " << start_col_c << "\n"
- << "Cref : \n" << C_assign_ref << "\n"
- << "C: \n" << C_assign;
- }
- }
- }
- }
-}
-
-TEST(BLAS, MatrixVectorMultiply) {
- const double kTolerance = 1e-16;
- const int kRowA = 5;
- const int kColA = 3;
- Matrix A(kRowA, kColA);
- A.setOnes();
-
- Vector b(kColA);
- b.setOnes();
-
- Vector c(kRowA);
- c.setOnes();
-
- Vector c_plus = c;
- Vector c_minus = c;
- Vector c_assign = c;
-
- Vector c_plus_ref = c;
- Vector c_minus_ref = c;
- Vector c_assign_ref = c;
-
- c_plus_ref += A * b;
- MatrixVectorMultiply<kRowA, kColA, 1>(A.data(), kRowA, kColA,
- b.data(),
- c_plus.data());
- EXPECT_NEAR((c_plus_ref - c_plus).norm(), 0.0, kTolerance)
- << "c += A * b \n"
- << "c_ref : \n" << c_plus_ref << "\n"
- << "c: \n" << c_plus;
-
- c_minus_ref -= A * b;
- MatrixVectorMultiply<kRowA, kColA, -1>(A.data(), kRowA, kColA,
- b.data(),
- c_minus.data());
- EXPECT_NEAR((c_minus_ref - c_minus).norm(), 0.0, kTolerance)
- << "c += A * b \n"
- << "c_ref : \n" << c_minus_ref << "\n"
- << "c: \n" << c_minus;
-
- c_assign_ref = A * b;
- MatrixVectorMultiply<kRowA, kColA, 0>(A.data(), kRowA, kColA,
- b.data(),
- c_assign.data());
- EXPECT_NEAR((c_assign_ref - c_assign).norm(), 0.0, kTolerance)
- << "c += A * b \n"
- << "c_ref : \n" << c_assign_ref << "\n"
- << "c: \n" << c_assign;
-}
-
-TEST(BLAS, MatrixTransposeVectorMultiply) {
- const double kTolerance = 1e-16;
- const int kRowA = 5;
- const int kColA = 3;
- Matrix A(kRowA, kColA);
- A.setRandom();
-
- Vector b(kRowA);
- b.setRandom();
-
- Vector c(kColA);
- c.setOnes();
-
- Vector c_plus = c;
- Vector c_minus = c;
- Vector c_assign = c;
-
- Vector c_plus_ref = c;
- Vector c_minus_ref = c;
- Vector c_assign_ref = c;
-
- c_plus_ref += A.transpose() * b;
- MatrixTransposeVectorMultiply<kRowA, kColA, 1>(A.data(), kRowA, kColA,
- b.data(),
- c_plus.data());
- EXPECT_NEAR((c_plus_ref - c_plus).norm(), 0.0, kTolerance)
- << "c += A' * b \n"
- << "c_ref : \n" << c_plus_ref << "\n"
- << "c: \n" << c_plus;
-
- c_minus_ref -= A.transpose() * b;
- MatrixTransposeVectorMultiply<kRowA, kColA, -1>(A.data(), kRowA, kColA,
- b.data(),
- c_minus.data());
- EXPECT_NEAR((c_minus_ref - c_minus).norm(), 0.0, kTolerance)
- << "c += A' * b \n"
- << "c_ref : \n" << c_minus_ref << "\n"
- << "c: \n" << c_minus;
-
- c_assign_ref = A.transpose() * b;
- MatrixTransposeVectorMultiply<kRowA, kColA, 0>(A.data(), kRowA, kColA,
- b.data(),
- c_assign.data());
- EXPECT_NEAR((c_assign_ref - c_assign).norm(), 0.0, kTolerance)
- << "c += A' * b \n"
- << "c_ref : \n" << c_assign_ref << "\n"
- << "c: \n" << c_assign;
-}
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/block_jacobi_preconditioner.cc b/internal/ceres/block_jacobi_preconditioner.cc
index 29974d4..19b749b 100644
--- a/internal/ceres/block_jacobi_preconditioner.cc
+++ b/internal/ceres/block_jacobi_preconditioner.cc
@@ -94,7 +94,9 @@
//
// MatrixRef(blocks_[cells[c].block_id],
// col_block_size,
- // col_block_size).selfadjointView<Eigen::Upper>().rankUpdate(m);
+ // col_block_size)
+ // .selfadjointView<Eigen::Upper>()
+ // .rankUpdate(m);
//
}
}
diff --git a/internal/ceres/block_random_access_crs_matrix.cc b/internal/ceres/block_random_access_crs_matrix.cc
deleted file mode 100644
index 5b008e2..0000000
--- a/internal/ceres/block_random_access_crs_matrix.cc
+++ /dev/null
@@ -1,170 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2013 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: sameeragarwal@google.com (Sameer Agarwal)
-
-#include "ceres/block_random_access_crs_matrix.h"
-
-#include <algorithm>
-#include <set>
-#include <utility>
-#include <vector>
-#include "ceres/compressed_row_sparse_matrix.h"
-#include "ceres/internal/port.h"
-#include "ceres/internal/scoped_ptr.h"
-#include "ceres/mutex.h"
-#include "ceres/triplet_sparse_matrix.h"
-#include "ceres/types.h"
-#include "glog/logging.h"
-
-namespace ceres {
-namespace internal {
-
-BlockRandomAccessCRSMatrix::BlockRandomAccessCRSMatrix(
- const vector<int>& blocks,
- const set<pair<int, int> >& block_pairs)
- : kMaxRowBlocks(10 * 1000 * 1000),
- blocks_(blocks) {
- CHECK_LT(blocks.size(), kMaxRowBlocks);
-
- col_layout_.resize(blocks_.size(), 0);
- row_strides_.resize(blocks_.size(), 0);
-
- // Build the row/column layout vector and count the number of scalar
- // rows/columns.
- int num_cols = 0;
- for (int i = 0; i < blocks_.size(); ++i) {
- col_layout_[i] = num_cols;
- num_cols += blocks_[i];
- }
-
- // Walk the sparsity pattern and count the number of non-zeros.
- int num_nonzeros = 0;
- for (set<pair<int, int> >::const_iterator it = block_pairs.begin();
- it != block_pairs.end();
- ++it) {
- const int row_block_size = blocks_[it->first];
- const int col_block_size = blocks_[it->second];
- num_nonzeros += row_block_size * col_block_size;
- }
-
- VLOG(2) << "Matrix Size [" << num_cols
- << "," << num_cols
- << "] " << num_nonzeros;
-
- crsm_.reset(new CompressedRowSparseMatrix(num_cols, num_cols, num_nonzeros));
- int* rows = crsm_->mutable_rows();
- int* cols = crsm_->mutable_cols();
- double* values = crsm_->mutable_values();
-
- // Iterate over the sparsity pattern and fill the scalar sparsity
- // pattern of the underlying compressed sparse row matrix. Along the
- // way also fill out the Layout object which will allow random
- // access into the CRS Matrix.
- set<pair<int, int> >::const_iterator it = block_pairs.begin();
- vector<int> col_blocks;
- int row_pos = 0;
- rows[0] = 0;
- while (it != block_pairs.end()) {
- // Add entries to layout_ for all the blocks for this row.
- col_blocks.clear();
- const int row_block_id = it->first;
- const int row_block_size = blocks_[row_block_id];
- int num_cols = 0;
- while ((it != block_pairs.end()) && (it->first == row_block_id)) {
- layout_[IntPairToLong(it->first, it->second)] =
- new CellInfo(values + num_cols);
- col_blocks.push_back(it->second);
- num_cols += blocks_[it->second];
- ++it;
- };
-
- // Count the number of non-zeros in the row block.
- for (int j = 0; j < row_block_size; ++j) {
- rows[row_pos + j + 1] = rows[row_pos + j] + num_cols;
- }
-
- // Fill out the sparsity pattern for each row.
- int col_pos = 0;
- for (int j = 0; j < col_blocks.size(); ++j) {
- const int col_block_id = col_blocks[j];
- const int col_block_size = blocks_[col_block_id];
- for (int r = 0; r < row_block_size; ++r) {
- const int column_block_begin = rows[row_pos + r] + col_pos;
- for (int c = 0; c < col_block_size; ++c) {
- cols[column_block_begin + c] = col_layout_[col_block_id] + c;
- }
- }
- col_pos += col_block_size;
- }
-
- row_pos += row_block_size;
- values += row_block_size * num_cols;
- row_strides_[row_block_id] = num_cols;
- }
-}
-
-// Assume that the user does not hold any locks on any cell blocks
-// when they are calling SetZero.
-BlockRandomAccessCRSMatrix::~BlockRandomAccessCRSMatrix() {
- // TODO(sameeragarwal) this should be rationalized going forward and
- // perhaps moved into BlockRandomAccessMatrix.
- for (LayoutType::iterator it = layout_.begin();
- it != layout_.end();
- ++it) {
- delete it->second;
- }
-}
-
-CellInfo* BlockRandomAccessCRSMatrix::GetCell(int row_block_id,
- int col_block_id,
- int* row,
- int* col,
- int* row_stride,
- int* col_stride) {
- const LayoutType::iterator it =
- layout_.find(IntPairToLong(row_block_id, col_block_id));
- if (it == layout_.end()) {
- return NULL;
- }
-
- *row = 0;
- *col = 0;
- *row_stride = blocks_[row_block_id];
- *col_stride = row_strides_[row_block_id];
- return it->second;
-}
-
-// Assume that the user does not hold any locks on any cell blocks
-// when they are calling SetZero.
-void BlockRandomAccessCRSMatrix::SetZero() {
- crsm_->SetZero();
-}
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/block_random_access_crs_matrix_test.cc b/internal/ceres/block_random_access_crs_matrix_test.cc
deleted file mode 100644
index 1266c4f..0000000
--- a/internal/ceres/block_random_access_crs_matrix_test.cc
+++ /dev/null
@@ -1,148 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2013 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: sameeragarwal@google.com (Sameer Agarwal)
-
-#include <limits>
-#include <vector>
-#include "ceres/block_random_access_crs_matrix.h"
-#include "ceres/internal/eigen.h"
-#include "glog/logging.h"
-#include "gtest/gtest.h"
-
-namespace ceres {
-namespace internal {
-
-TEST(BlockRandomAccessCRSMatrix, GetCell) {
- vector<int> blocks;
- blocks.push_back(3);
- blocks.push_back(4);
- blocks.push_back(5);
- const int num_rows = 3 + 4 + 5;
-
- set< pair<int, int> > block_pairs;
- int num_nonzeros = 0;
- block_pairs.insert(make_pair(0, 0));
- num_nonzeros += blocks[0] * blocks[0];
-
- block_pairs.insert(make_pair(1, 1));
- num_nonzeros += blocks[1] * blocks[1];
-
- block_pairs.insert(make_pair(1, 2));
- num_nonzeros += blocks[1] * blocks[2];
-
- block_pairs.insert(make_pair(2, 0));
- num_nonzeros += blocks[2] * blocks[0];
-
- BlockRandomAccessCRSMatrix m(blocks, block_pairs);
- EXPECT_EQ(m.num_rows(), num_rows);
- EXPECT_EQ(m.num_cols(), num_rows);
-
- for (set<pair<int, int> >::const_iterator it = block_pairs.begin();
- it != block_pairs.end();
- ++it) {
- const int row_block_id = it->first;
- const int col_block_id = it->second;
- int row;
- int col;
- int row_stride;
- int col_stride;
- CellInfo* cell = m.GetCell(row_block_id, col_block_id,
- &row, &col,
- &row_stride, &col_stride);
- EXPECT_TRUE(cell != NULL);
- EXPECT_EQ(row, 0);
- EXPECT_EQ(col, 0);
-
- // Write into the block.
- MatrixRef(cell->values, row_stride, col_stride).block(
- row, col, blocks[row_block_id], blocks[col_block_id]) =
- (row_block_id + 1) * (col_block_id +1) *
- Matrix::Ones(blocks[row_block_id], blocks[col_block_id]);
- }
-
- const CompressedRowSparseMatrix* crsm = m.matrix();
- EXPECT_EQ(crsm->num_nonzeros(), num_nonzeros);
-
- Matrix dense;
- crsm->ToDenseMatrix(&dense);
-
- double kTolerance = 1e-14;
-
- // (0,0)
- EXPECT_NEAR((dense.block(0, 0, 3, 3) - Matrix::Ones(3, 3)).norm(),
- 0.0,
- kTolerance);
- // (1,1)
- EXPECT_NEAR((dense.block(3, 3, 4, 4) - 2 * 2 * Matrix::Ones(4, 4)).norm(),
- 0.0,
- kTolerance);
- // (1,2)
- EXPECT_NEAR((dense.block(3, 3 + 4, 4, 5) - 2 * 3 * Matrix::Ones(4, 5)).norm(),
- 0.0,
- kTolerance);
- // (2,0)
- EXPECT_NEAR((dense.block(3 + 4, 0, 5, 3) - 3 * 1 * Matrix::Ones(5, 3)).norm(),
- 0.0,
- kTolerance);
-
- // There is nothing else in the matrix besides these four blocks.
- EXPECT_NEAR(dense.norm(), sqrt(9. + 16. * 16. + 36. * 20. + 9. * 15.),
- kTolerance);
-}
-
-// IntPairToLong is private, thus this fixture is needed to access and
-// test it.
-class BlockRandomAccessCRSMatrixTest : public ::testing::Test {
- public:
- virtual void SetUp() {
- vector<int> blocks;
- blocks.push_back(1);
- set< pair<int, int> > block_pairs;
- block_pairs.insert(make_pair(0, 0));
- m_.reset(new BlockRandomAccessCRSMatrix(blocks, block_pairs));
- }
-
- void CheckIntPair(int a, int b) {
- int64 value = m_->IntPairToLong(a, b);
- EXPECT_GT(value, 0) << "Overflow a = " << a << " b = " << b;
- EXPECT_GT(value, a) << "Overflow a = " << a << " b = " << b;
- EXPECT_GT(value, b) << "Overflow a = " << a << " b = " << b;
- }
-
- private:
- scoped_ptr<BlockRandomAccessCRSMatrix> m_;
-};
-
-TEST_F(BlockRandomAccessCRSMatrixTest, IntPairToLongOverflow) {
- CheckIntPair(numeric_limits<int>::max(), numeric_limits<int>::max());
-}
-
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/block_random_access_diagonal_matrix.cc b/internal/ceres/block_random_access_diagonal_matrix.cc
new file mode 100644
index 0000000..d8bf4ef
--- /dev/null
+++ b/internal/ceres/block_random_access_diagonal_matrix.cc
@@ -0,0 +1,120 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/block_random_access_diagonal_matrix.h"
+
+#include <algorithm>
+#include <set>
+#include <utility>
+#include <vector>
+#include "ceres/internal/port.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/triplet_sparse_matrix.h"
+#include "ceres/types.h"
+#include "ceres/stl_util.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix(
+ const vector<int>& blocks)
+ : blocks_(blocks) {
+ // Build the row/column layout vector and count the number of scalar
+ // rows/columns.
+ int num_cols = 0;
+ int num_nonzeros = 0;
+ vector<int> col_layout;
+ for (int i = 0; i < blocks_.size(); ++i) {
+ col_layout.push_back(num_cols);
+ num_cols += blocks_[i];
+ num_nonzeros += blocks_[i] * blocks_[i];
+ }
+
+ VLOG(1) << "Matrix Size [" << num_cols
+ << "," << num_cols
+ << "] " << num_nonzeros;
+
+ tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros));
+ tsm_->set_num_nonzeros(num_nonzeros);
+ int* rows = tsm_->mutable_rows();
+ int* cols = tsm_->mutable_cols();
+ double* values = tsm_->mutable_values();
+
+ int pos = 0;
+ for (int i = 0; i < blocks_.size(); ++i) {
+ const int block_size = blocks_[i];
+ layout_.push_back(new CellInfo(values + pos));
+ const int block_begin = col_layout[i];
+ for (int r = 0; r < block_size; ++r) {
+ for (int c = 0; c < block_size; ++c, ++pos) {
+ rows[pos] = block_begin + r;
+ cols[pos] = block_begin + c;
+ }
+ }
+ }
+}
+
+// Assume that the user does not hold any locks on any cell blocks
+// when they are calling SetZero.
+BlockRandomAccessDiagonalMatrix::~BlockRandomAccessDiagonalMatrix() {
+ STLDeleteContainerPointers(layout_.begin(), layout_.end());
+}
+
+CellInfo* BlockRandomAccessDiagonalMatrix::GetCell(int row_block_id,
+ int col_block_id,
+ int* row,
+ int* col,
+ int* row_stride,
+ int* col_stride) {
+ if (row_block_id != col_block_id) {
+ return NULL;
+ }
+ const int stride = blocks_[row_block_id];
+
+ // Each cell is stored contiguously as its own little dense matrix.
+ *row = 0;
+ *col = 0;
+ *row_stride = stride;
+ *col_stride = stride;
+ return layout_[row_block_id];
+}
+
+// Assume that the user does not hold any locks on any cell blocks
+// when they are calling SetZero.
+void BlockRandomAccessDiagonalMatrix::SetZero() {
+ if (tsm_->num_nonzeros()) {
+ VectorRef(tsm_->mutable_values(),
+ tsm_->num_nonzeros()).setZero();
+ }
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/block_random_access_crs_matrix.h b/internal/ceres/block_random_access_diagonal_matrix.h
similarity index 63%
rename from internal/ceres/block_random_access_crs_matrix.h
rename to internal/ceres/block_random_access_diagonal_matrix.h
index 11a203b..6b3cff2 100644
--- a/internal/ceres/block_random_access_crs_matrix.h
+++ b/internal/ceres/block_random_access_diagonal_matrix.h
@@ -28,16 +28,16 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
-#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_
-#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_
+#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_
+#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_
#include <set>
#include <vector>
#include <utility>
#include "ceres/mutex.h"
#include "ceres/block_random_access_matrix.h"
-#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/collections_port.h"
+#include "ceres/triplet_sparse_matrix.h"
#include "ceres/integral_types.h"
#include "ceres/internal/macros.h"
#include "ceres/internal/port.h"
@@ -47,19 +47,16 @@
namespace ceres {
namespace internal {
-// A square BlockRandomAccessMatrix where the underlying storage is a
-// compressed row sparse matrix. The matrix need not be symmetric.
-class BlockRandomAccessCRSMatrix : public BlockRandomAccessMatrix {
+// A thread safe block diagonal matrix implementation of
+// BlockRandomAccessMatrix.
+class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix {
public:
- // blocks is an array of block sizes. block_pairs is a set of
- // <row_block_id, col_block_id> pairs to identify the non-zero cells
- // of this matrix.
- BlockRandomAccessCRSMatrix(const vector<int>& blocks,
- const set<pair<int, int> >& block_pairs);
+ // blocks is an array of block sizes.
+ BlockRandomAccessDiagonalMatrix(const vector<int>& blocks);
// The destructor is not thread safe. It assumes that no one is
// modifying any cells when the matrix is being destroyed.
- virtual ~BlockRandomAccessCRSMatrix();
+ virtual ~BlockRandomAccessDiagonalMatrix();
// BlockRandomAccessMatrix Interface.
virtual CellInfo* GetCell(int row_block_id,
@@ -74,35 +71,26 @@
virtual void SetZero();
// Since the matrix is square, num_rows() == num_cols().
- virtual int num_rows() const { return crsm_->num_rows(); }
- virtual int num_cols() const { return crsm_->num_cols(); }
+ virtual int num_rows() const { return tsm_->num_rows(); }
+ virtual int num_cols() const { return tsm_->num_cols(); }
- // Access to the underlying matrix object.
- const CompressedRowSparseMatrix* matrix() const { return crsm_.get(); }
- CompressedRowSparseMatrix* mutable_matrix() { return crsm_.get(); }
+ // Access to the underlying matrix object.
+ const TripletSparseMatrix* matrix() const { return tsm_.get(); }
+ TripletSparseMatrix* mutable_matrix() { return tsm_.get(); }
private:
- int64 IntPairToLong(int a, int b) {
- return a * kMaxRowBlocks + b;
- }
-
- const int64 kMaxRowBlocks;
// row/column block sizes.
const vector<int> blocks_;
- vector<int> col_layout_;
- vector<int> row_strides_;
+ vector<CellInfo*> layout_;
- // A mapping from <row_block_id, col_block_id> to the position in
- // the values array of tsm_ where the block is stored.
- typedef HashMap<long int, CellInfo* > LayoutType;
- LayoutType layout_;
+ // The underlying matrix object which actually stores the cells.
+ scoped_ptr<TripletSparseMatrix> tsm_;
- scoped_ptr<CompressedRowSparseMatrix> crsm_;
- friend class BlockRandomAccessCRSMatrixTest;
- CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessCRSMatrix);
+ friend class BlockRandomAccessDiagonalMatrixTest;
+ CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDiagonalMatrix);
};
} // namespace internal
} // namespace ceres
-#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_
+#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_
diff --git a/internal/ceres/block_random_access_diagonal_matrix_test.cc b/internal/ceres/block_random_access_diagonal_matrix_test.cc
new file mode 100644
index 0000000..e19268b
--- /dev/null
+++ b/internal/ceres/block_random_access_diagonal_matrix_test.cc
@@ -0,0 +1,116 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include <limits>
+#include <vector>
+
+#include "ceres/block_random_access_diagonal_matrix.h"
+#include "ceres/internal/eigen.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+TEST(BlockRandomAccessDiagonalMatrix, GetCell) {
+ vector<int> blocks;
+ blocks.push_back(3);
+ blocks.push_back(4);
+ blocks.push_back(5);
+ const int num_rows = 3 + 4 + 5;
+ const int num_nonzeros = 3 * 3 + 4 * 4 + 5 * 5;
+
+ BlockRandomAccessDiagonalMatrix m(blocks);
+ EXPECT_EQ(m.num_rows(), num_rows);
+ EXPECT_EQ(m.num_cols(), num_rows);
+
+ for (int i = 0; i < blocks.size(); ++i) {
+ const int row_block_id = i;
+ int col_block_id;
+ int row;
+ int col;
+ int row_stride;
+ int col_stride;
+
+ for (int j = 0; j < blocks.size(); ++j) {
+ col_block_id = j;
+ CellInfo* cell = m.GetCell(row_block_id, col_block_id,
+ &row, &col,
+ &row_stride, &col_stride);
+ // Off diagonal entries are not present.
+ if (i != j) {
+ EXPECT_TRUE(cell == NULL);
+ continue;
+ }
+
+ EXPECT_TRUE(cell != NULL);
+ EXPECT_EQ(row, 0);
+ EXPECT_EQ(col, 0);
+ EXPECT_EQ(row_stride, blocks[row_block_id]);
+ EXPECT_EQ(col_stride, blocks[col_block_id]);
+
+ // Write into the block
+ MatrixRef(cell->values, row_stride, col_stride).block(
+ row, col, blocks[row_block_id], blocks[col_block_id]) =
+ (row_block_id + 1) * (col_block_id +1) *
+ Matrix::Ones(blocks[row_block_id], blocks[col_block_id]);
+ }
+ }
+
+ const TripletSparseMatrix* tsm = m.matrix();
+ EXPECT_EQ(tsm->num_nonzeros(), num_nonzeros);
+ EXPECT_EQ(tsm->max_num_nonzeros(), num_nonzeros);
+
+ Matrix dense;
+ tsm->ToDenseMatrix(&dense);
+
+ double kTolerance = 1e-14;
+
+ // (0,0)
+ EXPECT_NEAR((dense.block(0, 0, 3, 3) - Matrix::Ones(3, 3)).norm(),
+ 0.0,
+ kTolerance);
+
+ // (1,1)
+ EXPECT_NEAR((dense.block(3, 3, 4, 4) - 2 * 2 * Matrix::Ones(4, 4)).norm(),
+ 0.0,
+ kTolerance);
+
+ // (1,1)
+ EXPECT_NEAR((dense.block(7, 7, 5, 5) - 3 * 3 * Matrix::Ones(5, 5)).norm(),
+ 0.0,
+ kTolerance);
+
+ // There is nothing else in the matrix besides these four blocks.
+ EXPECT_NEAR(dense.norm(), sqrt(9.0 + 16. * 16. + 81.0 * 25.), kTolerance);
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/block_random_access_sparse_matrix.h b/internal/ceres/block_random_access_sparse_matrix.h
index a6b5f39..27b1029 100644
--- a/internal/ceres/block_random_access_sparse_matrix.h
+++ b/internal/ceres/block_random_access_sparse_matrix.h
@@ -47,7 +47,7 @@
namespace ceres {
namespace internal {
-// A threaf safe square block sparse implementation of
+// A thread safe square block sparse implementation of
// BlockRandomAccessMatrix. Internally a TripletSparseMatrix is used
// for doing the actual storage. This class augments this matrix with
// an unordered_map that allows random read/write access.
diff --git a/internal/ceres/block_structure.cc b/internal/ceres/block_structure.cc
index 5a1a5e1..00c4ce2 100644
--- a/internal/ceres/block_structure.cc
+++ b/internal/ceres/block_structure.cc
@@ -34,6 +34,9 @@
namespace internal {
bool CellLessThan(const Cell& lhs, const Cell& rhs) {
+ if (lhs.block_id == rhs.block_id) {
+ return (lhs.position < rhs.position);
+ }
return (lhs.block_id < rhs.block_id);
}
diff --git a/internal/ceres/block_structure.h b/internal/ceres/block_structure.h
index f509067..656716e 100644
--- a/internal/ceres/block_structure.h
+++ b/internal/ceres/block_structure.h
@@ -45,9 +45,7 @@
namespace ceres {
namespace internal {
-class BlockStructureProto;
-
-typedef int16 BlockSize;
+typedef int32 BlockSize;
struct Block {
Block() : size(-1), position(-1) {}
@@ -89,16 +87,6 @@
vector<CompressedColumn> cols;
};
-// Deserialize the given block structure proto to the given block structure.
-// Destroys previous contents of block_structure.
-void ProtoToBlockStructure(const BlockStructureProto &proto,
- CompressedRowBlockStructure *block_structure);
-
-// Serialize the given block structure to the given proto. Destroys previous
-// contents of proto.
-void BlockStructureToProto(const CompressedRowBlockStructure &block_structure,
- BlockStructureProto *proto);
-
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/callbacks.cc b/internal/ceres/callbacks.cc
new file mode 100644
index 0000000..d223633
--- /dev/null
+++ b/internal/ceres/callbacks.cc
@@ -0,0 +1,109 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include <iostream> // NO LINT
+#include "ceres/callbacks.h"
+#include "ceres/program.h"
+#include "ceres/stringprintf.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+StateUpdatingCallback::StateUpdatingCallback(Program* program,
+ double* parameters)
+ : program_(program), parameters_(parameters) {}
+
+StateUpdatingCallback::~StateUpdatingCallback() {}
+
+CallbackReturnType StateUpdatingCallback::operator()(
+ const IterationSummary& summary) {
+ if (summary.step_is_successful) {
+ program_->StateVectorToParameterBlocks(parameters_);
+ program_->CopyParameterBlockStateToUserState();
+ }
+ return SOLVER_CONTINUE;
+}
+
+LoggingCallback::LoggingCallback(const MinimizerType minimizer_type,
+ const bool log_to_stdout)
+ : minimizer_type(minimizer_type),
+ log_to_stdout_(log_to_stdout) {}
+
+LoggingCallback::~LoggingCallback() {}
+
+CallbackReturnType LoggingCallback::operator()(
+ const IterationSummary& summary) {
+ string output;
+ if (minimizer_type == LINE_SEARCH) {
+ const char* kReportRowFormat =
+ "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e "
+ "s:% 3.2e e:% 3d it:% 3.2e tt:% 3.2e";
+ output = StringPrintf(kReportRowFormat,
+ summary.iteration,
+ summary.cost,
+ summary.cost_change,
+ summary.gradient_max_norm,
+ summary.step_norm,
+ summary.step_size,
+ summary.line_search_function_evaluations,
+ summary.iteration_time_in_seconds,
+ summary.cumulative_time_in_seconds);
+ } else if (minimizer_type == TRUST_REGION) {
+ if (summary.iteration == 0) {
+ output = "iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time\n";
+ }
+ const char* kReportRowFormat =
+ "% 4d % 8e % 3.2e % 3.2e % 3.2e % 3.2e % 3.2e % 3d % 3.2e % 3.2e";
+ output += StringPrintf(kReportRowFormat,
+ summary.iteration,
+ summary.cost,
+ summary.cost_change,
+ summary.gradient_max_norm,
+ summary.step_norm,
+ summary.relative_decrease,
+ summary.trust_region_radius,
+ summary.linear_solver_iterations,
+ summary.iteration_time_in_seconds,
+ summary.cumulative_time_in_seconds);
+ } else {
+ LOG(FATAL) << "Unknown minimizer type.";
+ }
+
+ if (log_to_stdout_) {
+ cout << output << endl;
+ } else {
+ VLOG(1) << output;
+ }
+ return SOLVER_CONTINUE;
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/callbacks.h b/internal/ceres/callbacks.h
new file mode 100644
index 0000000..93704df
--- /dev/null
+++ b/internal/ceres/callbacks.h
@@ -0,0 +1,71 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_INTERNAL_CALLBACKS_H_
+#define CERES_INTERNAL_CALLBACKS_H_
+
+#include <string>
+#include "ceres/iteration_callback.h"
+#include "ceres/internal/port.h"
+
+namespace ceres {
+namespace internal {
+
+class Program;
+
+// Callback for updating the externally visible state of parameter
+// blocks.
+class StateUpdatingCallback : public IterationCallback {
+ public:
+ StateUpdatingCallback(Program* program, double* parameters);
+ virtual ~StateUpdatingCallback();
+ virtual CallbackReturnType operator()(const IterationSummary& summary);
+ private:
+ Program* program_;
+ double* parameters_;
+};
+
+// Callback for logging the state of the minimizer to STDERR or
+// STDOUT depending on the user's preferences and logging level.
+class LoggingCallback : public IterationCallback {
+ public:
+ LoggingCallback(MinimizerType minimizer_type, bool log_to_stdout);
+ virtual ~LoggingCallback();
+ virtual CallbackReturnType operator()(const IterationSummary& summary);
+
+ private:
+ const MinimizerType minimizer_type;
+ const bool log_to_stdout_;
+};
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_INTERNAL_CALLBACKS_H_
diff --git a/internal/ceres/canonical_views_clustering.cc b/internal/ceres/canonical_views_clustering.cc
index 6531945..2f032e6 100644
--- a/internal/ceres/canonical_views_clustering.cc
+++ b/internal/ceres/canonical_views_clustering.cc
@@ -29,6 +29,9 @@
// Author: David Gallup (dgallup@google.com)
// Sameer Agarwal (sameeragarwal@google.com)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include "ceres/canonical_views_clustering.h"
@@ -57,8 +60,8 @@
// configuration of the clustering algorithm that some of the
// vertices may not be assigned to any cluster. In this case they
// are assigned to a cluster with id = kInvalidClusterId.
- void ComputeClustering(const Graph<int>& graph,
- const CanonicalViewsClusteringOptions& options,
+ void ComputeClustering(const CanonicalViewsClusteringOptions& options,
+ const Graph<int>& graph,
vector<int>* centers,
IntMap* membership);
@@ -81,21 +84,21 @@
};
void ComputeCanonicalViewsClustering(
- const Graph<int>& graph,
const CanonicalViewsClusteringOptions& options,
+ const Graph<int>& graph,
vector<int>* centers,
IntMap* membership) {
time_t start_time = time(NULL);
CanonicalViewsClustering cv;
- cv.ComputeClustering(graph, options, centers, membership);
+ cv.ComputeClustering(options, graph, centers, membership);
VLOG(2) << "Canonical views clustering time (secs): "
<< time(NULL) - start_time;
}
// Implementation of CanonicalViewsClustering
void CanonicalViewsClustering::ComputeClustering(
- const Graph<int>& graph,
const CanonicalViewsClusteringOptions& options,
+ const Graph<int>& graph,
vector<int>* centers,
IntMap* membership) {
options_ = options;
diff --git a/internal/ceres/canonical_views_clustering.h b/internal/ceres/canonical_views_clustering.h
index 48d1ed2..1b4c4ee 100644
--- a/internal/ceres/canonical_views_clustering.h
+++ b/internal/ceres/canonical_views_clustering.h
@@ -41,15 +41,15 @@
#ifndef CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_
#define CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include <vector>
#include "ceres/collections_port.h"
#include "ceres/graph.h"
-#include "ceres/internal/macros.h"
-#include "ceres/map_util.h"
-#include "glog/logging.h"
namespace ceres {
namespace internal {
@@ -100,8 +100,8 @@
// algorithm that some of the vertices may not be assigned to any
// cluster. In this case they are assigned to a cluster with id = -1;
void ComputeCanonicalViewsClustering(
- const Graph<int>& graph,
const CanonicalViewsClusteringOptions& options,
+ const Graph<int>& graph,
vector<int>* centers,
HashMap<int, int>* membership);
diff --git a/internal/ceres/canonical_views_clustering_test.cc b/internal/ceres/canonical_views_clustering_test.cc
index 78d5635..f86084a 100644
--- a/internal/ceres/canonical_views_clustering_test.cc
+++ b/internal/ceres/canonical_views_clustering_test.cc
@@ -29,6 +29,9 @@
// Author: Sameer Agarwal (sameeragarwal@google.com)
// David Gallup (dgallup@google.com)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include "ceres/canonical_views_clustering.h"
@@ -69,7 +72,7 @@
}
void ComputeClustering() {
- ComputeCanonicalViewsClustering(graph_, options_, ¢ers_, &membership_);
+ ComputeCanonicalViewsClustering(options_, graph_, ¢ers_, &membership_);
}
Graph<int> graph_;
diff --git a/internal/ceres/cgnr_solver.cc b/internal/ceres/cgnr_solver.cc
index 9b8f980..88e61d9 100644
--- a/internal/ceres/cgnr_solver.cc
+++ b/internal/ceres/cgnr_solver.cc
@@ -33,6 +33,7 @@
#include "ceres/block_jacobi_preconditioner.h"
#include "ceres/cgnr_linear_operator.h"
#include "ceres/conjugate_gradients_solver.h"
+#include "ceres/internal/eigen.h"
#include "ceres/linear_solver.h"
#include "ceres/wall_time.h"
#include "glog/logging.h"
@@ -43,6 +44,10 @@
CgnrSolver::CgnrSolver(const LinearSolver::Options& options)
: options_(options),
preconditioner_(NULL) {
+ if (options_.preconditioner_type != JACOBI &&
+ options_.preconditioner_type != IDENTITY) {
+ LOG(FATAL) << "CGNR only supports IDENTITY and JACOBI preconditioners.";
+ }
}
LinearSolver::Summary CgnrSolver::SolveImpl(
@@ -53,9 +58,9 @@
EventLogger event_logger("CgnrSolver::Solve");
// Form z = Atb.
- scoped_array<double> z(new double[A->num_cols()]);
- std::fill(z.get(), z.get() + A->num_cols(), 0.0);
- A->LeftMultiply(b, z.get());
+ Vector z(A->num_cols());
+ z.setZero();
+ A->LeftMultiply(b, z.data());
// Precondition if necessary.
LinearSolver::PerSolveOptions cg_per_solve_options = per_solve_options;
@@ -65,20 +70,17 @@
}
preconditioner_->Update(*A, per_solve_options.D);
cg_per_solve_options.preconditioner = preconditioner_.get();
- } else if (options_.preconditioner_type != IDENTITY) {
- LOG(FATAL) << "CGNR only supports IDENTITY and JACOBI preconditioners.";
}
// Solve (AtA + DtD)x = z (= Atb).
- std::fill(x, x + A->num_cols(), 0.0);
+ VectorRef(x, A->num_cols()).setZero();
CgnrLinearOperator lhs(*A, per_solve_options.D);
event_logger.AddEvent("Setup");
ConjugateGradientsSolver conjugate_gradient_solver(options_);
LinearSolver::Summary summary =
- conjugate_gradient_solver.Solve(&lhs, z.get(), cg_per_solve_options, x);
+ conjugate_gradient_solver.Solve(&lhs, z.data(), cg_per_solve_options, x);
event_logger.AddEvent("Solve");
-
return summary;
}
diff --git a/internal/ceres/collections_port.h b/internal/ceres/collections_port.h
index 715c975..3f976b9 100644
--- a/internal/ceres/collections_port.h
+++ b/internal/ceres/collections_port.h
@@ -33,26 +33,48 @@
#ifndef CERES_INTERNAL_COLLECTIONS_PORT_H_
#define CERES_INTERNAL_COLLECTIONS_PORT_H_
-#if defined(CERES_NO_TR1)
+#include "ceres/internal/port.h"
+
+#if defined(CERES_NO_UNORDERED_MAP)
# include <map>
# include <set>
-#else
-# if defined(_MSC_VER)
-# include <unordered_map>
-# include <unordered_set>
-# else
-# include <tr1/unordered_map>
-# include <tr1/unordered_set>
-# endif
#endif
+
+#if defined(CERES_TR1_UNORDERED_MAP)
+# include <tr1/unordered_map>
+# include <tr1/unordered_set>
+# define CERES_HASH_NAMESPACE_START namespace std { namespace tr1 {
+# define CERES_HASH_NAMESPACE_END } }
+#endif
+
+#if defined(CERES_STD_UNORDERED_MAP)
+# include <unordered_map>
+# include <unordered_set>
+# define CERES_HASH_NAMESPACE_START namespace std {
+# define CERES_HASH_NAMESPACE_END }
+#endif
+
+#if defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE)
+# include <unordered_map>
+# include <unordered_set>
+# define CERES_HASH_NAMESPACE_START namespace std { namespace tr1 {
+# define CERES_HASH_NAMESPACE_END } }
+#endif
+
+#if !defined(CERES_NO_UNORDERED_MAP) && !defined(CERES_TR1_UNORDERED_MAP) && \
+ !defined(CERES_STD_UNORDERED_MAP) && !defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) // NOLINT
+# error One of: CERES_NO_UNORDERED_MAP, CERES_TR1_UNORDERED_MAP,\
+ CERES_STD_UNORDERED_MAP, CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE must be defined! // NOLINT
+#endif
+
#include <utility>
#include "ceres/integral_types.h"
#include "ceres/internal/port.h"
-// Some systems don't have access to TR1. In that case, substitute the hash
-// map/set with normal map/set. The price to pay is slightly slower speed for
-// some operations.
-#if defined(CERES_NO_TR1)
+// Some systems don't have access to unordered_map/unordered_set. In
+// that case, substitute the hash map/set with normal map/set. The
+// price to pay is slower speed for some operations.
+#if defined(CERES_NO_UNORDERED_MAP)
namespace ceres {
namespace internal {
@@ -71,11 +93,20 @@
namespace ceres {
namespace internal {
+#if defined(CERES_TR1_UNORDERED_MAP) || \
+ defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE)
template<typename K, typename V>
struct HashMap : std::tr1::unordered_map<K, V> {};
-
template<typename K>
struct HashSet : std::tr1::unordered_set<K> {};
+#endif
+
+#if defined(CERES_STD_UNORDERED_MAP)
+template<typename K, typename V>
+struct HashMap : std::unordered_map<K, V> {};
+template<typename K>
+struct HashSet : std::unordered_set<K> {};
+#endif
#if defined(_WIN32) && !defined(__MINGW64__) && !defined(__MINGW32__)
#define GG_LONGLONG(x) x##I64
@@ -162,6 +193,5 @@
CERES_HASH_NAMESPACE_END
-#endif // CERES_NO_TR1
-
+#endif // CERES_NO_UNORDERED_MAP
#endif // CERES_INTERNAL_COLLECTIONS_PORT_H_
diff --git a/internal/ceres/compressed_row_jacobian_writer.cc b/internal/ceres/compressed_row_jacobian_writer.cc
index bbadb77..ed8db14 100644
--- a/internal/ceres/compressed_row_jacobian_writer.cc
+++ b/internal/ceres/compressed_row_jacobian_writer.cc
@@ -40,6 +40,44 @@
namespace ceres {
namespace internal {
+void CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors(
+ const Program* program, CompressedRowSparseMatrix* jacobian) {
+ const vector<ParameterBlock*>& parameter_blocks =
+ program->parameter_blocks();
+ vector<int>& col_blocks = *(jacobian->mutable_col_blocks());
+ col_blocks.resize(parameter_blocks.size());
+ for (int i = 0; i < parameter_blocks.size(); ++i) {
+ col_blocks[i] = parameter_blocks[i]->LocalSize();
+ }
+
+ const vector<ResidualBlock*>& residual_blocks =
+ program->residual_blocks();
+ vector<int>& row_blocks = *(jacobian->mutable_row_blocks());
+ row_blocks.resize(residual_blocks.size());
+ for (int i = 0; i < residual_blocks.size(); ++i) {
+ row_blocks[i] = residual_blocks[i]->NumResiduals();
+ }
+}
+
+void CompressedRowJacobianWriter::GetOrderedParameterBlocks(
+ const Program* program,
+ int residual_id,
+ vector<pair<int, int> >* evaluated_jacobian_blocks) {
+ const ResidualBlock* residual_block =
+ program->residual_blocks()[residual_id];
+ const int num_parameter_blocks = residual_block->NumParameterBlocks();
+
+ for (int j = 0; j < num_parameter_blocks; ++j) {
+ const ParameterBlock* parameter_block =
+ residual_block->parameter_blocks()[j];
+ if (!parameter_block->IsConstant()) {
+ evaluated_jacobian_blocks->push_back(
+ make_pair(parameter_block->index(), j));
+ }
+ }
+ sort(evaluated_jacobian_blocks->begin(), evaluated_jacobian_blocks->end());
+}
+
SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const {
const vector<ResidualBlock*>& residual_blocks =
program_->residual_blocks();
@@ -71,7 +109,7 @@
total_num_effective_parameters,
num_jacobian_nonzeros + total_num_effective_parameters);
- // At this stage, the CompressedSparseMatrix is an invalid state. But this
+ // At this stage, the CompressedRowSparseMatrix is an invalid state. But this
// seems to be the only way to construct it without doing a memory copy.
int* rows = jacobian->mutable_rows();
int* cols = jacobian->mutable_cols();
@@ -132,22 +170,7 @@
}
CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]);
- // Populate the row and column block vectors for use by block
- // oriented ordering algorithms. This is useful when
- // Solver::Options::use_block_amd = true.
- const vector<ParameterBlock*>& parameter_blocks =
- program_->parameter_blocks();
- vector<int>& col_blocks = *(jacobian->mutable_col_blocks());
- col_blocks.resize(parameter_blocks.size());
- for (int i = 0; i < parameter_blocks.size(); ++i) {
- col_blocks[i] = parameter_blocks[i]->LocalSize();
- }
-
- vector<int>& row_blocks = *(jacobian->mutable_row_blocks());
- row_blocks.resize(residual_blocks.size());
- for (int i = 0; i < residual_blocks.size(); ++i) {
- row_blocks[i] = residual_blocks[i]->NumResiduals();
- }
+ PopulateJacobianRowAndColumnBlockVectors(program_, jacobian);
return jacobian;
}
@@ -164,25 +187,10 @@
const ResidualBlock* residual_block =
program_->residual_blocks()[residual_id];
- const int num_parameter_blocks = residual_block->NumParameterBlocks();
const int num_residuals = residual_block->NumResiduals();
- // It is necessary to determine the order of the jacobian blocks before
- // copying them into the CompressedRowSparseMatrix. Just because a cost
- // function uses parameter blocks 1 after 2 in its arguments does not mean
- // that the block 1 occurs before block 2 in the column layout of the
- // jacobian. Thus, determine the order by sorting the jacobian blocks by their
- // position in the state vector.
vector<pair<int, int> > evaluated_jacobian_blocks;
- for (int j = 0; j < num_parameter_blocks; ++j) {
- const ParameterBlock* parameter_block =
- residual_block->parameter_blocks()[j];
- if (!parameter_block->IsConstant()) {
- evaluated_jacobian_blocks.push_back(
- make_pair(parameter_block->index(), j));
- }
- }
- sort(evaluated_jacobian_blocks.begin(), evaluated_jacobian_blocks.end());
+ GetOrderedParameterBlocks(program_, residual_id, &evaluated_jacobian_blocks);
// Where in the current row does the jacobian for a parameter block begin.
int col_pos = 0;
diff --git a/internal/ceres/compressed_row_jacobian_writer.h b/internal/ceres/compressed_row_jacobian_writer.h
index c103165..a722a7c 100644
--- a/internal/ceres/compressed_row_jacobian_writer.h
+++ b/internal/ceres/compressed_row_jacobian_writer.h
@@ -39,6 +39,7 @@
namespace ceres {
namespace internal {
+class CompressedRowSparseMatrix;
class Program;
class SparseMatrix;
@@ -49,11 +50,44 @@
: program_(program) {
}
+ // PopulateJacobianRowAndColumnBlockVectors sets col_blocks and
+ // row_blocks for a CompressedRowSparseMatrix, based on the
+ // parameter block sizes and residual sizes respectively from the
+ // program. This is useful when Solver::Options::use_block_amd =
+ // true;
+ //
+ // This function is static so that it is available to other jacobian
+ // writers which use CompressedRowSparseMatrix (or derived types).
+ // (Jacobian writers do not fall under any type hierarchy; they only
+ // have to provide an interface as specified in program_evaluator.h).
+ static void PopulateJacobianRowAndColumnBlockVectors(
+ const Program* program,
+ CompressedRowSparseMatrix* jacobian);
+
+ // It is necessary to determine the order of the jacobian blocks
+ // before copying them into a CompressedRowSparseMatrix (or derived
+ // type). Just because a cost function uses parameter blocks 1
+ // after 2 in its arguments does not mean that the block 1 occurs
+ // before block 2 in the column layout of the jacobian. Thus,
+ // GetOrderedParameterBlocks determines the order by sorting the
+ // jacobian blocks by their position in the state vector.
+ //
+ // This function is static so that it is available to other jacobian
+ // writers which use CompressedRowSparseMatrix (or derived types).
+ // (Jacobian writers do not fall under any type hierarchy; they only
+ // have to provide an interface as specified in
+ // program_evaluator.h).
+ static void GetOrderedParameterBlocks(
+ const Program* program,
+ int residual_id,
+ vector<pair<int, int> >* evaluated_jacobian_blocks);
+
// JacobianWriter interface.
- // Since the compressed row matrix has different layout than that assumed by
- // the cost functions, use scratch space to store the jacobians temporarily
- // then copy them over to the larger jacobian in the Write() function.
+ // Since the compressed row matrix has different layout than that
+ // assumed by the cost functions, use scratch space to store the
+ // jacobians temporarily then copy them over to the larger jacobian
+ // in the Write() function.
ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) {
return ScratchEvaluatePreparer::Create(*program_, num_threads);
}
diff --git a/internal/ceres/compressed_row_sparse_matrix.cc b/internal/ceres/compressed_row_sparse_matrix.cc
index e200c92..7993ed6 100644
--- a/internal/ceres/compressed_row_sparse_matrix.cc
+++ b/internal/ceres/compressed_row_sparse_matrix.cc
@@ -31,6 +31,7 @@
#include "ceres/compressed_row_sparse_matrix.h"
#include <algorithm>
+#include <numeric>
#include <vector>
#include "ceres/crs_matrix.h"
#include "ceres/internal/port.h"
@@ -124,7 +125,7 @@
// Find the cumulative sum of the row counts.
for (int i = 1; i < num_rows_ + 1; ++i) {
- rows_[i] += rows_[i-1];
+ rows_[i] += rows_[i - 1];
}
CHECK_EQ(num_nonzeros(), m.num_nonzeros());
@@ -215,11 +216,28 @@
num_rows_ -= delta_rows;
rows_.resize(num_rows_ + 1);
+
+ // Walk the list of row blocks until we reach the new number of rows
+ // and the drop the rest of the row blocks.
+ int num_row_blocks = 0;
+ int num_rows = 0;
+ while (num_row_blocks < row_blocks_.size() && num_rows < num_rows_) {
+ num_rows += row_blocks_[num_row_blocks];
+ ++num_row_blocks;
+ }
+
+ row_blocks_.resize(num_row_blocks);
}
void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) {
CHECK_EQ(m.num_cols(), num_cols_);
+ CHECK(row_blocks_.size() == 0 || m.row_blocks().size() !=0)
+ << "Cannot append a matrix with row blocks to one without and vice versa."
+ << "This matrix has : " << row_blocks_.size() << " row blocks."
+ << "The matrix being appended has: " << m.row_blocks().size()
+ << " row blocks.";
+
if (cols_.size() < num_nonzeros() + m.num_nonzeros()) {
cols_.resize(num_nonzeros() + m.num_nonzeros());
values_.resize(num_nonzeros() + m.num_nonzeros());
@@ -239,6 +257,7 @@
}
num_rows_ += m.num_rows();
+ row_blocks_.insert(row_blocks_.end(), m.row_blocks().begin(), m.row_blocks().end());
}
void CompressedRowSparseMatrix::ToTextFile(FILE* file) const {
@@ -267,6 +286,13 @@
matrix->values.resize(matrix->rows[matrix->num_rows]);
}
+void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) {
+ CHECK_GE(num_nonzeros, 0);
+
+ cols_.resize(num_nonzeros);
+ values_.resize(num_nonzeros);
+}
+
void CompressedRowSparseMatrix::SolveLowerTriangularInPlace(
double* solution) const {
for (int r = 0; r < num_rows_; ++r) {
@@ -358,9 +384,161 @@
}
transpose_rows[0] = 0;
+ *(transpose->mutable_row_blocks()) = col_blocks_;
+ *(transpose->mutable_col_blocks()) = row_blocks_;
+
return transpose;
}
+namespace {
+// A ProductTerm is a term in the outer product of a matrix with
+// itself.
+struct ProductTerm {
+ ProductTerm(const int row, const int col, const int index)
+ : row(row), col(col), index(index) {
+ }
+
+ bool operator<(const ProductTerm& right) const {
+ if (row == right.row) {
+ if (col == right.col) {
+ return index < right.index;
+ }
+ return col < right.col;
+ }
+ return row < right.row;
+ }
+
+ int row;
+ int col;
+ int index;
+};
+
+CompressedRowSparseMatrix*
+CompressAndFillProgram(const int num_rows,
+ const int num_cols,
+ const vector<ProductTerm>& product,
+ vector<int>* program) {
+ CHECK_GT(product.size(), 0);
+
+ // Count the number of unique product term, which in turn is the
+ // number of non-zeros in the outer product.
+ int num_nonzeros = 1;
+ for (int i = 1; i < product.size(); ++i) {
+ if (product[i].row != product[i - 1].row ||
+ product[i].col != product[i - 1].col) {
+ ++num_nonzeros;
+ }
+ }
+
+ CompressedRowSparseMatrix* matrix =
+ new CompressedRowSparseMatrix(num_rows, num_cols, num_nonzeros);
+
+ int* crsm_rows = matrix->mutable_rows();
+ std::fill(crsm_rows, crsm_rows + num_rows + 1, 0);
+ int* crsm_cols = matrix->mutable_cols();
+ std::fill(crsm_cols, crsm_cols + num_nonzeros, 0);
+
+ CHECK_NOTNULL(program)->clear();
+ program->resize(product.size());
+
+ // Iterate over the sorted product terms. This means each row is
+ // filled one at a time, and we are able to assign a position in the
+ // values array to each term.
+ //
+ // If terms repeat, i.e., they contribute to the same entry in the
+ // result matrix), then they do not affect the sparsity structure of
+ // the result matrix.
+ int nnz = 0;
+ crsm_cols[0] = product[0].col;
+ crsm_rows[product[0].row + 1]++;
+ (*program)[product[0].index] = nnz;
+ for (int i = 1; i < product.size(); ++i) {
+ const ProductTerm& previous = product[i - 1];
+ const ProductTerm& current = product[i];
+
+ // Sparsity structure is updated only if the term is not a repeat.
+ if (previous.row != current.row || previous.col != current.col) {
+ crsm_cols[++nnz] = current.col;
+ crsm_rows[current.row + 1]++;
+ }
+
+ // All terms get assigned the position in the values array where
+ // their value is accumulated.
+ (*program)[current.index] = nnz;
+ }
+
+ for (int i = 1; i < num_rows + 1; ++i) {
+ crsm_rows[i] += crsm_rows[i - 1];
+ }
+
+ return matrix;
+}
+
+} // namespace
+
+CompressedRowSparseMatrix*
+CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
+ const CompressedRowSparseMatrix& m,
+ vector<int>* program) {
+ CHECK_NOTNULL(program)->clear();
+ CHECK_GT(m.num_nonzeros(), 0) << "Congratulations, "
+ << "you found a bug in Ceres. Please report it.";
+
+ vector<ProductTerm> product;
+ const vector<int>& row_blocks = m.row_blocks();
+ int row_block_begin = 0;
+ // Iterate over row blocks
+ for (int row_block = 0; row_block < row_blocks.size(); ++row_block) {
+ const int row_block_end = row_block_begin + row_blocks[row_block];
+ // Compute the outer product terms for just one row per row block.
+ const int r = row_block_begin;
+ // Compute the lower triangular part of the product.
+ for (int idx1 = m.rows()[r]; idx1 < m.rows()[r + 1]; ++idx1) {
+ for (int idx2 = m.rows()[r]; idx2 <= idx1; ++idx2) {
+ product.push_back(ProductTerm(m.cols()[idx1], m.cols()[idx2], product.size()));
+ }
+ }
+ row_block_begin = row_block_end;
+ }
+ CHECK_EQ(row_block_begin, m.num_rows());
+ sort(product.begin(), product.end());
+ return CompressAndFillProgram(m.num_cols(), m.num_cols(), product, program);
+}
+
+void CompressedRowSparseMatrix::ComputeOuterProduct(
+ const CompressedRowSparseMatrix& m,
+ const vector<int>& program,
+ CompressedRowSparseMatrix* result) {
+ result->SetZero();
+ double* values = result->mutable_values();
+ const vector<int>& row_blocks = m.row_blocks();
+
+ int cursor = 0;
+ int row_block_begin = 0;
+ const double* m_values = m.values();
+ const int* m_rows = m.rows();
+ // Iterate over row blocks.
+ for (int row_block = 0; row_block < row_blocks.size(); ++row_block) {
+ const int row_block_end = row_block_begin + row_blocks[row_block];
+ const int saved_cursor = cursor;
+ for (int r = row_block_begin; r < row_block_end; ++r) {
+ // Reuse the program segment for each row in this row block.
+ cursor = saved_cursor;
+ const int row_begin = m_rows[r];
+ const int row_end = m_rows[r + 1];
+ for (int idx1 = row_begin; idx1 < row_end; ++idx1) {
+ const double v1 = m_values[idx1];
+ for (int idx2 = row_begin; idx2 <= idx1; ++idx2, ++cursor) {
+ values[program[cursor]] += v1 * m_values[idx2];
+ }
+ }
+ }
+ row_block_begin = row_block_end;
+ }
+
+ CHECK_EQ(row_block_begin, m.num_rows());
+ CHECK_EQ(cursor, program.size());
+}
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/compressed_row_sparse_matrix.h b/internal/ceres/compressed_row_sparse_matrix.h
index c5721eb..a0ba7ee 100644
--- a/internal/ceres/compressed_row_sparse_matrix.h
+++ b/internal/ceres/compressed_row_sparse_matrix.h
@@ -115,6 +115,9 @@
const vector<int>& col_blocks() const { return col_blocks_; }
vector<int>* mutable_col_blocks() { return &col_blocks_; }
+ // Destructive array resizing method.
+ void SetMaxNumNonZeros(int num_nonzeros);
+
// Non-destructive array resizing method.
void set_num_rows(const int num_rows) { num_rows_ = num_rows; }
void set_num_cols(const int num_cols) { num_cols_ = num_cols; }
@@ -128,6 +131,32 @@
const double* diagonal,
const vector<int>& blocks);
+ // Compute the sparsity structure of the product m.transpose() * m
+ // and create a CompressedRowSparseMatrix corresponding to it.
+ //
+ // Also compute a "program" vector, which for every term in the
+ // outer product points to the entry in the values array of the
+ // result matrix where it should be accumulated.
+ //
+ // This program is used by the ComputeOuterProduct function below to
+ // compute the outer product.
+ //
+ // Since the entries of the program are the same for rows with the
+ // same sparsity structure, the program only stores the result for
+ // one row per row block. The ComputeOuterProduct function reuses
+ // this information for each row in the row block.
+ static CompressedRowSparseMatrix* CreateOuterProductMatrixAndProgram(
+ const CompressedRowSparseMatrix& m,
+ vector<int>* program);
+
+ // Compute the values array for the expression m.transpose() * m,
+ // where the matrix used to store the result and a program have been
+ // created using the CreateOuterProductMatrixAndProgram function
+ // above.
+ static void ComputeOuterProduct(const CompressedRowSparseMatrix& m,
+ const vector<int>& program,
+ CompressedRowSparseMatrix* result);
+
private:
int num_rows_;
int num_cols_;
diff --git a/internal/ceres/compressed_row_sparse_matrix_test.cc b/internal/ceres/compressed_row_sparse_matrix_test.cc
index 02109cc..999a661 100644
--- a/internal/ceres/compressed_row_sparse_matrix_test.cc
+++ b/internal/ceres/compressed_row_sparse_matrix_test.cc
@@ -30,11 +30,14 @@
#include "ceres/compressed_row_sparse_matrix.h"
+#include <numeric>
#include "ceres/casts.h"
#include "ceres/crs_matrix.h"
+#include "ceres/cxsparse.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/linear_least_squares_problems.h"
+#include "ceres/random.h"
#include "ceres/triplet_sparse_matrix.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -76,6 +79,14 @@
num_rows = tsm->num_rows();
num_cols = tsm->num_cols();
+
+ vector<int>* row_blocks = crsm->mutable_row_blocks();
+ row_blocks->resize(num_rows);
+ std::fill(row_blocks->begin(), row_blocks->end(), 1);
+
+ vector<int>* col_blocks = crsm->mutable_col_blocks();
+ col_blocks->resize(num_cols);
+ std::fill(col_blocks->begin(), col_blocks->end(), 1);
}
int num_rows;
@@ -126,6 +137,9 @@
}
TEST_F(CompressedRowSparseMatrixTest, DeleteRows) {
+ // Clear the row and column blocks as these are purely scalar tests.
+ crsm->mutable_row_blocks()->clear();
+ crsm->mutable_col_blocks()->clear();
for (int i = 0; i < num_rows; ++i) {
tsm->Resize(num_rows - i, num_cols);
crsm->DeleteRows(crsm->num_rows() - tsm->num_rows());
@@ -134,6 +148,10 @@
}
TEST_F(CompressedRowSparseMatrixTest, AppendRows) {
+ // Clear the row and column blocks as these are purely scalar tests.
+ crsm->mutable_row_blocks()->clear();
+ crsm->mutable_col_blocks()->clear();
+
for (int i = 0; i < num_rows; ++i) {
TripletSparseMatrix tsm_appendage(*tsm);
tsm_appendage.Resize(i, num_cols);
@@ -146,6 +164,47 @@
}
}
+TEST_F(CompressedRowSparseMatrixTest, AppendAndDeleteBlockDiagonalMatrix) {
+ int num_diagonal_rows = crsm->num_cols();
+
+ scoped_array<double> diagonal(new double[num_diagonal_rows]);
+ for (int i = 0; i < num_diagonal_rows; ++i) {
+ diagonal[i] =i;
+ }
+
+ vector<int> row_and_column_blocks;
+ row_and_column_blocks.push_back(1);
+ row_and_column_blocks.push_back(2);
+ row_and_column_blocks.push_back(2);
+
+ const vector<int> pre_row_blocks = crsm->row_blocks();
+ const vector<int> pre_col_blocks = crsm->col_blocks();
+
+ scoped_ptr<CompressedRowSparseMatrix> appendage(
+ CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(
+ diagonal.get(), row_and_column_blocks));
+ LOG(INFO) << appendage->row_blocks().size();
+
+ crsm->AppendRows(*appendage);
+
+ const vector<int> post_row_blocks = crsm->row_blocks();
+ const vector<int> post_col_blocks = crsm->col_blocks();
+
+ vector<int> expected_row_blocks = pre_row_blocks;
+ expected_row_blocks.insert(expected_row_blocks.end(),
+ row_and_column_blocks.begin(),
+ row_and_column_blocks.end());
+
+ vector<int> expected_col_blocks = pre_col_blocks;
+
+ EXPECT_EQ(expected_row_blocks, crsm->row_blocks());
+ EXPECT_EQ(expected_col_blocks, crsm->col_blocks());
+
+ crsm->DeleteRows(num_diagonal_rows);
+ EXPECT_EQ(crsm->row_blocks(), pre_row_blocks);
+ EXPECT_EQ(crsm->col_blocks(), pre_col_blocks);
+}
+
TEST_F(CompressedRowSparseMatrixTest, ToDenseMatrix) {
Matrix tsm_dense;
Matrix crsm_dense;
@@ -279,10 +338,22 @@
// 13 0 14 15 9 0
// 0 16 17 0 0 0
+ // Block structure:
+ // A A A A B B
+ // A A A A B B
+ // A A A A B B
+ // C C C C D D
+ // C C C C D D
+ // C C C C D D
+
CompressedRowSparseMatrix matrix(5, 6, 30);
int* rows = matrix.mutable_rows();
int* cols = matrix.mutable_cols();
double* values = matrix.mutable_values();
+ matrix.mutable_row_blocks()->push_back(3);
+ matrix.mutable_row_blocks()->push_back(3);
+ matrix.mutable_col_blocks()->push_back(4);
+ matrix.mutable_col_blocks()->push_back(2);
rows[0] = 0;
cols[0] = 1;
@@ -317,6 +388,16 @@
scoped_ptr<CompressedRowSparseMatrix> transpose(matrix.Transpose());
+ ASSERT_EQ(transpose->row_blocks().size(), matrix.col_blocks().size());
+ for (int i = 0; i < transpose->row_blocks().size(); ++i) {
+ EXPECT_EQ(transpose->row_blocks()[i], matrix.col_blocks()[i]);
+ }
+
+ ASSERT_EQ(transpose->col_blocks().size(), matrix.row_blocks().size());
+ for (int i = 0; i < transpose->col_blocks().size(); ++i) {
+ EXPECT_EQ(transpose->col_blocks()[i], matrix.row_blocks()[i]);
+ }
+
Matrix dense_matrix;
matrix.ToDenseMatrix(&dense_matrix);
@@ -325,5 +406,170 @@
EXPECT_NEAR((dense_matrix - dense_transpose.transpose()).norm(), 0.0, 1e-14);
}
+#ifndef CERES_NO_CXSPARSE
+
+struct RandomMatrixOptions {
+ int num_row_blocks;
+ int min_row_block_size;
+ int max_row_block_size;
+ int num_col_blocks;
+ int min_col_block_size;
+ int max_col_block_size;
+ double block_density;
+};
+
+CompressedRowSparseMatrix* CreateRandomCompressedRowSparseMatrix(
+ const RandomMatrixOptions& options) {
+ vector<int> row_blocks;
+ for (int i = 0; i < options.num_row_blocks; ++i) {
+ const int delta_block_size =
+ Uniform(options.max_row_block_size - options.min_row_block_size);
+ row_blocks.push_back(options.min_row_block_size + delta_block_size);
+ }
+
+ vector<int> col_blocks;
+ for (int i = 0; i < options.num_col_blocks; ++i) {
+ const int delta_block_size =
+ Uniform(options.max_col_block_size - options.min_col_block_size);
+ col_blocks.push_back(options.min_col_block_size + delta_block_size);
+ }
+
+ vector<int> rows;
+ vector<int> cols;
+ vector<double> values;
+
+ while (values.size() == 0) {
+ int row_block_begin = 0;
+ for (int r = 0; r < options.num_row_blocks; ++r) {
+ int col_block_begin = 0;
+ for (int c = 0; c < options.num_col_blocks; ++c) {
+ if (RandDouble() <= options.block_density) {
+ for (int i = 0; i < row_blocks[r]; ++i) {
+ for (int j = 0; j < col_blocks[c]; ++j) {
+ rows.push_back(row_block_begin + i);
+ cols.push_back(col_block_begin + j);
+ values.push_back(RandNormal());
+ }
+ }
+ }
+ col_block_begin += col_blocks[c];
+ }
+ row_block_begin += row_blocks[r];
+ }
+ }
+
+ const int num_rows = std::accumulate(row_blocks.begin(), row_blocks.end(), 0);
+ const int num_cols = std::accumulate(col_blocks.begin(), col_blocks.end(), 0);
+ const int num_nonzeros = values.size();
+
+ TripletSparseMatrix tsm(num_rows, num_cols, num_nonzeros);
+ std::copy(rows.begin(), rows.end(), tsm.mutable_rows());
+ std::copy(cols.begin(), cols.end(), tsm.mutable_cols());
+ std::copy(values.begin(), values.end(), tsm.mutable_values());
+ tsm.set_num_nonzeros(num_nonzeros);
+ CompressedRowSparseMatrix* matrix = new CompressedRowSparseMatrix(tsm);
+ (*matrix->mutable_row_blocks()) = row_blocks;
+ (*matrix->mutable_col_blocks()) = col_blocks;
+ return matrix;
+}
+
+void ToDenseMatrix(const cs_di* matrix, Matrix* dense_matrix) {
+ dense_matrix->resize(matrix->m, matrix->n);
+ dense_matrix->setZero();
+
+ for (int c = 0; c < matrix->n; ++c) {
+ for (int idx = matrix->p[c]; idx < matrix->p[c + 1]; ++idx) {
+ const int r = matrix->i[idx];
+ (*dense_matrix)(r, c) = matrix->x[idx];
+ }
+ }
+}
+
+TEST(CompressedRowSparseMatrix, ComputeOuterProduct) {
+ // "Randomly generated seed."
+ SetRandomState(29823);
+ int kMaxNumRowBlocks = 10;
+ int kMaxNumColBlocks = 10;
+ int kNumTrials = 10;
+
+ CXSparse cxsparse;
+ const double kTolerance = 1e-18;
+
+ // Create a random matrix, compute its outer product using CXSParse
+ // and ComputeOuterProduct. Convert both matrices to dense matrices
+ // and compare their upper triangular parts. They should be within
+ // kTolerance of each other.
+ for (int num_row_blocks = 1;
+ num_row_blocks < kMaxNumRowBlocks;
+ ++num_row_blocks) {
+ for (int num_col_blocks = 1;
+ num_col_blocks < kMaxNumColBlocks;
+ ++num_col_blocks) {
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+
+
+ RandomMatrixOptions options;
+ options.num_row_blocks = num_row_blocks;
+ options.num_col_blocks = num_col_blocks;
+ options.min_row_block_size = 1;
+ options.max_row_block_size = 5;
+ options.min_col_block_size = 1;
+ options.max_col_block_size = 10;
+ options.block_density = std::max(0.1, RandDouble());
+
+ VLOG(2) << "num row blocks: " << options.num_row_blocks;
+ VLOG(2) << "num col blocks: " << options.num_col_blocks;
+ VLOG(2) << "min row block size: " << options.min_row_block_size;
+ VLOG(2) << "max row block size: " << options.max_row_block_size;
+ VLOG(2) << "min col block size: " << options.min_col_block_size;
+ VLOG(2) << "max col block size: " << options.max_col_block_size;
+ VLOG(2) << "block density: " << options.block_density;
+
+ scoped_ptr<CompressedRowSparseMatrix> matrix(
+ CreateRandomCompressedRowSparseMatrix(options));
+
+ cs_di cs_matrix_transpose = cxsparse.CreateSparseMatrixTransposeView(matrix.get());
+ cs_di* cs_matrix = cxsparse.TransposeMatrix(&cs_matrix_transpose);
+ cs_di* expected_outer_product =
+ cxsparse.MatrixMatrixMultiply(&cs_matrix_transpose, cs_matrix);
+
+ vector<int> program;
+ scoped_ptr<CompressedRowSparseMatrix> outer_product(
+ CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
+ *matrix, &program));
+ CompressedRowSparseMatrix::ComputeOuterProduct(*matrix,
+ program,
+ outer_product.get());
+
+ cs_di actual_outer_product =
+ cxsparse.CreateSparseMatrixTransposeView(outer_product.get());
+
+ ASSERT_EQ(actual_outer_product.m, actual_outer_product.n);
+ ASSERT_EQ(expected_outer_product->m, expected_outer_product->n);
+ ASSERT_EQ(actual_outer_product.m, expected_outer_product->m);
+
+ Matrix actual_matrix;
+ Matrix expected_matrix;
+
+ ToDenseMatrix(expected_outer_product, &expected_matrix);
+ expected_matrix.triangularView<Eigen::StrictlyLower>().setZero();
+
+ ToDenseMatrix(&actual_outer_product, &actual_matrix);
+ const double diff_norm = (actual_matrix - expected_matrix).norm() / expected_matrix.norm();
+ ASSERT_NEAR(diff_norm, 0.0, kTolerance)
+ << "expected: \n"
+ << expected_matrix
+ << "\nactual: \n"
+ << actual_matrix;
+
+ cxsparse.Free(cs_matrix);
+ cxsparse.Free(expected_outer_product);
+ }
+ }
+ }
+}
+
+#endif // CERES_NO_CXSPARSE
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/conjugate_gradients_solver.cc b/internal/ceres/conjugate_gradients_solver.cc
index ae8e877..524cb8a 100644
--- a/internal/ceres/conjugate_gradients_solver.cc
+++ b/internal/ceres/conjugate_gradients_solver.cc
@@ -44,6 +44,7 @@
#include "ceres/fpclassify.h"
#include "ceres/internal/eigen.h"
#include "ceres/linear_operator.h"
+#include "ceres/stringprintf.h"
#include "ceres/types.h"
#include "glog/logging.h"
@@ -55,9 +56,6 @@
return ((x == 0.0) || (IsInfinite(x)));
}
-// Constant used in the MATLAB implementation ~ 2 * eps.
-const double kEpsilon = 2.2204e-16;
-
} // namespace
ConjugateGradientsSolver::ConjugateGradientsSolver(
@@ -76,17 +74,19 @@
CHECK_EQ(A->num_rows(), A->num_cols());
LinearSolver::Summary summary;
- summary.termination_type = MAX_ITERATIONS;
+ summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE;
+ summary.message = "Maximum number of iterations reached.";
summary.num_iterations = 0;
- int num_cols = A->num_cols();
+ const int num_cols = A->num_cols();
VectorRef xref(x, num_cols);
ConstVectorRef bref(b, num_cols);
- double norm_b = bref.norm();
+ const double norm_b = bref.norm();
if (norm_b == 0.0) {
xref.setZero();
- summary.termination_type = TOLERANCE;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Convergence. |b| = 0.";
return summary;
}
@@ -95,15 +95,16 @@
Vector z(num_cols);
Vector tmp(num_cols);
- double tol_r = per_solve_options.r_tolerance * norm_b;
+ const double tol_r = per_solve_options.r_tolerance * norm_b;
tmp.setZero();
A->RightMultiply(x, tmp.data());
r = bref - tmp;
double norm_r = r.norm();
-
if (norm_r <= tol_r) {
- summary.termination_type = TOLERANCE;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message =
+ StringPrintf("Convergence. |r| = %e <= %e.", norm_r, tol_r);
return summary;
}
@@ -115,8 +116,6 @@
for (summary.num_iterations = 1;
summary.num_iterations < options_.max_num_iterations;
++summary.num_iterations) {
- VLOG(3) << "cg iteration " << summary.num_iterations;
-
// Apply preconditioner
if (per_solve_options.preconditioner != NULL) {
z.setZero();
@@ -127,10 +126,9 @@
double last_rho = rho;
rho = r.dot(z);
-
if (IsZeroOrInfinity(rho)) {
- LOG(ERROR) << "Numerical failure. rho = " << rho;
- summary.termination_type = FAILURE;
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = StringPrintf("Numerical failure. rho = r'z = %e.", rho);
break;
};
@@ -139,8 +137,9 @@
} else {
double beta = rho / last_rho;
if (IsZeroOrInfinity(beta)) {
- LOG(ERROR) << "Numerical failure. beta = " << beta;
- summary.termination_type = FAILURE;
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = StringPrintf(
+ "Numerical failure. beta = rho_n / rho_{n-1} = %e.", beta);
break;
}
p = z + beta * p;
@@ -149,18 +148,18 @@
Vector& q = z;
q.setZero();
A->RightMultiply(p.data(), q.data());
- double pq = p.dot(q);
-
+ const double pq = p.dot(q);
if ((pq <= 0) || IsInfinite(pq)) {
- LOG(ERROR) << "Numerical failure. pq = " << pq;
- summary.termination_type = FAILURE;
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = StringPrintf("Numerical failure. p'q = %e.", pq);
break;
}
- double alpha = rho / pq;
+ const double alpha = rho / pq;
if (IsInfinite(alpha)) {
- LOG(ERROR) << "Numerical failure. alpha " << alpha;
- summary.termination_type = FAILURE;
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message =
+ StringPrintf("Numerical failure. alpha = rho / pq = %e", alpha);
break;
}
@@ -183,7 +182,7 @@
// Quadratic model based termination.
// Q1 = x'Ax - 2 * b' x.
- double Q1 = -1.0 * xref.dot(bref + r);
+ const double Q1 = -1.0 * xref.dot(bref + r);
// For PSD matrices A, let
//
@@ -207,21 +206,23 @@
// Journal of Computational and Applied Mathematics,
// 124(1-2), 45-59, 2000.
//
- double zeta = summary.num_iterations * (Q1 - Q0) / Q1;
- VLOG(3) << "Q termination: zeta " << zeta
- << " " << per_solve_options.q_tolerance;
+ const double zeta = summary.num_iterations * (Q1 - Q0) / Q1;
if (zeta < per_solve_options.q_tolerance) {
- summary.termination_type = TOLERANCE;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message =
+ StringPrintf("Convergence: zeta = %e < %e",
+ zeta,
+ per_solve_options.q_tolerance);
break;
}
Q0 = Q1;
// Residual based termination.
norm_r = r. norm();
- VLOG(3) << "R termination: norm_r " << norm_r
- << " " << tol_r;
if (norm_r <= tol_r) {
- summary.termination_type = TOLERANCE;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message =
+ StringPrintf("Convergence. |r| = %e <= %e.", norm_r, tol_r);
break;
}
}
diff --git a/internal/ceres/coordinate_descent_minimizer.cc b/internal/ceres/coordinate_descent_minimizer.cc
index c4da987..3b0553e 100644
--- a/internal/ceres/coordinate_descent_minimizer.cc
+++ b/internal/ceres/coordinate_descent_minimizer.cc
@@ -40,15 +40,15 @@
#include "ceres/evaluator.h"
#include "ceres/linear_solver.h"
#include "ceres/minimizer.h"
-#include "ceres/ordered_groups.h"
#include "ceres/parameter_block.h"
+#include "ceres/parameter_block_ordering.h"
#include "ceres/problem_impl.h"
#include "ceres/program.h"
#include "ceres/residual_block.h"
#include "ceres/solver.h"
-#include "ceres/solver_impl.h"
#include "ceres/trust_region_minimizer.h"
#include "ceres/trust_region_strategy.h"
+#include "ceres/parameter_block_ordering.h"
namespace ceres {
namespace internal {
@@ -227,10 +227,44 @@
minimizer_options.evaluator = evaluator.get();
minimizer_options.jacobian = jacobian.get();
minimizer_options.trust_region_strategy = trust_region_strategy.get();
+ minimizer_options.is_silent = true;
TrustRegionMinimizer minimizer;
minimizer.Minimize(minimizer_options, parameter, summary);
}
+bool CoordinateDescentMinimizer::IsOrderingValid(
+ const Program& program,
+ const ParameterBlockOrdering& ordering,
+ string* message) {
+ const map<int, set<double*> >& group_to_elements =
+ ordering.group_to_elements();
+
+ // Verify that each group is an independent set
+ map<int, set<double*> >::const_iterator it = group_to_elements.begin();
+ for ( ; it != group_to_elements.end(); ++it) {
+ if (!program.IsParameterBlockSetIndependent(it->second)) {
+ *message =
+ StringPrintf("The user-provided "
+ "parameter_blocks_for_inner_iterations does not "
+ "form an independent set. Group Id: %d", it->first);
+ return false;
+ }
+ }
+ return true;
+}
+
+// Find a recursive decomposition of the Hessian matrix as a set
+// of independent sets of decreasing size and invert it. This
+// seems to work better in practice, i.e., Cameras before
+// points.
+ParameterBlockOrdering* CoordinateDescentMinimizer::CreateOrdering(
+ const Program& program) {
+ scoped_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering);
+ ComputeRecursiveIndependentSetOrdering(program, ordering.get());
+ ordering->Reverse();
+ return ordering.release();
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/coordinate_descent_minimizer.h b/internal/ceres/coordinate_descent_minimizer.h
index 424acda..e324b38 100644
--- a/internal/ceres/coordinate_descent_minimizer.h
+++ b/internal/ceres/coordinate_descent_minimizer.h
@@ -37,12 +37,13 @@
#include "ceres/evaluator.h"
#include "ceres/minimizer.h"
#include "ceres/problem_impl.h"
-#include "ceres/program.h"
#include "ceres/solver.h"
namespace ceres {
namespace internal {
+class Program;
+
// Given a Program, and a ParameterBlockOrdering which partitions
// (non-exhaustively) the Hessian matrix into independent sets,
// perform coordinate descent on the parameter blocks in the
@@ -66,6 +67,17 @@
double* parameters,
Solver::Summary* summary);
+ // Verify that each group in the ordering forms an independent set.
+ static bool IsOrderingValid(const Program& program,
+ const ParameterBlockOrdering& ordering,
+ string* message);
+
+ // Find a recursive decomposition of the Hessian matrix as a set
+ // of independent sets of decreasing size and invert it. This
+ // seems to work better in practice, i.e., Cameras before
+ // points.
+ static ParameterBlockOrdering* CreateOrdering(const Program& program);
+
private:
void Solve(Program* program,
LinearSolver* linear_solver,
diff --git a/internal/ceres/corrector.cc b/internal/ceres/corrector.cc
index 60269a6..581fc6d 100644
--- a/internal/ceres/corrector.cc
+++ b/internal/ceres/corrector.cc
@@ -32,14 +32,14 @@
#include <cstddef>
#include <cmath>
+#include "ceres/internal/eigen.h"
#include "glog/logging.h"
namespace ceres {
namespace internal {
-Corrector::Corrector(double sq_norm, const double rho[3]) {
+Corrector::Corrector(const double sq_norm, const double rho[3]) {
CHECK_GE(sq_norm, 0.0);
- CHECK_GT(rho[1], 0.0);
sqrt_rho1_ = sqrt(rho[1]);
// If sq_norm = 0.0, the correction becomes trivial, the residual
@@ -84,6 +84,14 @@
return;
}
+ // We now require that the first derivative of the loss function be
+ // positive only if the second derivative is positive. This is
+ // because when the second derivative is non-positive, we do not use
+ // the second order correction suggested by BANS and instead use a
+ // simpler first order strategy which does not use a division by the
+ // gradient of the loss function.
+ CHECK_GT(rho[1], 0.0);
+
// Calculate the smaller of the two solutions to the equation
//
// 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0.
@@ -101,20 +109,25 @@
alpha_sq_norm_ = alpha / sq_norm;
}
-void Corrector::CorrectResiduals(int num_rows, double* residuals) {
+void Corrector::CorrectResiduals(const int num_rows, double* residuals) {
DCHECK(residuals != NULL);
// Equation 11 in BANS.
- for (int r = 0; r < num_rows; ++r) {
- residuals[r] *= residual_scaling_;
- }
+ VectorRef(residuals, num_rows) *= residual_scaling_;
}
-void Corrector::CorrectJacobian(int num_rows,
- int num_cols,
+void Corrector::CorrectJacobian(const int num_rows,
+ const int num_cols,
double* residuals,
double* jacobian) {
DCHECK(residuals != NULL);
DCHECK(jacobian != NULL);
+
+ // The common case (rho[2] <= 0).
+ if (alpha_sq_norm_ == 0.0) {
+ VectorRef(jacobian, num_rows * num_cols) *= sqrt_rho1_;
+ return;
+ }
+
// Equation 11 in BANS.
//
// J = sqrt(rho) * (J - alpha^2 r * r' J)
diff --git a/internal/ceres/corrector_test.cc b/internal/ceres/corrector_test.cc
index 55e7d6b..9355616 100644
--- a/internal/ceres/corrector_test.cc
+++ b/internal/ceres/corrector_test.cc
@@ -43,14 +43,14 @@
// If rho[1] is zero, the Corrector constructor should crash.
TEST(Corrector, ZeroGradientDeathTest) {
- const double kRho[] = {0.0, 0.0, 0.0};
+ const double kRho[] = {0.0, 0.0, 1.0};
EXPECT_DEATH_IF_SUPPORTED({Corrector c(1.0, kRho);},
".*");
}
// If rho[1] is negative, the Corrector constructor should crash.
TEST(Corrector, NegativeGradientDeathTest) {
- const double kRho[] = {0.0, -0.1, 0.0};
+ const double kRho[] = {0.0, -0.1, 1.0};
EXPECT_DEATH_IF_SUPPORTED({Corrector c(1.0, kRho);},
".*");
}
diff --git a/internal/ceres/cost_function_to_functor_test.cc b/internal/ceres/cost_function_to_functor_test.cc
index 90ccc82..fd828ce 100644
--- a/internal/ceres/cost_function_to_functor_test.cc
+++ b/internal/ceres/cost_function_to_functor_test.cc
@@ -42,9 +42,9 @@
EXPECT_EQ(cost_function.num_residuals(),
actual_cost_function.num_residuals());
const int num_residuals = cost_function.num_residuals();
- const vector<int16>& parameter_block_sizes =
+ const vector<int32>& parameter_block_sizes =
cost_function.parameter_block_sizes();
- const vector<int16>& actual_parameter_block_sizes =
+ const vector<int32>& actual_parameter_block_sizes =
actual_cost_function.parameter_block_sizes();
EXPECT_EQ(parameter_block_sizes.size(),
actual_parameter_block_sizes.size());
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index 19d545c..821be49 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -35,8 +35,28 @@
#endif
#include <algorithm>
+#include <cstdlib>
#include <utility>
#include <vector>
+#include "Eigen/SparseCore"
+
+// Suppress unused local variable warning from Eigen Ordering.h #included by
+// SparseQR in Eigen 3.2.0. This was fixed in Eigen 3.2.1, but 3.2.0 is still
+// widely used (Ubuntu 14.04), and Ceres won't compile otherwise due to -Werror.
+#if defined(_MSC_VER)
+#pragma warning( push )
+#pragma warning( disable : 4189 )
+#else
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
+#endif
+#include "Eigen/SparseQR"
+#if defined(_MSC_VER)
+#pragma warning( pop )
+#else
+#pragma GCC diagnostic pop
+#endif
+
#include "Eigen/SVD"
#include "ceres/compressed_col_sparse_matrix_utils.h"
#include "ceres/compressed_row_sparse_matrix.h"
@@ -52,40 +72,6 @@
namespace ceres {
namespace internal {
-namespace {
-
-// Per thread storage for SuiteSparse.
-#ifndef CERES_NO_SUITESPARSE
-
-struct PerThreadContext {
- explicit PerThreadContext(int num_rows)
- : solution(NULL),
- solution_set(NULL),
- y_workspace(NULL),
- e_workspace(NULL),
- rhs(NULL) {
- rhs = ss.CreateDenseVector(NULL, num_rows, num_rows);
- }
-
- ~PerThreadContext() {
- ss.Free(solution);
- ss.Free(solution_set);
- ss.Free(y_workspace);
- ss.Free(e_workspace);
- ss.Free(rhs);
- }
-
- cholmod_dense* solution;
- cholmod_sparse* solution_set;
- cholmod_dense* y_workspace;
- cholmod_dense* e_workspace;
- cholmod_dense* rhs;
- SuiteSparse ss;
-};
-
-#endif
-
-} // namespace
typedef vector<pair<const double*, const double*> > CovarianceBlocks;
@@ -164,9 +150,9 @@
}
if (offset == row_size) {
- LOG(WARNING) << "Unable to find covariance block for "
- << original_parameter_block1 << " "
- << original_parameter_block2;
+ LOG(ERROR) << "Unable to find covariance block for "
+ << original_parameter_block1 << " "
+ << original_parameter_block2;
return false;
}
@@ -347,8 +333,8 @@
// values of the parameter blocks. Thus iterating over the keys of
// parameter_block_to_row_index_ corresponds to iterating over the
// rows of the covariance matrix in order.
- int i = 0; // index into covariance_blocks.
- int cursor = 0; // index into the covariance matrix.
+ int i = 0; // index into covariance_blocks.
+ int cursor = 0; // index into the covariance matrix.
for (map<const double*, int>::const_iterator it =
parameter_block_to_row_index_.begin();
it != parameter_block_to_row_index_.end();
@@ -392,14 +378,18 @@
bool CovarianceImpl::ComputeCovarianceValues() {
switch (options_.algorithm_type) {
- case (DENSE_SVD):
+ case DENSE_SVD:
return ComputeCovarianceValuesUsingDenseSVD();
#ifndef CERES_NO_SUITESPARSE
- case (SPARSE_CHOLESKY):
- return ComputeCovarianceValuesUsingSparseCholesky();
- case (SPARSE_QR):
- return ComputeCovarianceValuesUsingSparseQR();
+ case SUITE_SPARSE_QR:
+ return ComputeCovarianceValuesUsingSuiteSparseQR();
+#else
+ LOG(ERROR) << "SuiteSparse is required to use the "
+ << "SUITE_SPARSE_QR algorithm.";
+ return false;
#endif
+ case EIGEN_SPARSE_QR:
+ return ComputeCovarianceValuesUsingEigenSparseQR();
default:
LOG(ERROR) << "Unsupported covariance estimation algorithm type: "
<< CovarianceAlgorithmTypeToString(options_.algorithm_type);
@@ -408,186 +398,7 @@
return false;
}
-bool CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky() {
- EventLogger event_logger(
- "CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky");
-#ifndef CERES_NO_SUITESPARSE
- if (covariance_matrix_.get() == NULL) {
- // Nothing to do, all zeros covariance matrix.
- return true;
- }
-
- SuiteSparse ss;
-
- CRSMatrix jacobian;
- problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
-
- event_logger.AddEvent("Evaluate");
- // m is a transposed view of the Jacobian.
- cholmod_sparse cholmod_jacobian_view;
- cholmod_jacobian_view.nrow = jacobian.num_cols;
- cholmod_jacobian_view.ncol = jacobian.num_rows;
- cholmod_jacobian_view.nzmax = jacobian.values.size();
- cholmod_jacobian_view.nz = NULL;
- cholmod_jacobian_view.p = reinterpret_cast<void*>(&jacobian.rows[0]);
- cholmod_jacobian_view.i = reinterpret_cast<void*>(&jacobian.cols[0]);
- cholmod_jacobian_view.x = reinterpret_cast<void*>(&jacobian.values[0]);
- cholmod_jacobian_view.z = NULL;
- cholmod_jacobian_view.stype = 0; // Matrix is not symmetric.
- cholmod_jacobian_view.itype = CHOLMOD_INT;
- cholmod_jacobian_view.xtype = CHOLMOD_REAL;
- cholmod_jacobian_view.dtype = CHOLMOD_DOUBLE;
- cholmod_jacobian_view.sorted = 1;
- cholmod_jacobian_view.packed = 1;
-
- cholmod_factor* factor = ss.AnalyzeCholesky(&cholmod_jacobian_view);
- event_logger.AddEvent("Symbolic Factorization");
- bool factorization_succeeded = ss.Cholesky(&cholmod_jacobian_view, factor);
- if (factorization_succeeded) {
- const double reciprocal_condition_number =
- cholmod_rcond(factor, ss.mutable_cc());
- if (reciprocal_condition_number <
- options_.min_reciprocal_condition_number) {
- LOG(WARNING) << "Cholesky factorization of J'J is not reliable. "
- << "Reciprocal condition number: "
- << reciprocal_condition_number << " "
- << "min_reciprocal_condition_number : "
- << options_.min_reciprocal_condition_number;
- factorization_succeeded = false;
- }
- }
-
- event_logger.AddEvent("Numeric Factorization");
- if (!factorization_succeeded) {
- ss.Free(factor);
- LOG(WARNING) << "Cholesky factorization failed.";
- return false;
- }
-
- const int num_rows = covariance_matrix_->num_rows();
- const int* rows = covariance_matrix_->rows();
- const int* cols = covariance_matrix_->cols();
- double* values = covariance_matrix_->mutable_values();
-
- // The following loop exploits the fact that the i^th column of A^{-1}
- // is given by the solution to the linear system
- //
- // A x = e_i
- //
- // where e_i is a vector with e(i) = 1 and all other entries zero.
- //
- // Since the covariance matrix is symmetric, the i^th row and column
- // are equal.
- //
- // The ifdef separates two different version of SuiteSparse. Newer
- // versions of SuiteSparse have the cholmod_solve2 function which
- // re-uses memory across calls.
-#if (SUITESPARSE_VERSION < 4002)
- cholmod_dense* rhs = ss.CreateDenseVector(NULL, num_rows, num_rows);
- double* rhs_x = reinterpret_cast<double*>(rhs->x);
-
- for (int r = 0; r < num_rows; ++r) {
- int row_begin = rows[r];
- int row_end = rows[r + 1];
- if (row_end == row_begin) {
- continue;
- }
-
- rhs_x[r] = 1.0;
- cholmod_dense* solution = ss.Solve(factor, rhs);
- double* solution_x = reinterpret_cast<double*>(solution->x);
- for (int idx = row_begin; idx < row_end; ++idx) {
- const int c = cols[idx];
- values[idx] = solution_x[c];
- }
- ss.Free(solution);
- rhs_x[r] = 0.0;
- }
-
- ss.Free(rhs);
-#else // SUITESPARSE_VERSION < 4002
-
- const int num_threads = options_.num_threads;
- vector<PerThreadContext*> contexts(num_threads);
- for (int i = 0; i < num_threads; ++i) {
- contexts[i] = new PerThreadContext(num_rows);
- }
-
- // The first call to cholmod_solve2 is not thread safe, since it
- // changes the factorization from supernodal to simplicial etc.
- {
- PerThreadContext* context = contexts[0];
- double* context_rhs_x = reinterpret_cast<double*>(context->rhs->x);
- context_rhs_x[0] = 1.0;
- cholmod_solve2(CHOLMOD_A,
- factor,
- context->rhs,
- NULL,
- &context->solution,
- &context->solution_set,
- &context->y_workspace,
- &context->e_workspace,
- context->ss.mutable_cc());
- context_rhs_x[0] = 0.0;
- }
-
-#pragma omp parallel for num_threads(num_threads) schedule(dynamic)
- for (int r = 0; r < num_rows; ++r) {
- int row_begin = rows[r];
- int row_end = rows[r + 1];
- if (row_end == row_begin) {
- continue;
- }
-
-# ifdef CERES_USE_OPENMP
- int thread_id = omp_get_thread_num();
-# else
- int thread_id = 0;
-# endif
-
- PerThreadContext* context = contexts[thread_id];
- double* context_rhs_x = reinterpret_cast<double*>(context->rhs->x);
- context_rhs_x[r] = 1.0;
-
- // TODO(sameeragarwal) There should be a more efficient way
- // involving the use of Bset but I am unable to make it work right
- // now.
- cholmod_solve2(CHOLMOD_A,
- factor,
- context->rhs,
- NULL,
- &context->solution,
- &context->solution_set,
- &context->y_workspace,
- &context->e_workspace,
- context->ss.mutable_cc());
-
- double* solution_x = reinterpret_cast<double*>(context->solution->x);
- for (int idx = row_begin; idx < row_end; ++idx) {
- const int c = cols[idx];
- values[idx] = solution_x[c];
- }
- context_rhs_x[r] = 0.0;
- }
-
- for (int i = 0; i < num_threads; ++i) {
- delete contexts[i];
- }
-
-#endif // SUITESPARSE_VERSION < 4002
-
- ss.Free(factor);
- event_logger.AddEvent("Inversion");
- return true;
-
-#else // CERES_NO_SUITESPARSE
-
- return false;
-
-#endif // CERES_NO_SUITESPARSE
-};
-
-bool CovarianceImpl::ComputeCovarianceValuesUsingSparseQR() {
+bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() {
EventLogger event_logger(
"CovarianceImpl::ComputeCovarianceValuesUsingSparseQR");
@@ -681,10 +492,10 @@
CHECK_NOTNULL(R);
if (rank < cholmod_jacobian.ncol) {
- LOG(WARNING) << "Jacobian matrix is rank deficient."
- << "Number of columns: " << cholmod_jacobian.ncol
- << " rank: " << rank;
- delete []permutation;
+ LOG(ERROR) << "Jacobian matrix is rank deficient. "
+ << "Number of columns: " << cholmod_jacobian.ncol
+ << " rank: " << rank;
+ free(permutation);
cholmod_l_free_sparse(&R, &cc);
cholmod_l_finish(&cc);
return false;
@@ -739,7 +550,7 @@
}
}
- delete []permutation;
+ free(permutation);
cholmod_l_free_sparse(&R, &cc);
cholmod_l_finish(&cc);
event_logger.AddEvent("Inversion");
@@ -807,11 +618,11 @@
if (automatic_truncation) {
break;
} else {
- LOG(WARNING) << "Cholesky factorization of J'J is not reliable. "
- << "Reciprocal condition number: "
- << singular_value_ratio * singular_value_ratio << " "
- << "min_reciprocal_condition_number : "
- << options_.min_reciprocal_condition_number;
+ LOG(ERROR) << "Cholesky factorization of J'J is not reliable. "
+ << "Reciprocal condition number: "
+ << singular_value_ratio * singular_value_ratio << " "
+ << "min_reciprocal_condition_number: "
+ << options_.min_reciprocal_condition_number;
return false;
}
}
@@ -839,7 +650,102 @@
}
event_logger.AddEvent("CopyToCovarianceMatrix");
return true;
-};
+}
+
+bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() {
+ EventLogger event_logger(
+ "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR");
+ if (covariance_matrix_.get() == NULL) {
+ // Nothing to do, all zeros covariance matrix.
+ return true;
+ }
+
+ CRSMatrix jacobian;
+ problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
+ event_logger.AddEvent("Evaluate");
+
+ typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
+
+ // Convert the matrix to column major order as required by SparseQR.
+ EigenSparseMatrix sparse_jacobian =
+ Eigen::MappedSparseMatrix<double, Eigen::RowMajor>(
+ jacobian.num_rows, jacobian.num_cols,
+ static_cast<int>(jacobian.values.size()),
+ jacobian.rows.data(), jacobian.cols.data(), jacobian.values.data());
+ event_logger.AddEvent("ConvertToSparseMatrix");
+
+ Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int> >
+ qr_solver(sparse_jacobian);
+ event_logger.AddEvent("QRDecomposition");
+
+ if(qr_solver.info() != Eigen::Success) {
+ LOG(ERROR) << "Eigen::SparseQR decomposition failed.";
+ return false;
+ }
+
+ if (qr_solver.rank() < jacobian.num_cols) {
+ LOG(ERROR) << "Jacobian matrix is rank deficient. "
+ << "Number of columns: " << jacobian.num_cols
+ << " rank: " << qr_solver.rank();
+ return false;
+ }
+
+ const int* rows = covariance_matrix_->rows();
+ const int* cols = covariance_matrix_->cols();
+ double* values = covariance_matrix_->mutable_values();
+
+ // Compute the inverse column permutation used by QR factorization.
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation =
+ qr_solver.colsPermutation().inverse();
+
+ // The following loop exploits the fact that the i^th column of A^{-1}
+ // is given by the solution to the linear system
+ //
+ // A x = e_i
+ //
+ // where e_i is a vector with e(i) = 1 and all other entries zero.
+ //
+ // Since the covariance matrix is symmetric, the i^th row and column
+ // are equal.
+ const int num_cols = jacobian.num_cols;
+ const int num_threads = options_.num_threads;
+ scoped_array<double> workspace(new double[num_threads * num_cols]);
+
+#pragma omp parallel for num_threads(num_threads) schedule(dynamic)
+ for (int r = 0; r < num_cols; ++r) {
+ const int row_begin = rows[r];
+ const int row_end = rows[r + 1];
+ if (row_end == row_begin) {
+ continue;
+ }
+
+# ifdef CERES_USE_OPENMP
+ int thread_id = omp_get_thread_num();
+# else
+ int thread_id = 0;
+# endif
+
+ double* solution = workspace.get() + thread_id * num_cols;
+ SolveRTRWithSparseRHS<int>(
+ num_cols,
+ qr_solver.matrixR().innerIndexPtr(),
+ qr_solver.matrixR().outerIndexPtr(),
+ &qr_solver.matrixR().data().value(0),
+ inverse_permutation.indices().coeff(r),
+ solution);
+
+ // Assign the values of the computed covariance using the
+ // inverse permutation used in the QR factorization.
+ for (int idx = row_begin; idx < row_end; ++idx) {
+ const int c = cols[idx];
+ values[idx] = solution[inverse_permutation.indices().coeff(c)];
+ }
+ }
+
+ event_logger.AddEvent("Inverse");
+
+ return true;
+}
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/covariance_impl.h b/internal/ceres/covariance_impl.h
index 0e7e217..135f4a1 100644
--- a/internal/ceres/covariance_impl.h
+++ b/internal/ceres/covariance_impl.h
@@ -64,9 +64,9 @@
ProblemImpl* problem);
bool ComputeCovarianceValues();
- bool ComputeCovarianceValuesUsingSparseCholesky();
- bool ComputeCovarianceValuesUsingSparseQR();
bool ComputeCovarianceValuesUsingDenseSVD();
+ bool ComputeCovarianceValuesUsingSuiteSparseQR();
+ bool ComputeCovarianceValuesUsingEigenSparseQR();
const CompressedRowSparseMatrix* covariance_matrix() const {
return covariance_matrix_.get();
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc
index f3a5051..6c506b7 100644
--- a/internal/ceres/covariance_test.cc
+++ b/internal/ceres/covariance_test.cc
@@ -125,7 +125,7 @@
class UnaryCostFunction: public CostFunction {
public:
UnaryCostFunction(const int num_residuals,
- const int16 parameter_block_size,
+ const int32 parameter_block_size,
const double* jacobian)
: jacobian_(jacobian, jacobian + num_residuals * parameter_block_size) {
set_num_residuals(num_residuals);
@@ -158,8 +158,8 @@
class BinaryCostFunction: public CostFunction {
public:
BinaryCostFunction(const int num_residuals,
- const int16 parameter_block1_size,
- const int16 parameter_block2_size,
+ const int32 parameter_block1_size,
+ const int32 parameter_block2_size,
const double* jacobian1,
const double* jacobian2)
: jacobian1_(jacobian1,
@@ -400,15 +400,15 @@
Covariance::Options options;
#ifndef CERES_NO_SUITESPARSE
- options.algorithm_type = SPARSE_CHOLESKY;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
-
- options.algorithm_type = SPARSE_QR;
+ options.algorithm_type = SUITE_SPARSE_QR;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+ options.algorithm_type = EIGEN_SPARSE_QR;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
#ifdef CERES_USE_OPENMP
@@ -448,15 +448,15 @@
options.num_threads = 4;
#ifndef CERES_NO_SUITESPARSE
- options.algorithm_type = SPARSE_CHOLESKY;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
-
- options.algorithm_type = SPARSE_QR;
+ options.algorithm_type = SUITE_SPARSE_QR;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+ options.algorithm_type = EIGEN_SPARSE_QR;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
#endif // CERES_USE_OPENMP
@@ -497,15 +497,15 @@
Covariance::Options options;
#ifndef CERES_NO_SUITESPARSE
- options.algorithm_type = SPARSE_CHOLESKY;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
-
- options.algorithm_type = SPARSE_QR;
+ options.algorithm_type = SUITE_SPARSE_QR;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+ options.algorithm_type = EIGEN_SPARSE_QR;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
TEST_F(CovarianceTest, LocalParameterization) {
@@ -553,15 +553,15 @@
Covariance::Options options;
#ifndef CERES_NO_SUITESPARSE
- options.algorithm_type = SPARSE_CHOLESKY;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
-
- options.algorithm_type = SPARSE_QR;
+ options.algorithm_type = SUITE_SPARSE_QR;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+ options.algorithm_type = EIGEN_SPARSE_QR;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
@@ -727,7 +727,7 @@
parameter_block_size_,
jacobian.data()),
NULL,
- block_i );
+ block_i);
for (int j = i; j < num_parameter_blocks_; ++j) {
double* block_j = parameters_.get() + j * parameter_block_size_;
all_covariance_blocks_.push_back(make_pair(block_i, block_j));
@@ -781,8 +781,7 @@
#if !defined(CERES_NO_SUITESPARSE) && defined(CERES_USE_OPENMP)
TEST_F(LargeScaleCovarianceTest, Parallel) {
- ComputeAndCompare(SPARSE_CHOLESKY, 4);
- ComputeAndCompare(SPARSE_QR, 4);
+ ComputeAndCompare(SUITE_SPARSE_QR, 4);
}
#endif // !defined(CERES_NO_SUITESPARSE) && defined(CERES_USE_OPENMP)
diff --git a/internal/ceres/cxsparse.cc b/internal/ceres/cxsparse.cc
index c6d7743..87503d0 100644
--- a/internal/ceres/cxsparse.cc
+++ b/internal/ceres/cxsparse.cc
@@ -28,6 +28,9 @@
//
// Author: strandmark@google.com (Petter Strandmark)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_CXSPARSE
#include "ceres/cxsparse.h"
@@ -175,8 +178,8 @@
cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) {
cs_di_sparse tsm_wrapper;
- tsm_wrapper.nzmax = tsm->num_nonzeros();;
- tsm_wrapper.nz = tsm->num_nonzeros();;
+ tsm_wrapper.nzmax = tsm->num_nonzeros();
+ tsm_wrapper.nz = tsm->num_nonzeros();
tsm_wrapper.m = tsm->num_rows();
tsm_wrapper.n = tsm->num_cols();
tsm_wrapper.p = tsm->mutable_cols();
diff --git a/internal/ceres/cxsparse.h b/internal/ceres/cxsparse.h
index cd87908..5868401 100644
--- a/internal/ceres/cxsparse.h
+++ b/internal/ceres/cxsparse.h
@@ -31,11 +31,13 @@
#ifndef CERES_INTERNAL_CXSPARSE_H_
#define CERES_INTERNAL_CXSPARSE_H_
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_CXSPARSE
#include <vector>
#include "cs.h"
-#include "ceres/internal/port.h"
namespace ceres {
namespace internal {
@@ -127,9 +129,13 @@
#else // CERES_NO_CXSPARSE
-class CXSparse {};
typedef void cs_dis;
+class CXSparse {
+ public:
+ void Free(void*) {};
+
+};
#endif // CERES_NO_CXSPARSE
#endif // CERES_INTERNAL_CXSPARSE_H_
diff --git a/internal/ceres/dense_normal_cholesky_solver.cc b/internal/ceres/dense_normal_cholesky_solver.cc
index fbf3cbe..f44d6da 100644
--- a/internal/ceres/dense_normal_cholesky_solver.cc
+++ b/internal/ceres/dense_normal_cholesky_solver.cc
@@ -95,9 +95,19 @@
LinearSolver::Summary summary;
summary.num_iterations = 1;
- summary.termination_type = TOLERANCE;
- VectorRef(x, num_cols) =
- lhs.selfadjointView<Eigen::Upper>().llt().solve(rhs);
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ Eigen::LLT<Matrix, Eigen::Upper> llt =
+ lhs.selfadjointView<Eigen::Upper>().llt();
+
+ if (llt.info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = "Eigen LLT decomposition failed.";
+ } else {
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
+ }
+
+ VectorRef(x, num_cols) = llt.solve(rhs);
event_logger.AddEvent("Solve");
return summary;
}
@@ -142,14 +152,14 @@
A->matrix().transpose() * ConstVectorRef(b, A->num_rows());
event_logger.AddEvent("Product");
- const int info = LAPACK::SolveInPlaceUsingCholesky(num_cols, lhs.data(), x);
- event_logger.AddEvent("Solve");
-
LinearSolver::Summary summary;
summary.num_iterations = 1;
- summary.termination_type = info == 0 ? TOLERANCE : FAILURE;
-
- event_logger.AddEvent("TearDown");
+ summary.termination_type =
+ LAPACK::SolveInPlaceUsingCholesky(num_cols,
+ lhs.data(),
+ x,
+ &summary.message);
+ event_logger.AddEvent("Solve");
return summary;
}
} // namespace internal
diff --git a/internal/ceres/dense_qr_solver.cc b/internal/ceres/dense_qr_solver.cc
index d76d58b..4388357 100644
--- a/internal/ceres/dense_qr_solver.cc
+++ b/internal/ceres/dense_qr_solver.cc
@@ -60,6 +60,7 @@
return SolveUsingLAPACK(A, b, per_solve_options, x);
}
}
+
LinearSolver::Summary DenseQRSolver::SolveUsingLAPACK(
DenseSparseMatrix* A,
const double* b,
@@ -100,21 +101,18 @@
work_.resize(work_size);
}
- const int info = LAPACK::SolveUsingQR(lhs_.rows(),
- lhs_.cols(),
- lhs_.data(),
- work_.rows(),
- work_.data(),
- rhs_.data());
- event_logger.AddEvent("Solve");
-
LinearSolver::Summary summary;
summary.num_iterations = 1;
- if (info == 0) {
+ summary.termination_type = LAPACK::SolveInPlaceUsingQR(lhs_.rows(),
+ lhs_.cols(),
+ lhs_.data(),
+ work_.rows(),
+ work_.data(),
+ rhs_.data(),
+ &summary.message);
+ event_logger.AddEvent("Solve");
+ if (summary.termination_type == LINEAR_SOLVER_SUCCESS) {
VectorRef(x, num_cols) = rhs_.head(num_cols);
- summary.termination_type = TOLERANCE;
- } else {
- summary.termination_type = FAILURE;
}
event_logger.AddEvent("TearDown");
@@ -161,7 +159,8 @@
// is good enough or not.
LinearSolver::Summary summary;
summary.num_iterations = 1;
- summary.termination_type = TOLERANCE;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
event_logger.AddEvent("TearDown");
return summary;
diff --git a/internal/ceres/dogleg_strategy.cc b/internal/ceres/dogleg_strategy.cc
index c85c8e5..f29376d 100644
--- a/internal/ceres/dogleg_strategy.cc
+++ b/internal/ceres/dogleg_strategy.cc
@@ -99,7 +99,7 @@
}
TrustRegionStrategy::Summary summary;
summary.num_iterations = 0;
- summary.termination_type = TOLERANCE;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
return summary;
}
@@ -135,7 +135,11 @@
summary.num_iterations = linear_solver_summary.num_iterations;
summary.termination_type = linear_solver_summary.termination_type;
- if (linear_solver_summary.termination_type != FAILURE) {
+ if (linear_solver_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) {
+ return summary;
+ }
+
+ if (linear_solver_summary.termination_type != LINEAR_SOLVER_FAILURE) {
switch (dogleg_type_) {
// Interpolate the Cauchy point and the Gauss-Newton step.
case TRADITIONAL_DOGLEG:
@@ -146,7 +150,7 @@
// Cauchy point and the (Gauss-)Newton step.
case SUBSPACE_DOGLEG:
if (!ComputeSubspaceModel(jacobian)) {
- summary.termination_type = FAILURE;
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
break;
}
ComputeSubspaceDoglegStep(step);
@@ -513,7 +517,7 @@
const double* residuals) {
const int n = jacobian->num_cols();
LinearSolver::Summary linear_solver_summary;
- linear_solver_summary.termination_type = FAILURE;
+ linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE;
// The Jacobian matrix is often quite poorly conditioned. Thus it is
// necessary to add a diagonal matrix at the bottom to prevent the
@@ -526,7 +530,7 @@
// If the solve fails, the multiplier to the diagonal is increased
// up to max_mu_ by a factor of mu_increase_factor_ every time. If
// the linear solver is still not successful, the strategy returns
- // with FAILURE.
+ // with LINEAR_SOLVER_FAILURE.
//
// Next time when a new Gauss-Newton step is requested, the
// multiplier starts out from the last successful solve.
@@ -579,17 +583,21 @@
}
}
- if (linear_solver_summary.termination_type == FAILURE ||
+ if (linear_solver_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) {
+ return linear_solver_summary;
+ }
+
+ if (linear_solver_summary.termination_type == LINEAR_SOLVER_FAILURE ||
!IsArrayValid(n, gauss_newton_step_.data())) {
mu_ *= mu_increase_factor_;
VLOG(2) << "Increasing mu " << mu_;
- linear_solver_summary.termination_type = FAILURE;
+ linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE;
continue;
}
break;
}
- if (linear_solver_summary.termination_type != FAILURE) {
+ if (linear_solver_summary.termination_type != LINEAR_SOLVER_FAILURE) {
// The scaled Gauss-Newton step is D * GN:
//
// - (D^-1 J^T J D^-1)^-1 (D^-1 g)
diff --git a/internal/ceres/dogleg_strategy_test.cc b/internal/ceres/dogleg_strategy_test.cc
index ace635f..795719d 100644
--- a/internal/ceres/dogleg_strategy_test.cc
+++ b/internal/ceres/dogleg_strategy_test.cc
@@ -144,7 +144,7 @@
residual_.data(),
x_.data());
- EXPECT_NE(summary.termination_type, FAILURE);
+ EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE);
EXPECT_LE(x_.norm(), options_.initial_radius * (1.0 + 4.0 * kEpsilon));
}
@@ -164,7 +164,7 @@
residual_.data(),
x_.data());
- EXPECT_NE(summary.termination_type, FAILURE);
+ EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE);
EXPECT_LE(x_.norm(), options_.initial_radius * (1.0 + 4.0 * kEpsilon));
}
@@ -184,7 +184,7 @@
residual_.data(),
x_.data());
- EXPECT_NE(summary.termination_type, FAILURE);
+ EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE);
EXPECT_NEAR(x_(0), 1.0, kToleranceLoose);
EXPECT_NEAR(x_(1), 1.0, kToleranceLoose);
EXPECT_NEAR(x_(2), 1.0, kToleranceLoose);
@@ -246,7 +246,7 @@
residual_.data(),
x_.data());
- EXPECT_NE(summary.termination_type, FAILURE);
+ EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE);
EXPECT_NEAR(x_(0), 0.0, kToleranceLoose);
EXPECT_NEAR(x_(1), 0.0, kToleranceLoose);
EXPECT_NEAR(x_(2), options_.initial_radius, kToleranceLoose);
@@ -274,7 +274,7 @@
residual_.data(),
x_.data());
- EXPECT_NE(summary.termination_type, FAILURE);
+ EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE);
EXPECT_NEAR(x_(0), 0.0, kToleranceLoose);
EXPECT_NEAR(x_(1), 0.0, kToleranceLoose);
EXPECT_NEAR(x_(2), 1.0, kToleranceLoose);
diff --git a/internal/ceres/dynamic_compressed_row_finalizer.h b/internal/ceres/dynamic_compressed_row_finalizer.h
new file mode 100644
index 0000000..5e6b0d8
--- /dev/null
+++ b/internal/ceres/dynamic_compressed_row_finalizer.h
@@ -0,0 +1,51 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: richie.stebbing@gmail.com (Richard Stebbing)
+
+#ifndef CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALIZER_H_
+#define CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALIZER_H_
+
+#include "ceres/casts.h"
+#include "ceres/dynamic_compressed_row_sparse_matrix.h"
+
+namespace ceres {
+namespace internal {
+
+struct DynamicCompressedRowJacobianFinalizer {
+ void operator()(SparseMatrix* base_jacobian, int num_parameters) {
+ DynamicCompressedRowSparseMatrix* jacobian =
+ down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian);
+ jacobian->Finalize(num_parameters);
+ }
+};
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALISER_H_
diff --git a/internal/ceres/dynamic_compressed_row_jacobian_writer.cc b/internal/ceres/dynamic_compressed_row_jacobian_writer.cc
new file mode 100644
index 0000000..2f01617
--- /dev/null
+++ b/internal/ceres/dynamic_compressed_row_jacobian_writer.cc
@@ -0,0 +1,107 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: richie.stebbing@gmail.com (Richard Stebbing)
+
+#include "ceres/compressed_row_jacobian_writer.h"
+#include "ceres/dynamic_compressed_row_jacobian_writer.h"
+#include "ceres/casts.h"
+#include "ceres/dynamic_compressed_row_sparse_matrix.h"
+#include "ceres/parameter_block.h"
+#include "ceres/program.h"
+#include "ceres/residual_block.h"
+
+namespace ceres {
+namespace internal {
+
+ScratchEvaluatePreparer*
+DynamicCompressedRowJacobianWriter::CreateEvaluatePreparers(int num_threads) {
+ return ScratchEvaluatePreparer::Create(*program_, num_threads);
+}
+
+SparseMatrix* DynamicCompressedRowJacobianWriter::CreateJacobian() const {
+ // Initialize `jacobian` with zero number of `max_num_nonzeros`.
+ const int num_residuals = program_->NumResiduals();
+ const int num_effective_parameters = program_->NumEffectiveParameters();
+
+ DynamicCompressedRowSparseMatrix* jacobian =
+ new DynamicCompressedRowSparseMatrix(num_residuals,
+ num_effective_parameters,
+ 0);
+
+ CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors(
+ program_, jacobian);
+
+ return jacobian;
+}
+
+void DynamicCompressedRowJacobianWriter::Write(int residual_id,
+ int residual_offset,
+ double **jacobians,
+ SparseMatrix* base_jacobian) {
+ DynamicCompressedRowSparseMatrix* jacobian =
+ down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian);
+
+ // Get the `residual_block` of interest.
+ const ResidualBlock* residual_block =
+ program_->residual_blocks()[residual_id];
+ const int num_residuals = residual_block->NumResiduals();
+
+ vector<pair<int, int> > evaluated_jacobian_blocks;
+ CompressedRowJacobianWriter::GetOrderedParameterBlocks(
+ program_, residual_id, &evaluated_jacobian_blocks);
+
+ // `residual_offset` is the residual row in the global jacobian.
+ // Empty the jacobian rows.
+ jacobian->ClearRows(residual_offset, num_residuals);
+
+ // Iterate over each parameter block.
+ for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) {
+ const ParameterBlock* parameter_block =
+ program_->parameter_blocks()[evaluated_jacobian_blocks[i].first];
+ const int parameter_block_jacobian_index =
+ evaluated_jacobian_blocks[i].second;
+ const int parameter_block_size = parameter_block->LocalSize();
+
+ // For each parameter block only insert its non-zero entries.
+ for (int r = 0; r < num_residuals; ++r) {
+ for (int c = 0; c < parameter_block_size; ++c) {
+ const double& v = jacobians[parameter_block_jacobian_index][
+ r * parameter_block_size + c];
+ // Only insert non-zero entries.
+ if (v != 0.0) {
+ jacobian->InsertEntry(
+ residual_offset + r, parameter_block->delta_offset() + c, v);
+ }
+ }
+ }
+ }
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/dynamic_compressed_row_jacobian_writer.h b/internal/ceres/dynamic_compressed_row_jacobian_writer.h
new file mode 100644
index 0000000..df9581b
--- /dev/null
+++ b/internal/ceres/dynamic_compressed_row_jacobian_writer.h
@@ -0,0 +1,83 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: richie.stebbing@gmail.com (Richard Stebbing)
+//
+// A jacobian writer that directly writes to dynamic compressed row sparse
+// matrices.
+
+#ifndef CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_
+#define CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_
+
+#include "ceres/evaluator.h"
+#include "ceres/scratch_evaluate_preparer.h"
+
+namespace ceres {
+namespace internal {
+
+class Program;
+class SparseMatrix;
+
+class DynamicCompressedRowJacobianWriter {
+ public:
+ DynamicCompressedRowJacobianWriter(Evaluator::Options /* ignored */,
+ Program* program)
+ : program_(program) {
+ }
+
+ // JacobianWriter interface.
+
+ // The compressed row matrix has different layout than that assumed by
+ // the cost functions. The scratch space is therefore used to store
+ // the jacobians (including zeros) temporarily before only the non-zero
+ // entries are copied over to the larger jacobian in `Write`.
+ ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads);
+
+ // Return a `DynamicCompressedRowSparseMatrix` which is filled by
+ // `Write`. Note that `Finalize` must be called to make the
+ // `CompressedRowSparseMatrix` interface valid.
+ SparseMatrix* CreateJacobian() const;
+
+ // Write only the non-zero jacobian entries for a residual block
+ // (specified by `residual_id`) into `base_jacobian`, starting at the row
+ // specifed by `residual_offset`.
+ //
+ // This method is thread-safe over residual blocks (each `residual_id`).
+ void Write(int residual_id,
+ int residual_offset,
+ double **jacobians,
+ SparseMatrix* base_jacobian);
+
+ private:
+ Program* program_;
+};
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_
diff --git a/internal/ceres/dynamic_compressed_row_sparse_matrix.cc b/internal/ceres/dynamic_compressed_row_sparse_matrix.cc
new file mode 100644
index 0000000..f285d52
--- /dev/null
+++ b/internal/ceres/dynamic_compressed_row_sparse_matrix.cc
@@ -0,0 +1,107 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: richie.stebbing@gmail.com (Richard Stebbing)
+
+#include <cstring>
+#include "ceres/dynamic_compressed_row_sparse_matrix.h"
+
+namespace ceres {
+namespace internal {
+
+DynamicCompressedRowSparseMatrix::DynamicCompressedRowSparseMatrix(
+ int num_rows,
+ int num_cols,
+ int initial_max_num_nonzeros)
+ : CompressedRowSparseMatrix(num_rows,
+ num_cols,
+ initial_max_num_nonzeros) {
+ dynamic_cols_.resize(num_rows);
+ dynamic_values_.resize(num_rows);
+ }
+
+void DynamicCompressedRowSparseMatrix::InsertEntry(int row,
+ int col,
+ const double& value) {
+ CHECK_GE(row, 0);
+ CHECK_LT(row, num_rows());
+ CHECK_GE(col, 0);
+ CHECK_LT(col, num_cols());
+ dynamic_cols_[row].push_back(col);
+ dynamic_values_[row].push_back(value);
+}
+
+void DynamicCompressedRowSparseMatrix::ClearRows(int row_start,
+ int num_rows) {
+ for (int r = 0; r < num_rows; ++r) {
+ const int i = row_start + r;
+ CHECK_GE(i, 0);
+ CHECK_LT(i, this->num_rows());
+ dynamic_cols_[i].resize(0);
+ dynamic_values_[i].resize(0);
+ }
+}
+
+void DynamicCompressedRowSparseMatrix::Finalize(int num_additional_elements) {
+ // `num_additional_elements` is provided as an argument so that additional
+ // storage can be reserved when it is known by the finalizer.
+ CHECK_GE(num_additional_elements, 0);
+
+ // Count the number of non-zeros and resize `cols_` and `values_`.
+ int num_jacobian_nonzeros = 0;
+ for (int i = 0; i < dynamic_cols_.size(); ++i) {
+ num_jacobian_nonzeros += dynamic_cols_[i].size();
+ }
+
+ SetMaxNumNonZeros(num_jacobian_nonzeros + num_additional_elements);
+
+ // Flatten `dynamic_cols_` into `cols_` and `dynamic_values_`
+ // into `values_`.
+ int index_into_values_and_cols = 0;
+ for (int i = 0; i < num_rows(); ++i) {
+ mutable_rows()[i] = index_into_values_and_cols;
+ const int num_nonzero_columns = dynamic_cols_[i].size();
+ if (num_nonzero_columns > 0) {
+ memcpy(mutable_cols() + index_into_values_and_cols,
+ &dynamic_cols_[i][0],
+ dynamic_cols_[i].size() * sizeof(dynamic_cols_[0][0]));
+ memcpy(mutable_values() + index_into_values_and_cols,
+ &dynamic_values_[i][0],
+ dynamic_values_[i].size() * sizeof(dynamic_values_[0][0]));
+ index_into_values_and_cols += dynamic_cols_[i].size();
+ }
+ }
+ mutable_rows()[num_rows()] = index_into_values_and_cols;
+
+ CHECK_EQ(index_into_values_and_cols, num_jacobian_nonzeros)
+ << "Ceres bug: final index into values_ and cols_ should be equal to "
+ << "the number of jacobian nonzeros. Please contact the developers!";
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/dynamic_compressed_row_sparse_matrix.h b/internal/ceres/dynamic_compressed_row_sparse_matrix.h
new file mode 100644
index 0000000..7a89a70
--- /dev/null
+++ b/internal/ceres/dynamic_compressed_row_sparse_matrix.h
@@ -0,0 +1,99 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: richie.stebbing@gmail.com (Richard Stebbing)
+//
+// A compressed row sparse matrix that provides an extended interface to
+// allow dynamic insertion of entries. This is provided for the use case
+// where the sparsity structure and number of non-zero entries is dynamic.
+// This flexibility is achieved by using an (internal) scratch space that
+// allows independent insertion of entries into each row (thread-safe).
+// Once insertion is complete, the `Finalize` method must be called to ensure
+// that the underlying `CompressedRowSparseMatrix` is consistent.
+//
+// This should only be used if you really do need a dynamic sparsity pattern.
+
+#ifndef CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_
+#define CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_
+
+#include "ceres/compressed_row_sparse_matrix.h"
+
+namespace ceres {
+namespace internal {
+
+class DynamicCompressedRowSparseMatrix : public CompressedRowSparseMatrix {
+ public:
+ // Set the number of rows and columns for the underlyig
+ // `CompressedRowSparseMatrix` and set the initial number of maximum non-zero
+ // entries. Note that following the insertion of entries, when `Finalize`
+ // is called the number of non-zeros is determined and all internal
+ // structures are adjusted as required. If you know the upper limit on the
+ // number of non-zeros, then passing this value here can prevent future
+ // memory reallocations which may improve performance. Otherwise, if no
+ // upper limit is available a value of 0 is sufficient.
+ //
+ // Typical usage of this class is to define a new instance with a given
+ // number of rows, columns and maximum number of non-zero elements
+ // (if available). Next, entries are inserted at row and column positions
+ // using `InsertEntry`. Finally, once all elements have been inserted,
+ // `Finalize` must be called to make the underlying
+ // `CompressedRowSparseMatrix` consistent.
+ DynamicCompressedRowSparseMatrix(int num_rows,
+ int num_cols,
+ int initial_max_num_nonzeros);
+
+ // Insert an entry at a given row and column position. This method is
+ // thread-safe across rows i.e. different threads can insert values
+ // simultaneously into different rows. It should be emphasised that this
+ // method always inserts a new entry and does not check for existing
+ // entries at the specified row and column position. Duplicate entries
+ // for a given row and column position will result in undefined
+ // behavior.
+ void InsertEntry(int row, int col, const double& value);
+
+ // Clear all entries for rows, starting from row index `row_start`
+ // and proceeding for `num_rows`.
+ void ClearRows(int row_start, int num_rows);
+
+ // Make the underlying internal `CompressedRowSparseMatrix` data structures
+ // consistent. Additional space for non-zero entries in the
+ // `CompressedRowSparseMatrix` can be reserved by specifying
+ // `num_additional_elements`. This is useful when it is known that rows will
+ // be appended to the `CompressedRowSparseMatrix` (e.g. appending a diagonal
+ // matrix to the jacobian) as it prevents need for future reallocation.
+ void Finalize(int num_additional_elements);
+
+ private:
+ vector<vector<int> > dynamic_cols_;
+ vector<vector<double> > dynamic_values_;
+};
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_
diff --git a/internal/ceres/dynamic_compressed_row_sparse_matrix_test.cc b/internal/ceres/dynamic_compressed_row_sparse_matrix_test.cc
new file mode 100644
index 0000000..03bfcb6
--- /dev/null
+++ b/internal/ceres/dynamic_compressed_row_sparse_matrix_test.cc
@@ -0,0 +1,217 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: richie.stebbing@gmail.com (Richard Stebbing)
+
+#include "ceres/dynamic_compressed_row_sparse_matrix.h"
+
+#include "ceres/casts.h"
+#include "ceres/compressed_row_sparse_matrix.h"
+#include "ceres/casts.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/linear_least_squares_problems.h"
+#include "ceres/triplet_sparse_matrix.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+class DynamicCompressedRowSparseMatrixTest : public ::testing::Test {
+ protected:
+ virtual void SetUp() {
+ num_rows = 7;
+ num_cols = 4;
+
+ // The number of additional elements reserved when `Finalize` is called
+ // should have no effect on the number of rows, columns or nonzeros.
+ // Set this to some nonzero value to be sure.
+ num_additional_elements = 13;
+
+ expected_num_nonzeros = num_rows * num_cols - min(num_rows, num_cols);
+
+ InitialiseDenseReference();
+ InitialiseSparseMatrixReferences();
+
+ dcrsm.reset(new DynamicCompressedRowSparseMatrix(num_rows,
+ num_cols,
+ 0));
+ }
+
+ void Finalize() {
+ dcrsm->Finalize(num_additional_elements);
+ }
+
+ void InitialiseDenseReference() {
+ dense.resize(num_rows, num_cols);
+ dense.setZero();
+ int num_nonzeros = 0;
+ for (int i = 0; i < (num_rows * num_cols); ++i) {
+ const int r = i / num_cols, c = i % num_cols;
+ if (r != c) {
+ dense(r, c) = i + 1;
+ ++num_nonzeros;
+ }
+ }
+ ASSERT_EQ(num_nonzeros, expected_num_nonzeros);
+ }
+
+ void InitialiseSparseMatrixReferences() {
+ std::vector<int> rows, cols;
+ std::vector<double> values;
+ for (int i = 0; i < (num_rows * num_cols); ++i) {
+ const int r = i / num_cols, c = i % num_cols;
+ if (r != c) {
+ rows.push_back(r);
+ cols.push_back(c);
+ values.push_back(i + 1);
+ }
+ }
+ ASSERT_EQ(values.size(), expected_num_nonzeros);
+
+ tsm.reset(new TripletSparseMatrix(num_rows,
+ num_cols,
+ expected_num_nonzeros));
+ std::copy(rows.begin(), rows.end(), tsm->mutable_rows());
+ std::copy(cols.begin(), cols.end(), tsm->mutable_cols());
+ std::copy(values.begin(), values.end(), tsm->mutable_values());
+ tsm->set_num_nonzeros(values.size());
+
+ Matrix dense_from_tsm;
+ tsm->ToDenseMatrix(&dense_from_tsm);
+ ASSERT_TRUE((dense.array() == dense_from_tsm.array()).all());
+
+ crsm.reset(new CompressedRowSparseMatrix(*tsm));
+ Matrix dense_from_crsm;
+ crsm->ToDenseMatrix(&dense_from_crsm);
+ ASSERT_TRUE((dense.array() == dense_from_crsm.array()).all());
+ }
+
+ void InsertNonZeroEntriesFromDenseReference() {
+ for (int r = 0; r < num_rows; ++r) {
+ for (int c = 0; c < num_cols; ++c) {
+ const double& v = dense(r, c);
+ if (v != 0.0) {
+ dcrsm->InsertEntry(r, c, v);
+ }
+ }
+ }
+ }
+
+ void ExpectEmpty() {
+ EXPECT_EQ(dcrsm->num_rows(), num_rows);
+ EXPECT_EQ(dcrsm->num_cols(), num_cols);
+ EXPECT_EQ(dcrsm->num_nonzeros(), 0);
+
+ Matrix dense_from_dcrsm;
+ dcrsm->ToDenseMatrix(&dense_from_dcrsm);
+ EXPECT_EQ(dense_from_dcrsm.rows(), num_rows);
+ EXPECT_EQ(dense_from_dcrsm.cols(), num_cols);
+ EXPECT_TRUE((dense_from_dcrsm.array() == 0.0).all());
+ }
+
+ void ExpectEqualToDenseReference() {
+ Matrix dense_from_dcrsm;
+ dcrsm->ToDenseMatrix(&dense_from_dcrsm);
+ EXPECT_TRUE((dense.array() == dense_from_dcrsm.array()).all());
+ }
+
+ void ExpectEqualToCompressedRowSparseMatrixReference() {
+ typedef Eigen::Map<const Eigen::VectorXi> ConstIntVectorRef;
+
+ ConstIntVectorRef crsm_rows(crsm->rows(), crsm->num_rows() + 1);
+ ConstIntVectorRef dcrsm_rows(dcrsm->rows(), dcrsm->num_rows() + 1);
+ EXPECT_TRUE((crsm_rows.array() == dcrsm_rows.array()).all());
+
+ ConstIntVectorRef crsm_cols(crsm->cols(), crsm->num_nonzeros());
+ ConstIntVectorRef dcrsm_cols(dcrsm->cols(), dcrsm->num_nonzeros());
+ EXPECT_TRUE((crsm_cols.array() == dcrsm_cols.array()).all());
+
+ ConstVectorRef crsm_values(crsm->values(), crsm->num_nonzeros());
+ ConstVectorRef dcrsm_values(dcrsm->values(), dcrsm->num_nonzeros());
+ EXPECT_TRUE((crsm_values.array() == dcrsm_values.array()).all());
+ }
+
+ int num_rows;
+ int num_cols;
+
+ int num_additional_elements;
+
+ int expected_num_nonzeros;
+
+ Matrix dense;
+ scoped_ptr<TripletSparseMatrix> tsm;
+ scoped_ptr<CompressedRowSparseMatrix> crsm;
+
+ scoped_ptr<DynamicCompressedRowSparseMatrix> dcrsm;
+};
+
+TEST_F(DynamicCompressedRowSparseMatrixTest, Initialization) {
+ ExpectEmpty();
+
+ Finalize();
+ ExpectEmpty();
+}
+
+TEST_F(DynamicCompressedRowSparseMatrixTest, InsertEntryAndFinalize) {
+ InsertNonZeroEntriesFromDenseReference();
+ ExpectEmpty();
+
+ Finalize();
+ ExpectEqualToDenseReference();
+ ExpectEqualToCompressedRowSparseMatrixReference();
+}
+
+TEST_F(DynamicCompressedRowSparseMatrixTest, ClearRows) {
+ InsertNonZeroEntriesFromDenseReference();
+ Finalize();
+ ExpectEqualToDenseReference();
+ ExpectEqualToCompressedRowSparseMatrixReference();
+
+ dcrsm->ClearRows(0, 0);
+ Finalize();
+ ExpectEqualToDenseReference();
+ ExpectEqualToCompressedRowSparseMatrixReference();
+
+ dcrsm->ClearRows(0, num_rows);
+ ExpectEqualToCompressedRowSparseMatrixReference();
+
+ Finalize();
+ ExpectEmpty();
+
+ InsertNonZeroEntriesFromDenseReference();
+ dcrsm->ClearRows(1, 2);
+ Finalize();
+ dense.block(1, 0, 2, num_cols).setZero();
+ ExpectEqualToDenseReference();
+
+ InitialiseDenseReference();
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/dynamic_numeric_diff_cost_function_test.cc b/internal/ceres/dynamic_numeric_diff_cost_function_test.cc
new file mode 100644
index 0000000..19f4d88
--- /dev/null
+++ b/internal/ceres/dynamic_numeric_diff_cost_function_test.cc
@@ -0,0 +1,519 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+// mierle@gmail.com (Keir Mierle)
+
+#include <cstddef>
+
+#include "ceres/dynamic_numeric_diff_cost_function.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+const double kTolerance = 1e-6;
+
+// Takes 2 parameter blocks:
+// parameters[0] is size 10.
+// parameters[1] is size 5.
+// Emits 21 residuals:
+// A: i - parameters[0][i], for i in [0,10) -- this is 10 residuals
+// B: parameters[0][i] - i, for i in [0,10) -- this is another 10.
+// C: sum(parameters[0][i]^2 - 8*parameters[0][i]) + sum(parameters[1][i])
+class MyCostFunctor {
+ public:
+ bool operator()(double const* const* parameters, double* residuals) const {
+ const double* params0 = parameters[0];
+ int r = 0;
+ for (int i = 0; i < 10; ++i) {
+ residuals[r++] = i - params0[i];
+ residuals[r++] = params0[i] - i;
+ }
+
+ double c_residual = 0.0;
+ for (int i = 0; i < 10; ++i) {
+ c_residual += pow(params0[i], 2) - 8.0 * params0[i];
+ }
+
+ const double* params1 = parameters[1];
+ for (int i = 0; i < 5; ++i) {
+ c_residual += params1[i];
+ }
+ residuals[r++] = c_residual;
+ return true;
+ }
+};
+
+TEST(DynamicNumericdiffCostFunctionTest, TestResiduals) {
+ vector<double> param_block_0(10, 0.0);
+ vector<double> param_block_1(5, 0.0);
+ DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
+ new MyCostFunctor());
+ cost_function.AddParameterBlock(param_block_0.size());
+ cost_function.AddParameterBlock(param_block_1.size());
+ cost_function.SetNumResiduals(21);
+
+ // Test residual computation.
+ vector<double> residuals(21, -100000);
+ vector<double*> parameter_blocks(2);
+ parameter_blocks[0] = ¶m_block_0[0];
+ parameter_blocks[1] = ¶m_block_1[0];
+ EXPECT_TRUE(cost_function.Evaluate(¶meter_blocks[0],
+ residuals.data(),
+ NULL));
+ for (int r = 0; r < 10; ++r) {
+ EXPECT_EQ(1.0 * r, residuals.at(r * 2));
+ EXPECT_EQ(-1.0 * r, residuals.at(r * 2 + 1));
+ }
+ EXPECT_EQ(0, residuals.at(20));
+}
+
+
+TEST(DynamicNumericdiffCostFunctionTest, TestJacobian) {
+ // Test the residual counting.
+ vector<double> param_block_0(10, 0.0);
+ for (int i = 0; i < 10; ++i) {
+ param_block_0[i] = 2 * i;
+ }
+ vector<double> param_block_1(5, 0.0);
+ DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
+ new MyCostFunctor());
+ cost_function.AddParameterBlock(param_block_0.size());
+ cost_function.AddParameterBlock(param_block_1.size());
+ cost_function.SetNumResiduals(21);
+
+ // Prepare the residuals.
+ vector<double> residuals(21, -100000);
+
+ // Prepare the parameters.
+ vector<double*> parameter_blocks(2);
+ parameter_blocks[0] = ¶m_block_0[0];
+ parameter_blocks[1] = ¶m_block_1[0];
+
+ // Prepare the jacobian.
+ vector<vector<double> > jacobian_vect(2);
+ jacobian_vect[0].resize(21 * 10, -100000);
+ jacobian_vect[1].resize(21 * 5, -100000);
+ vector<double*> jacobian;
+ jacobian.push_back(jacobian_vect[0].data());
+ jacobian.push_back(jacobian_vect[1].data());
+
+ // Test jacobian computation.
+ EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int r = 0; r < 10; ++r) {
+ EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
+ EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
+ }
+ EXPECT_EQ(420, residuals.at(20));
+ for (int p = 0; p < 10; ++p) {
+ // Check "A" Jacobian.
+ EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance);
+ // Check "B" Jacobian.
+ EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance);
+ jacobian_vect[0][2*p * 10 + p] = 0.0;
+ jacobian_vect[0][(2*p+1) * 10 + p] = 0.0;
+ }
+
+ // Check "C" Jacobian for first parameter block.
+ for (int p = 0; p < 10; ++p) {
+ EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance);
+ jacobian_vect[0][20 * 10 + p] = 0.0;
+ }
+ for (int i = 0; i < jacobian_vect[0].size(); ++i) {
+ EXPECT_NEAR(0.0, jacobian_vect[0][i], kTolerance);
+ }
+
+ // Check "C" Jacobian for second parameter block.
+ for (int p = 0; p < 5; ++p) {
+ EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance);
+ jacobian_vect[1][20 * 5 + p] = 0.0;
+ }
+ for (int i = 0; i < jacobian_vect[1].size(); ++i) {
+ EXPECT_NEAR(0.0, jacobian_vect[1][i], kTolerance);
+ }
+}
+
+TEST(DynamicNumericdiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) { // NOLINT
+ // Test the residual counting.
+ vector<double> param_block_0(10, 0.0);
+ for (int i = 0; i < 10; ++i) {
+ param_block_0[i] = 2 * i;
+ }
+ vector<double> param_block_1(5, 0.0);
+ DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
+ new MyCostFunctor());
+ cost_function.AddParameterBlock(param_block_0.size());
+ cost_function.AddParameterBlock(param_block_1.size());
+ cost_function.SetNumResiduals(21);
+
+ // Prepare the residuals.
+ vector<double> residuals(21, -100000);
+
+ // Prepare the parameters.
+ vector<double*> parameter_blocks(2);
+ parameter_blocks[0] = ¶m_block_0[0];
+ parameter_blocks[1] = ¶m_block_1[0];
+
+ // Prepare the jacobian.
+ vector<vector<double> > jacobian_vect(2);
+ jacobian_vect[0].resize(21 * 10, -100000);
+ jacobian_vect[1].resize(21 * 5, -100000);
+ vector<double*> jacobian;
+ jacobian.push_back(NULL);
+ jacobian.push_back(jacobian_vect[1].data());
+
+ // Test jacobian computation.
+ EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int r = 0; r < 10; ++r) {
+ EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
+ EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
+ }
+ EXPECT_EQ(420, residuals.at(20));
+
+ // Check "C" Jacobian for second parameter block.
+ for (int p = 0; p < 5; ++p) {
+ EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance);
+ jacobian_vect[1][20 * 5 + p] = 0.0;
+ }
+ for (int i = 0; i < jacobian_vect[1].size(); ++i) {
+ EXPECT_EQ(0.0, jacobian_vect[1][i]);
+ }
+}
+
+TEST(DynamicNumericdiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) { // NOLINT
+ // Test the residual counting.
+ vector<double> param_block_0(10, 0.0);
+ for (int i = 0; i < 10; ++i) {
+ param_block_0[i] = 2 * i;
+ }
+ vector<double> param_block_1(5, 0.0);
+ DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
+ new MyCostFunctor());
+ cost_function.AddParameterBlock(param_block_0.size());
+ cost_function.AddParameterBlock(param_block_1.size());
+ cost_function.SetNumResiduals(21);
+
+ // Prepare the residuals.
+ vector<double> residuals(21, -100000);
+
+ // Prepare the parameters.
+ vector<double*> parameter_blocks(2);
+ parameter_blocks[0] = ¶m_block_0[0];
+ parameter_blocks[1] = ¶m_block_1[0];
+
+ // Prepare the jacobian.
+ vector<vector<double> > jacobian_vect(2);
+ jacobian_vect[0].resize(21 * 10, -100000);
+ jacobian_vect[1].resize(21 * 5, -100000);
+ vector<double*> jacobian;
+ jacobian.push_back(jacobian_vect[0].data());
+ jacobian.push_back(NULL);
+
+ // Test jacobian computation.
+ EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int r = 0; r < 10; ++r) {
+ EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
+ EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
+ }
+ EXPECT_EQ(420, residuals.at(20));
+ for (int p = 0; p < 10; ++p) {
+ // Check "A" Jacobian.
+ EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance);
+ // Check "B" Jacobian.
+ EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance);
+ jacobian_vect[0][2*p * 10 + p] = 0.0;
+ jacobian_vect[0][(2*p+1) * 10 + p] = 0.0;
+ }
+
+ // Check "C" Jacobian for first parameter block.
+ for (int p = 0; p < 10; ++p) {
+ EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance);
+ jacobian_vect[0][20 * 10 + p] = 0.0;
+ }
+ for (int i = 0; i < jacobian_vect[0].size(); ++i) {
+ EXPECT_EQ(0.0, jacobian_vect[0][i]);
+ }
+}
+
+// Takes 3 parameter blocks:
+// parameters[0] (x) is size 1.
+// parameters[1] (y) is size 2.
+// parameters[2] (z) is size 3.
+// Emits 7 residuals:
+// A: x[0] (= sum_x)
+// B: y[0] + 2.0 * y[1] (= sum_y)
+// C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z)
+// D: sum_x * sum_y
+// E: sum_y * sum_z
+// F: sum_x * sum_z
+// G: sum_x * sum_y * sum_z
+class MyThreeParameterCostFunctor {
+ public:
+ template <typename T>
+ bool operator()(T const* const* parameters, T* residuals) const {
+ const T* x = parameters[0];
+ const T* y = parameters[1];
+ const T* z = parameters[2];
+
+ T sum_x = x[0];
+ T sum_y = y[0] + 2.0 * y[1];
+ T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2];
+
+ residuals[0] = sum_x;
+ residuals[1] = sum_y;
+ residuals[2] = sum_z;
+ residuals[3] = sum_x * sum_y;
+ residuals[4] = sum_y * sum_z;
+ residuals[5] = sum_x * sum_z;
+ residuals[6] = sum_x * sum_y * sum_z;
+ return true;
+ }
+};
+
+class ThreeParameterCostFunctorTest : public ::testing::Test {
+ protected:
+ virtual void SetUp() {
+ // Prepare the parameters.
+ x_.resize(1);
+ x_[0] = 0.0;
+
+ y_.resize(2);
+ y_[0] = 1.0;
+ y_[1] = 3.0;
+
+ z_.resize(3);
+ z_[0] = 2.0;
+ z_[1] = 4.0;
+ z_[2] = 6.0;
+
+ parameter_blocks_.resize(3);
+ parameter_blocks_[0] = &x_[0];
+ parameter_blocks_[1] = &y_[0];
+ parameter_blocks_[2] = &z_[0];
+
+ // Prepare the cost function.
+ typedef DynamicNumericDiffCostFunction<MyThreeParameterCostFunctor>
+ DynamicMyThreeParameterCostFunction;
+ DynamicMyThreeParameterCostFunction * cost_function =
+ new DynamicMyThreeParameterCostFunction(
+ new MyThreeParameterCostFunctor());
+ cost_function->AddParameterBlock(1);
+ cost_function->AddParameterBlock(2);
+ cost_function->AddParameterBlock(3);
+ cost_function->SetNumResiduals(7);
+
+ cost_function_.reset(cost_function);
+
+ // Setup jacobian data.
+ jacobian_vect_.resize(3);
+ jacobian_vect_[0].resize(7 * x_.size(), -100000);
+ jacobian_vect_[1].resize(7 * y_.size(), -100000);
+ jacobian_vect_[2].resize(7 * z_.size(), -100000);
+
+ // Prepare the expected residuals.
+ const double sum_x = x_[0];
+ const double sum_y = y_[0] + 2.0 * y_[1];
+ const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2];
+
+ expected_residuals_.resize(7);
+ expected_residuals_[0] = sum_x;
+ expected_residuals_[1] = sum_y;
+ expected_residuals_[2] = sum_z;
+ expected_residuals_[3] = sum_x * sum_y;
+ expected_residuals_[4] = sum_y * sum_z;
+ expected_residuals_[5] = sum_x * sum_z;
+ expected_residuals_[6] = sum_x * sum_y * sum_z;
+
+ // Prepare the expected jacobian entries.
+ expected_jacobian_x_.resize(7);
+ expected_jacobian_x_[0] = 1.0;
+ expected_jacobian_x_[1] = 0.0;
+ expected_jacobian_x_[2] = 0.0;
+ expected_jacobian_x_[3] = sum_y;
+ expected_jacobian_x_[4] = 0.0;
+ expected_jacobian_x_[5] = sum_z;
+ expected_jacobian_x_[6] = sum_y * sum_z;
+
+ expected_jacobian_y_.resize(14);
+ expected_jacobian_y_[0] = 0.0;
+ expected_jacobian_y_[1] = 0.0;
+ expected_jacobian_y_[2] = 1.0;
+ expected_jacobian_y_[3] = 2.0;
+ expected_jacobian_y_[4] = 0.0;
+ expected_jacobian_y_[5] = 0.0;
+ expected_jacobian_y_[6] = sum_x;
+ expected_jacobian_y_[7] = 2.0 * sum_x;
+ expected_jacobian_y_[8] = sum_z;
+ expected_jacobian_y_[9] = 2.0 * sum_z;
+ expected_jacobian_y_[10] = 0.0;
+ expected_jacobian_y_[11] = 0.0;
+ expected_jacobian_y_[12] = sum_x * sum_z;
+ expected_jacobian_y_[13] = 2.0 * sum_x * sum_z;
+
+ expected_jacobian_z_.resize(21);
+ expected_jacobian_z_[0] = 0.0;
+ expected_jacobian_z_[1] = 0.0;
+ expected_jacobian_z_[2] = 0.0;
+ expected_jacobian_z_[3] = 0.0;
+ expected_jacobian_z_[4] = 0.0;
+ expected_jacobian_z_[5] = 0.0;
+ expected_jacobian_z_[6] = 1.0;
+ expected_jacobian_z_[7] = 3.0;
+ expected_jacobian_z_[8] = 6.0;
+ expected_jacobian_z_[9] = 0.0;
+ expected_jacobian_z_[10] = 0.0;
+ expected_jacobian_z_[11] = 0.0;
+ expected_jacobian_z_[12] = sum_y;
+ expected_jacobian_z_[13] = 3.0 * sum_y;
+ expected_jacobian_z_[14] = 6.0 * sum_y;
+ expected_jacobian_z_[15] = sum_x;
+ expected_jacobian_z_[16] = 3.0 * sum_x;
+ expected_jacobian_z_[17] = 6.0 * sum_x;
+ expected_jacobian_z_[18] = sum_x * sum_y;
+ expected_jacobian_z_[19] = 3.0 * sum_x * sum_y;
+ expected_jacobian_z_[20] = 6.0 * sum_x * sum_y;
+ }
+
+ protected:
+ vector<double> x_;
+ vector<double> y_;
+ vector<double> z_;
+
+ vector<double*> parameter_blocks_;
+
+ scoped_ptr<CostFunction> cost_function_;
+
+ vector<vector<double> > jacobian_vect_;
+
+ vector<double> expected_residuals_;
+
+ vector<double> expected_jacobian_x_;
+ vector<double> expected_jacobian_y_;
+ vector<double> expected_jacobian_z_;
+};
+
+TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) {
+ vector<double> residuals(7, -100000);
+ EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
+ residuals.data(),
+ NULL));
+ for (int i = 0; i < 7; ++i) {
+ EXPECT_EQ(expected_residuals_[i], residuals[i]);
+ }
+}
+
+TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) {
+ vector<double> residuals(7, -100000);
+
+ vector<double*> jacobian;
+ jacobian.push_back(jacobian_vect_[0].data());
+ jacobian.push_back(jacobian_vect_[1].data());
+ jacobian.push_back(jacobian_vect_[2].data());
+
+ EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int i = 0; i < 7; ++i) {
+ EXPECT_EQ(expected_residuals_[i], residuals[i]);
+ }
+
+ for (int i = 0; i < 7; ++i) {
+ EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance);
+ }
+
+ for (int i = 0; i < 14; ++i) {
+ EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance);
+ }
+
+ for (int i = 0; i < 21; ++i) {
+ EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance);
+ }
+}
+
+TEST_F(ThreeParameterCostFunctorTest,
+ ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) {
+ vector<double> residuals(7, -100000);
+
+ vector<double*> jacobian;
+ jacobian.push_back(NULL);
+ jacobian.push_back(jacobian_vect_[1].data());
+ jacobian.push_back(NULL);
+
+ EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int i = 0; i < 7; ++i) {
+ EXPECT_EQ(expected_residuals_[i], residuals[i]);
+ }
+
+ for (int i = 0; i < 14; ++i) {
+ EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance);
+ }
+}
+
+TEST_F(ThreeParameterCostFunctorTest,
+ ThreeParameterJacobianWithSecondParameterBlockConstant) {
+ vector<double> residuals(7, -100000);
+
+ vector<double*> jacobian;
+ jacobian.push_back(jacobian_vect_[0].data());
+ jacobian.push_back(NULL);
+ jacobian.push_back(jacobian_vect_[2].data());
+
+ EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int i = 0; i < 7; ++i) {
+ EXPECT_EQ(expected_residuals_[i], residuals[i]);
+ }
+
+ for (int i = 0; i < 7; ++i) {
+ EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance);
+ }
+
+ for (int i = 0; i < 21; ++i) {
+ EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance);
+ }
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/evaluator.cc b/internal/ceres/evaluator.cc
index 31a4176..c94c62c 100644
--- a/internal/ceres/evaluator.cc
+++ b/internal/ceres/evaluator.cc
@@ -35,6 +35,8 @@
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/crs_matrix.h"
#include "ceres/dense_jacobian_writer.h"
+#include "ceres/dynamic_compressed_row_finalizer.h"
+#include "ceres/dynamic_compressed_row_jacobian_writer.h"
#include "ceres/evaluator.h"
#include "ceres/internal/port.h"
#include "ceres/program_evaluator.h"
@@ -63,9 +65,17 @@
BlockJacobianWriter>(options,
program);
case SPARSE_NORMAL_CHOLESKY:
- return new ProgramEvaluator<ScratchEvaluatePreparer,
- CompressedRowJacobianWriter>(options,
- program);
+ if (options.dynamic_sparsity) {
+ return new ProgramEvaluator<ScratchEvaluatePreparer,
+ DynamicCompressedRowJacobianWriter,
+ DynamicCompressedRowJacobianFinalizer>(
+ options, program);
+ } else {
+ return new ProgramEvaluator<ScratchEvaluatePreparer,
+ CompressedRowJacobianWriter>(options,
+ program);
+ }
+
default:
*error = "Invalid Linear Solver Type. Unable to create evaluator.";
return NULL;
diff --git a/internal/ceres/evaluator.h b/internal/ceres/evaluator.h
index 3d25462..8fc60b8 100644
--- a/internal/ceres/evaluator.h
+++ b/internal/ceres/evaluator.h
@@ -61,11 +61,13 @@
Options()
: num_threads(1),
num_eliminate_blocks(-1),
- linear_solver_type(DENSE_QR) {}
+ linear_solver_type(DENSE_QR),
+ dynamic_sparsity(false) {}
int num_threads;
int num_eliminate_blocks;
LinearSolverType linear_solver_type;
+ bool dynamic_sparsity;
};
static Evaluator* Create(const Options& options,
diff --git a/internal/ceres/evaluator_test.cc b/internal/ceres/evaluator_test.cc
index ea24504..c0de3fc 100644
--- a/internal/ceres/evaluator_test.cc
+++ b/internal/ceres/evaluator_test.cc
@@ -44,6 +44,7 @@
#include "ceres/program.h"
#include "ceres/sized_cost_function.h"
#include "ceres/sparse_matrix.h"
+#include "ceres/stringprintf.h"
#include "ceres/types.h"
#include "gtest/gtest.h"
@@ -91,18 +92,42 @@
}
};
+struct EvaluatorTestOptions {
+ EvaluatorTestOptions(LinearSolverType linear_solver_type,
+ int num_eliminate_blocks,
+ bool dynamic_sparsity = false)
+ : linear_solver_type(linear_solver_type),
+ num_eliminate_blocks(num_eliminate_blocks),
+ dynamic_sparsity(dynamic_sparsity) {}
+
+ LinearSolverType linear_solver_type;
+ int num_eliminate_blocks;
+ bool dynamic_sparsity;
+};
+
struct EvaluatorTest
- : public ::testing::TestWithParam<pair<LinearSolverType, int> > {
+ : public ::testing::TestWithParam<EvaluatorTestOptions> {
Evaluator* CreateEvaluator(Program* program) {
// This program is straight from the ProblemImpl, and so has no index/offset
// yet; compute it here as required by the evalutor implementations.
program->SetParameterOffsetsAndIndex();
- VLOG(1) << "Creating evaluator with type: " << GetParam().first
- << " and num_eliminate_blocks: " << GetParam().second;
+ if (VLOG_IS_ON(1)) {
+ string report;
+ StringAppendF(&report, "Creating evaluator with type: %d",
+ GetParam().linear_solver_type);
+ if (GetParam().linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
+ StringAppendF(&report, ", dynamic_sparsity: %d",
+ GetParam().dynamic_sparsity);
+ }
+ StringAppendF(&report, " and num_eliminate_blocks: %d",
+ GetParam().num_eliminate_blocks);
+ VLOG(1) << report;
+ }
Evaluator::Options options;
- options.linear_solver_type = GetParam().first;
- options.num_eliminate_blocks = GetParam().second;
+ options.linear_solver_type = GetParam().linear_solver_type;
+ options.num_eliminate_blocks = GetParam().num_eliminate_blocks;
+ options.dynamic_sparsity = GetParam().dynamic_sparsity;
string error;
return Evaluator::Create(options, program, &error);
}
@@ -517,23 +542,25 @@
INSTANTIATE_TEST_CASE_P(
LinearSolvers,
EvaluatorTest,
- ::testing::Values(make_pair(DENSE_QR, 0),
- make_pair(DENSE_SCHUR, 0),
- make_pair(DENSE_SCHUR, 1),
- make_pair(DENSE_SCHUR, 2),
- make_pair(DENSE_SCHUR, 3),
- make_pair(DENSE_SCHUR, 4),
- make_pair(SPARSE_SCHUR, 0),
- make_pair(SPARSE_SCHUR, 1),
- make_pair(SPARSE_SCHUR, 2),
- make_pair(SPARSE_SCHUR, 3),
- make_pair(SPARSE_SCHUR, 4),
- make_pair(ITERATIVE_SCHUR, 0),
- make_pair(ITERATIVE_SCHUR, 1),
- make_pair(ITERATIVE_SCHUR, 2),
- make_pair(ITERATIVE_SCHUR, 3),
- make_pair(ITERATIVE_SCHUR, 4),
- make_pair(SPARSE_NORMAL_CHOLESKY, 0)));
+ ::testing::Values(
+ EvaluatorTestOptions(DENSE_QR, 0),
+ EvaluatorTestOptions(DENSE_SCHUR, 0),
+ EvaluatorTestOptions(DENSE_SCHUR, 1),
+ EvaluatorTestOptions(DENSE_SCHUR, 2),
+ EvaluatorTestOptions(DENSE_SCHUR, 3),
+ EvaluatorTestOptions(DENSE_SCHUR, 4),
+ EvaluatorTestOptions(SPARSE_SCHUR, 0),
+ EvaluatorTestOptions(SPARSE_SCHUR, 1),
+ EvaluatorTestOptions(SPARSE_SCHUR, 2),
+ EvaluatorTestOptions(SPARSE_SCHUR, 3),
+ EvaluatorTestOptions(SPARSE_SCHUR, 4),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 0),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 1),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 2),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 3),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 4),
+ EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, false),
+ EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, true)));
// Simple cost function used to check if the evaluator is sensitive to
// state changes.
diff --git a/internal/ceres/generate_eliminator_specialization.py b/internal/ceres/generate_eliminator_specialization.py
index caeca69..2ec3c5b 100644
--- a/internal/ceres/generate_eliminator_specialization.py
+++ b/internal/ceres/generate_eliminator_specialization.py
@@ -59,7 +59,10 @@
(2, 3, "Eigen::Dynamic"),
(2, 4, 3),
(2, 4, 4),
+ (2, 4, 8),
+ (2, 4, 9),
(2, 4, "Eigen::Dynamic"),
+ (2, "Eigen::Dynamic", "Eigen::Dynamic"),
(4, 4, 2),
(4, 4, 3),
(4, 4, 4),
@@ -123,6 +126,9 @@
"""
SPECIALIZATION_FILE = """
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generate_partitioned_matrix_view_specializations.py b/internal/ceres/generate_partitioned_matrix_view_specializations.py
new file mode 100644
index 0000000..c9bdf23
--- /dev/null
+++ b/internal/ceres/generate_partitioned_matrix_view_specializations.py
@@ -0,0 +1,231 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2013 Google Inc. All rights reserved.
+# http://code.google.com/p/ceres-solver/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: sameeragarwal@google.com (Sameer Agarwal)
+#
+# Script for explicitly generating template specialization of the
+# PartitionedMatrixView class. Explicitly generating these
+# instantiations in separate .cc files breaks the compilation into
+# separate compilation unit rather than one large cc file.
+#
+# This script creates two sets of files.
+#
+# 1. partitioned_matrix_view_x_x_x.cc
+# where the x indicates the template parameters and
+#
+# 2. partitioned_matrix_view.cc
+#
+# that contains a factory function for instantiating these classes
+# based on runtime parameters.
+#
+# The list of tuples, specializations indicates the set of
+# specializations that is generated.
+
+# Set of template specializations to generate
+SPECIALIZATIONS = [(2, 2, 2),
+ (2, 2, 3),
+ (2, 2, 4),
+ (2, 2, "Eigen::Dynamic"),
+ (2, 3, 3),
+ (2, 3, 4),
+ (2, 3, 9),
+ (2, 3, "Eigen::Dynamic"),
+ (2, 4, 3),
+ (2, 4, 4),
+ (2, 4, 8),
+ (2, 4, 9),
+ (2, 4, "Eigen::Dynamic"),
+ (2, "Eigen::Dynamic", "Eigen::Dynamic"),
+ (4, 4, 2),
+ (4, 4, 3),
+ (4, 4, 4),
+ (4, 4, "Eigen::Dynamic"),
+ ("Eigen::Dynamic", "Eigen::Dynamic", "Eigen::Dynamic")]
+HEADER = """// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+"""
+
+DYNAMIC_FILE = """
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<%s, %s, %s>;
+
+} // namespace internal
+} // namespace ceres
+"""
+
+SPECIALIZATION_FILE = """
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<%s, %s, %s>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
+"""
+
+FACTORY_FILE_HEADER = """
+#include "ceres/linear_solver.h"
+#include "ceres/partitioned_matrix_view.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+PartitionedMatrixViewBase*
+PartitionedMatrixViewBase::Create(const LinearSolver::Options& options,
+ const BlockSparseMatrix& matrix) {
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+"""
+
+FACTORY_CONDITIONAL = """ if ((options.row_block_size == %s) &&
+ (options.e_block_size == %s) &&
+ (options.f_block_size == %s)) {
+ return new PartitionedMatrixView<%s, %s, %s>(
+ matrix, options.elimination_groups[0]);
+ }
+"""
+
+FACTORY_FOOTER = """
+#endif
+ VLOG(1) << "Template specializations not found for <"
+ << options.row_block_size << ","
+ << options.e_block_size << ","
+ << options.f_block_size << ">";
+ return new PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
+};
+
+} // namespace internal
+} // namespace ceres
+"""
+
+
+def SuffixForSize(size):
+ if size == "Eigen::Dynamic":
+ return "d"
+ return str(size)
+
+
+def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size):
+ return "_".join([prefix] + map(SuffixForSize, (row_block_size,
+ e_block_size,
+ f_block_size)))
+
+
+def Specialize():
+ """
+ Generate specialization code and the conditionals to instantiate it.
+ """
+ f = open("partitioned_matrix_view.cc", "w")
+ f.write(HEADER)
+ f.write(FACTORY_FILE_HEADER)
+
+ for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS:
+ output = SpecializationFilename("generated/partitioned_matrix_view",
+ row_block_size,
+ e_block_size,
+ f_block_size) + ".cc"
+ fptr = open(output, "w")
+ fptr.write(HEADER)
+
+ template = SPECIALIZATION_FILE
+ if (row_block_size == "Eigen::Dynamic" and
+ e_block_size == "Eigen::Dynamic" and
+ f_block_size == "Eigen::Dynamic"):
+ template = DYNAMIC_FILE
+
+ fptr.write(template % (row_block_size, e_block_size, f_block_size))
+ fptr.close()
+
+ f.write(FACTORY_CONDITIONAL % (row_block_size,
+ e_block_size,
+ f_block_size,
+ row_block_size,
+ e_block_size,
+ f_block_size))
+ f.write(FACTORY_FOOTER)
+ f.close()
+
+
+if __name__ == "__main__":
+ Specialize()
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc b/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc
new file mode 100644
index 0000000..a7d802a
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 2, 2>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc b/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc
new file mode 100644
index 0000000..89e6f77
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 2, 3>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc b/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc
new file mode 100644
index 0000000..3a3e8b6
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 2, 4>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc b/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc
new file mode 100644
index 0000000..661f135
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 2, Eigen::Dynamic>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc b/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc
new file mode 100644
index 0000000..e79e001
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 3, 3>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc b/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc
new file mode 100644
index 0000000..2f1ae68
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 3, 4>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc b/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc
new file mode 100644
index 0000000..ab40550
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 3, 9>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc b/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc
new file mode 100644
index 0000000..89ecff7
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 3, Eigen::Dynamic>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc b/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc
new file mode 100644
index 0000000..182707d
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 4, 3>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc b/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc
new file mode 100644
index 0000000..a2cf8f4
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 4, 4>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc b/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc
new file mode 100644
index 0000000..a263769
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 4, 8>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc b/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc
new file mode 100644
index 0000000..d853860
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 4, 9>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc b/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc
new file mode 100644
index 0000000..7d622fc
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, 4, Eigen::Dynamic>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc b/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc
new file mode 100644
index 0000000..31981ca
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<2, Eigen::Dynamic, Eigen::Dynamic>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc b/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc
new file mode 100644
index 0000000..d51ab5f
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<4, 4, 2>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc b/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc
new file mode 100644
index 0000000..4b17fbd
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<4, 4, 3>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc b/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc
new file mode 100644
index 0000000..7b5fe0f
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<4, 4, 4>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc b/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc
new file mode 100644
index 0000000..c31fed3
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<4, 4, Eigen::Dynamic>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc b/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc
new file mode 100644
index 0000000..a3308ed
--- /dev/null
+++ b/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc
@@ -0,0 +1,53 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
+
+
+#include "ceres/partitioned_matrix_view_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>;
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/generated/schur_eliminator_2_2_2.cc b/internal/ceres/generated/schur_eliminator_2_2_2.cc
index 7f9ce14..db2a4dc 100644
--- a/internal/ceres/generated/schur_eliminator_2_2_2.cc
+++ b/internal/ceres/generated/schur_eliminator_2_2_2.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_2_3.cc b/internal/ceres/generated/schur_eliminator_2_2_3.cc
index d9ab1dd..f53c12a 100644
--- a/internal/ceres/generated/schur_eliminator_2_2_3.cc
+++ b/internal/ceres/generated/schur_eliminator_2_2_3.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_2_4.cc b/internal/ceres/generated/schur_eliminator_2_2_4.cc
index a268810..9e29383 100644
--- a/internal/ceres/generated/schur_eliminator_2_2_4.cc
+++ b/internal/ceres/generated/schur_eliminator_2_2_4.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_2_d.cc b/internal/ceres/generated/schur_eliminator_2_2_d.cc
index 46f9492..541def6 100644
--- a/internal/ceres/generated/schur_eliminator_2_2_d.cc
+++ b/internal/ceres/generated/schur_eliminator_2_2_d.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_3_3.cc b/internal/ceres/generated/schur_eliminator_2_3_3.cc
index ce53c6c..e450263 100644
--- a/internal/ceres/generated/schur_eliminator_2_3_3.cc
+++ b/internal/ceres/generated/schur_eliminator_2_3_3.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_3_4.cc b/internal/ceres/generated/schur_eliminator_2_3_4.cc
index 7f6d41d..0618c68 100644
--- a/internal/ceres/generated/schur_eliminator_2_3_4.cc
+++ b/internal/ceres/generated/schur_eliminator_2_3_4.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_3_9.cc b/internal/ceres/generated/schur_eliminator_2_3_9.cc
index 10f84af..c1ca665 100644
--- a/internal/ceres/generated/schur_eliminator_2_3_9.cc
+++ b/internal/ceres/generated/schur_eliminator_2_3_9.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_3_d.cc b/internal/ceres/generated/schur_eliminator_2_3_d.cc
index 047d473..1b6092c 100644
--- a/internal/ceres/generated/schur_eliminator_2_3_d.cc
+++ b/internal/ceres/generated/schur_eliminator_2_3_d.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_4_3.cc b/internal/ceres/generated/schur_eliminator_2_4_3.cc
index 12fdb86..edce8ef 100644
--- a/internal/ceres/generated/schur_eliminator_2_4_3.cc
+++ b/internal/ceres/generated/schur_eliminator_2_4_3.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_4_4.cc b/internal/ceres/generated/schur_eliminator_2_4_4.cc
index 0e29dc1..a6f3c52 100644
--- a/internal/ceres/generated/schur_eliminator_2_4_4.cc
+++ b/internal/ceres/generated/schur_eliminator_2_4_4.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_4_8.cc b/internal/ceres/generated/schur_eliminator_2_4_8.cc
new file mode 100644
index 0000000..bf2f0ab
--- /dev/null
+++ b/internal/ceres/generated/schur_eliminator_2_4_8.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012, 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of SchurEliminator.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_eliminator_specialization.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/schur_eliminator_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class SchurEliminator<2, 4, 8>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/schur_eliminator_2_4_9.cc b/internal/ceres/generated/schur_eliminator_2_4_9.cc
new file mode 100644
index 0000000..a63d0bb
--- /dev/null
+++ b/internal/ceres/generated/schur_eliminator_2_4_9.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012, 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of SchurEliminator.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_eliminator_specialization.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/schur_eliminator_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class SchurEliminator<2, 4, 9>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/schur_eliminator_2_4_d.cc b/internal/ceres/generated/schur_eliminator_2_4_d.cc
index 4d4ac56..b3a7fff 100644
--- a/internal/ceres/generated/schur_eliminator_2_4_d.cc
+++ b/internal/ceres/generated/schur_eliminator_2_4_d.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_2_d_d.cc b/internal/ceres/generated/schur_eliminator_2_d_d.cc
new file mode 100644
index 0000000..f4d28cd
--- /dev/null
+++ b/internal/ceres/generated/schur_eliminator_2_d_d.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012, 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of SchurEliminator.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_eliminator_specialization.py.
+// Editing it manually is not recommended.
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+#include "ceres/schur_eliminator_impl.h"
+#include "ceres/internal/eigen.h"
+
+namespace ceres {
+namespace internal {
+
+template class SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION
diff --git a/internal/ceres/generated/schur_eliminator_4_4_2.cc b/internal/ceres/generated/schur_eliminator_4_4_2.cc
index 4ad7d41..d1eadc1 100644
--- a/internal/ceres/generated/schur_eliminator_4_4_2.cc
+++ b/internal/ceres/generated/schur_eliminator_4_4_2.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_4_4_3.cc b/internal/ceres/generated/schur_eliminator_4_4_3.cc
index 87f2fc5..c340dbf 100644
--- a/internal/ceres/generated/schur_eliminator_4_4_3.cc
+++ b/internal/ceres/generated/schur_eliminator_4_4_3.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_4_4_4.cc b/internal/ceres/generated/schur_eliminator_4_4_4.cc
index 8b3f570..b7d58ad 100644
--- a/internal/ceres/generated/schur_eliminator_4_4_4.cc
+++ b/internal/ceres/generated/schur_eliminator_4_4_4.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_4_4_d.cc b/internal/ceres/generated/schur_eliminator_4_4_d.cc
index b21feb2..47e0059 100644
--- a/internal/ceres/generated/schur_eliminator_4_4_d.cc
+++ b/internal/ceres/generated/schur_eliminator_4_4_d.cc
@@ -37,9 +37,12 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
#include "ceres/schur_eliminator_impl.h"
diff --git a/internal/ceres/generated/schur_eliminator_d_d_d.cc b/internal/ceres/generated/schur_eliminator_d_d_d.cc
index d483db7..d54a03c 100644
--- a/internal/ceres/generated/schur_eliminator_d_d_d.cc
+++ b/internal/ceres/generated/schur_eliminator_d_d_d.cc
@@ -37,7 +37,7 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
diff --git a/internal/ceres/gradient_checking_cost_function.cc b/internal/ceres/gradient_checking_cost_function.cc
index 3edf95d..bca22e6 100644
--- a/internal/ceres/gradient_checking_cost_function.cc
+++ b/internal/ceres/gradient_checking_cost_function.cc
@@ -44,7 +44,7 @@
#include "ceres/problem_impl.h"
#include "ceres/program.h"
#include "ceres/residual_block.h"
-#include "ceres/runtime_numeric_diff_cost_function.h"
+#include "ceres/dynamic_numeric_diff_cost_function.h"
#include "ceres/stringprintf.h"
#include "ceres/types.h"
#include "glog/logging.h"
@@ -84,14 +84,24 @@
double relative_precision,
const string& extra_info)
: function_(function),
- finite_diff_cost_function_(
- CreateRuntimeNumericDiffCostFunction(function,
- CENTRAL,
- relative_step_size)),
relative_precision_(relative_precision),
extra_info_(extra_info) {
- *mutable_parameter_block_sizes() = function->parameter_block_sizes();
+ DynamicNumericDiffCostFunction<CostFunction, CENTRAL>*
+ finite_diff_cost_function =
+ new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>(
+ function,
+ DO_NOT_TAKE_OWNERSHIP,
+ relative_step_size);
+
+ const vector<int32>& parameter_block_sizes =
+ function->parameter_block_sizes();
+ for (int i = 0; i < parameter_block_sizes.size(); ++i) {
+ finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]);
+ }
+ *mutable_parameter_block_sizes() = parameter_block_sizes;
set_num_residuals(function->num_residuals());
+ finite_diff_cost_function->SetNumResiduals(num_residuals());
+ finite_diff_cost_function_.reset(finite_diff_cost_function);
}
virtual ~GradientCheckingCostFunction() { }
@@ -107,7 +117,7 @@
int num_residuals = function_->num_residuals();
// Make space for the jacobians of the two methods.
- const vector<int16>& block_sizes = function_->parameter_block_sizes();
+ const vector<int32>& block_sizes = function_->parameter_block_sizes();
vector<Matrix> term_jacobians(block_sizes.size());
vector<Matrix> finite_difference_jacobians(block_sizes.size());
vector<double*> term_jacobian_pointers(block_sizes.size());
diff --git a/internal/ceres/gradient_checking_cost_function_test.cc b/internal/ceres/gradient_checking_cost_function_test.cc
index ac06503..caba2f6 100644
--- a/internal/ceres/gradient_checking_cost_function_test.cc
+++ b/internal/ceres/gradient_checking_cost_function_test.cc
@@ -264,7 +264,7 @@
// Trivial cost function that accepts a single argument.
class UnaryCostFunction : public CostFunction {
public:
- UnaryCostFunction(int num_residuals, int16 parameter_block_size) {
+ UnaryCostFunction(int num_residuals, int32 parameter_block_size) {
set_num_residuals(num_residuals);
mutable_parameter_block_sizes()->push_back(parameter_block_size);
}
@@ -284,8 +284,8 @@
class BinaryCostFunction: public CostFunction {
public:
BinaryCostFunction(int num_residuals,
- int16 parameter_block1_size,
- int16 parameter_block2_size) {
+ int32 parameter_block1_size,
+ int32 parameter_block2_size) {
set_num_residuals(num_residuals);
mutable_parameter_block_sizes()->push_back(parameter_block1_size);
mutable_parameter_block_sizes()->push_back(parameter_block2_size);
@@ -305,9 +305,9 @@
class TernaryCostFunction: public CostFunction {
public:
TernaryCostFunction(int num_residuals,
- int16 parameter_block1_size,
- int16 parameter_block2_size,
- int16 parameter_block3_size) {
+ int32 parameter_block1_size,
+ int32 parameter_block2_size,
+ int32 parameter_block3_size) {
set_num_residuals(num_residuals);
mutable_parameter_block_sizes()->push_back(parameter_block1_size);
mutable_parameter_block_sizes()->push_back(parameter_block2_size);
diff --git a/internal/ceres/implicit_schur_complement.cc b/internal/ceres/implicit_schur_complement.cc
index 32722bb..2da6235 100644
--- a/internal/ceres/implicit_schur_complement.cc
+++ b/internal/ceres/implicit_schur_complement.cc
@@ -35,21 +35,18 @@
#include "ceres/block_structure.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
+#include "ceres/linear_solver.h"
#include "ceres/types.h"
#include "glog/logging.h"
namespace ceres {
namespace internal {
-ImplicitSchurComplement::ImplicitSchurComplement(int num_eliminate_blocks,
- bool preconditioner)
- : num_eliminate_blocks_(num_eliminate_blocks),
- preconditioner_(preconditioner),
- A_(NULL),
+ImplicitSchurComplement::ImplicitSchurComplement(
+ const LinearSolver::Options& options)
+ : options_(options),
D_(NULL),
- b_(NULL),
- block_diagonal_EtE_inverse_(NULL),
- block_diagonal_FtF_inverse_(NULL) {
+ b_(NULL) {
}
ImplicitSchurComplement::~ImplicitSchurComplement() {
@@ -61,7 +58,7 @@
// Since initialization is reasonably heavy, perhaps we can save on
// constructing a new object everytime.
if (A_ == NULL) {
- A_.reset(new PartitionedMatrixView(A, num_eliminate_blocks_));
+ A_.reset(PartitionedMatrixViewBase::Create(options_, A));
}
D_ = D;
@@ -71,7 +68,7 @@
// E'E and F'E.
if (block_diagonal_EtE_inverse_ == NULL) {
block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE());
- if (preconditioner_) {
+ if (options_.preconditioner_type == JACOBI) {
block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF());
}
rhs_.resize(A_->num_cols_f());
@@ -82,7 +79,7 @@
tmp_f_cols_.resize(A_->num_cols_f());
} else {
A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get());
- if (preconditioner_) {
+ if (options_.preconditioner_type == JACOBI) {
A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get());
}
}
@@ -91,7 +88,7 @@
// contributions from the diagonal D if it is non-null. Add that to
// the block diagonals and invert them.
AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get());
- if (preconditioner_) {
+ if (options_.preconditioner_type == JACOBI) {
AddDiagonalAndInvert((D_ == NULL) ? NULL : D_ + A_->num_cols_e(),
block_diagonal_FtF_inverse_.get());
}
diff --git a/internal/ceres/implicit_schur_complement.h b/internal/ceres/implicit_schur_complement.h
index c1bb6e1..c992bdc 100644
--- a/internal/ceres/implicit_schur_complement.h
+++ b/internal/ceres/implicit_schur_complement.h
@@ -35,6 +35,7 @@
#define CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_
#include "ceres/linear_operator.h"
+#include "ceres/linear_solver.h"
#include "ceres/partitioned_matrix_view.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
@@ -96,7 +97,7 @@
//
// TODO(sameeragarwal): Get rid of the two bools below and replace
// them with enums.
- ImplicitSchurComplement(int num_eliminate_blocks, bool preconditioner);
+ ImplicitSchurComplement(const LinearSolver::Options& options);
virtual ~ImplicitSchurComplement();
// Initialize the Schur complement for a linear least squares
@@ -142,10 +143,9 @@
void AddDiagonalAndInvert(const double* D, BlockSparseMatrix* matrix);
void UpdateRhs();
- int num_eliminate_blocks_;
- bool preconditioner_;
+ const LinearSolver::Options& options_;
- scoped_ptr<PartitionedMatrixView> A_;
+ scoped_ptr<PartitionedMatrixViewBase> A_;
const double* D_;
const double* b_;
diff --git a/internal/ceres/implicit_schur_complement_test.cc b/internal/ceres/implicit_schur_complement_test.cc
index 1694273..3369ecb 100644
--- a/internal/ceres/implicit_schur_complement_test.cc
+++ b/internal/ceres/implicit_schur_complement_test.cc
@@ -120,7 +120,10 @@
Vector reference_solution;
ReducedLinearSystemAndSolution(D, &lhs, &rhs, &reference_solution);
- ImplicitSchurComplement isc(num_eliminate_blocks_, true);
+ LinearSolver::Options options;
+ options.elimination_groups.push_back(num_eliminate_blocks_);
+ options.preconditioner_type = JACOBI;
+ ImplicitSchurComplement isc(options);
isc.Init(*A_, D, b_.get());
int num_sc_cols = lhs.cols();
diff --git a/internal/ceres/integral_types.h b/internal/ceres/integral_types.h
index 01e0493..d4913f5 100644
--- a/internal/ceres/integral_types.h
+++ b/internal/ceres/integral_types.h
@@ -77,7 +77,6 @@
#undef CERES_INTSIZE
typedef Integer< 8>::type int8;
-typedef Integer<16>::type int16;
typedef Integer<32>::type int32;
typedef Integer<64>::type int64;
diff --git a/internal/ceres/iterative_schur_complement_solver.cc b/internal/ceres/iterative_schur_complement_solver.cc
index 1aac565..6de410b 100644
--- a/internal/ceres/iterative_schur_complement_solver.cc
+++ b/internal/ceres/iterative_schur_complement_solver.cc
@@ -38,6 +38,7 @@
#include "ceres/block_sparse_matrix.h"
#include "ceres/block_structure.h"
#include "ceres/conjugate_gradients_solver.h"
+#include "ceres/detect_structure.h"
#include "ceres/implicit_schur_complement.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
@@ -69,35 +70,36 @@
EventLogger event_logger("IterativeSchurComplementSolver::Solve");
CHECK_NOTNULL(A->block_structure());
-
+ const int num_eliminate_blocks = options_.elimination_groups[0];
// Initialize a ImplicitSchurComplement object.
if (schur_complement_ == NULL) {
- schur_complement_.reset(
- new ImplicitSchurComplement(options_.elimination_groups[0],
- options_.preconditioner_type == JACOBI));
+ DetectStructure(*(A->block_structure()),
+ num_eliminate_blocks,
+ &options_.row_block_size,
+ &options_.e_block_size,
+ &options_.f_block_size);
+ schur_complement_.reset(new ImplicitSchurComplement(options_));
}
schur_complement_->Init(*A, per_solve_options.D, b);
const int num_schur_complement_blocks =
- A->block_structure()->cols.size() - options_.elimination_groups[0];
+ A->block_structure()->cols.size() - num_eliminate_blocks;
if (num_schur_complement_blocks == 0) {
VLOG(2) << "No parameter blocks left in the schur complement.";
LinearSolver::Summary cg_summary;
cg_summary.num_iterations = 0;
- cg_summary.termination_type = TOLERANCE;
+ cg_summary.termination_type = LINEAR_SOLVER_SUCCESS;
schur_complement_->BackSubstitute(NULL, x);
return cg_summary;
}
// Initialize the solution to the Schur complement system to zero.
- //
- // TODO(sameeragarwal): There maybe a better initialization than an
- // all zeros solution. Explore other cheap starting points.
reduced_linear_system_solution_.resize(schur_complement_->num_rows());
reduced_linear_system_solution_.setZero();
- // Instantiate a conjugate gradient solver that runs on the Schur complement
- // matrix with the block diagonal of the matrix F'F as the preconditioner.
+ // Instantiate a conjugate gradient solver that runs on the Schur
+ // complement matrix with the block diagonal of the matrix F'F as
+ // the preconditioner.
LinearSolver::Options cg_options;
cg_options.max_num_iterations = options_.max_num_iterations;
ConjugateGradientsSolver cg_solver(cg_options);
@@ -108,6 +110,8 @@
Preconditioner::Options preconditioner_options;
preconditioner_options.type = options_.preconditioner_type;
+ preconditioner_options.visibility_clustering_type =
+ options_.visibility_clustering_type;
preconditioner_options.sparse_linear_algebra_library_type =
options_.sparse_linear_algebra_library_type;
preconditioner_options.num_threads = options_.num_threads;
@@ -149,26 +153,26 @@
preconditioner_->Update(*A, per_solve_options.D);
cg_per_solve_options.preconditioner = preconditioner_.get();
}
-
event_logger.AddEvent("Setup");
LinearSolver::Summary cg_summary;
cg_summary.num_iterations = 0;
- cg_summary.termination_type = FAILURE;
+ cg_summary.termination_type = LINEAR_SOLVER_FAILURE;
+ // TODO(sameeragarwal): Refactor preconditioners to return a more
+ // sane message.
+ cg_summary.message = "Preconditioner update failed.";
if (preconditioner_update_was_successful) {
cg_summary = cg_solver.Solve(schur_complement_.get(),
schur_complement_->rhs().data(),
cg_per_solve_options,
reduced_linear_system_solution_.data());
- if (cg_summary.termination_type != FAILURE) {
+ if (cg_summary.termination_type != LINEAR_SOLVER_FAILURE &&
+ cg_summary.termination_type != LINEAR_SOLVER_FATAL_ERROR) {
schur_complement_->BackSubstitute(
reduced_linear_system_solution_.data(), x);
}
}
-
- VLOG(2) << "CG Iterations : " << cg_summary.num_iterations;
-
event_logger.AddEvent("Solve");
return cg_summary;
}
diff --git a/internal/ceres/jet_quaternion_integration_test.cc b/internal/ceres/jet_quaternion_integration_test.cc
deleted file mode 100644
index 63101fb..0000000
--- a/internal/ceres/jet_quaternion_integration_test.cc
+++ /dev/null
@@ -1,201 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: keir@google.com (Keir Mierle)
-//
-// Tests the use of Cere's Jet type with the quaternions found in util/math/. In
-// theory, the unittests for the quaternion class should be type parameterized
-// to make for easier testing of instantiations of the quaternion class, but it
-// is not so, and not obviously worth the work to make the switch at this time.
-
-#include "base/stringprintf.h"
-#include "gtest/gtest.h"
-#include "util/math/mathlimits.h"
-#include "util/math/matrix3x3.h"
-#include "util/math/quaternion.h"
-#include "util/math/vector3.h"
-#include "ceres/test_util.h"
-#include "ceres/jet.h"
-#include "ceres/jet_traits.h"
-
-namespace ceres {
-namespace internal {
-
-// Use a 4-element derivative to simulate the case where each of the
-// quaternion elements are derivative parameters.
-typedef Jet<double, 4> J;
-
-struct JetTraitsTest : public ::testing::Test {
- protected:
- JetTraitsTest()
- : a(J(1.1, 0), J(2.1, 1), J(3.1, 2), J(4.1, 3)),
- b(J(0.1, 0), J(1.1, 1), J(2.1, 2), J(5.0, 3)),
- double_a(a[0].a, a[1].a, a[2].a, a[3].a),
- double_b(b[0].a, b[1].a, b[2].a, b[3].a) {
- // The quaternions should be valid rotations, so normalize them.
- a.Normalize();
- b.Normalize();
- double_a.Normalize();
- double_b.Normalize();
- }
-
- virtual ~JetTraitsTest() {}
-
- // A couple of arbitrary normalized quaternions.
- Quaternion<J> a, b;
-
- // The equivalent of a, b but in scalar form.
- Quaternion<double> double_a, double_b;
-};
-
-// Compare scalar multiplication to jet multiplication. Ignores derivatives.
-TEST_F(JetTraitsTest, QuaternionScalarMultiplicationWorks) {
- Quaternion<J> c = a * b;
- Quaternion<double> double_c = double_a * double_b;
-
- for (int i = 0; i < 4; ++i) {
- EXPECT_EQ(double_c[i], c[i].a);
- }
-}
-
-// Compare scalar slerp to jet slerp. Ignores derivatives.
-TEST_F(JetTraitsTest, QuaternionScalarSlerpWorks) {
- const J fraction(0.1);
- Quaternion<J> c = Quaternion<J>::Slerp(a, b, fraction);
- Quaternion<double> double_c =
- Quaternion<double>::Slerp(double_a, double_b, fraction.a);
-
- for (int i = 0; i < 4; ++i) {
- EXPECT_EQ(double_c[i], c[i].a);
- }
-}
-
-// On a 32-bit optimized build, the mismatch is about 1.4e-14.
-double const kTolerance = 1e-13;
-
-void ExpectJetsClose(const J &x, const J &y) {
- ExpectClose(x.a, y.a, kTolerance);
- ExpectClose(x.v[0], y.v[0], kTolerance);
- ExpectClose(x.v[1], y.v[1], kTolerance);
- ExpectClose(x.v[2], y.v[2], kTolerance);
- ExpectClose(x.v[3], y.v[3], kTolerance);
-}
-
-void ExpectQuaternionsClose(const Quaternion<J>& x, const Quaternion<J>& y) {
- for (int i = 0; i < 4; ++i) {
- ExpectJetsClose(x[i], y[i]);
- }
-}
-
-// Compare jet slurp to jet slerp using identies, checking derivatives.
-TEST_F(JetTraitsTest, CheckSlerpIdentitiesWithNontrivialDerivatives) {
- // Do a slerp to 0.75 directly.
- Quaternion<J> direct = Quaternion<J>::Slerp(a, b, J(0.75));
-
- // Now go part way twice, in theory ending at the same place.
- Quaternion<J> intermediate = Quaternion<J>::Slerp(a, b, J(0.5));
- Quaternion<J> indirect = Quaternion<J>::Slerp(intermediate, b, J(0.5));
-
- // Check that the destination is the same, including derivatives.
- ExpectQuaternionsClose(direct, indirect);
-}
-
-TEST_F(JetTraitsTest, CheckAxisAngleIsInvertibleWithNontrivialDerivatives) {
- Vector3<J> axis;
- J angle;
- a.GetAxisAngle(&axis, &angle);
- b.SetFromAxisAngle(axis, angle);
-
- ExpectQuaternionsClose(a, b);
-}
-
-TEST_F(JetTraitsTest,
- CheckRotationMatrixIsInvertibleWithNontrivialDerivatives) {
- Vector3<J> axis;
- J angle;
- Matrix3x3<J> R;
- a.ToRotationMatrix(&R);
- b.SetFromRotationMatrix(R);
-
- ExpectQuaternionsClose(a, b);
-}
-
-// This doesn't check correctnenss, only that the instantiation compiles.
-TEST_F(JetTraitsTest, CheckRotationBetweenIsCompilable) {
- // Get two arbitrary vectors x and y.
- Vector3<J> x, y;
- J ignored_angle;
- a.GetAxisAngle(&x, &ignored_angle);
- b.GetAxisAngle(&y, &ignored_angle);
-
- Quaternion<J> between_x_and_y = Quaternion<J>::RotationBetween(x, y);
-
- // Prevent optimizing this away.
- EXPECT_NE(between_x_and_y[0].a, 0.0);
-}
-
-TEST_F(JetTraitsTest, CheckRotatedWorksAsExpected) {
- // Get two arbitrary vectors x and y.
- Vector3<J> x;
- J ignored_angle;
- a.GetAxisAngle(&x, &ignored_angle);
-
- // Rotate via a quaternion.
- Vector3<J> y = b.Rotated(x);
-
- // Rotate via a rotation matrix.
- Matrix3x3<J> R;
- b.ToRotationMatrix(&R);
- Vector3<J> yp = R * x;
-
- ExpectJetsClose(yp[0], y[0]);
- ExpectJetsClose(yp[1], y[1]);
- ExpectJetsClose(yp[2], y[2]);
-}
-
-TEST_F(JetTraitsTest, CheckRotatedWorksAsExpectedWithDoubles) {
- // Get two arbitrary vectors x and y.
- Vector3<double> x;
- double ignored_angle;
- double_a.GetAxisAngle(&x, &ignored_angle);
-
- // Rotate via a quaternion.
- Vector3<double> y = double_b.Rotated(x);
-
- // Rotate via a rotation matrix.
- Matrix3x3<double> R;
- double_b.ToRotationMatrix(&R);
- Vector3<double> yp = R * x;
-
- ExpectClose(yp[0], y[0], kTolerance);
- ExpectClose(yp[1], y[1], kTolerance);
- ExpectClose(yp[2], y[2], kTolerance);
-}
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/lapack.cc b/internal/ceres/lapack.cc
index 73bfa69..e124d75 100644
--- a/internal/ceres/lapack.cc
+++ b/internal/ceres/lapack.cc
@@ -29,6 +29,9 @@
// Author: sameeragarwal@google.com (Sameer Agarwal)
#include "ceres/lapack.h"
+
+#include "ceres/internal/port.h"
+#include "ceres/linear_solver.h"
#include "glog/logging.h"
// C interface to the LAPACK Cholesky factorization and triangular solve.
@@ -63,12 +66,14 @@
namespace ceres {
namespace internal {
-int LAPACK::SolveInPlaceUsingCholesky(int num_rows,
- const double* in_lhs,
- double* rhs_and_solution) {
+LinearSolverTerminationType LAPACK::SolveInPlaceUsingCholesky(
+ int num_rows,
+ const double* in_lhs,
+ double* rhs_and_solution,
+ string* message) {
#ifdef CERES_NO_LAPACK
LOG(FATAL) << "Ceres was built without a BLAS library.";
- return -1;
+ return LINEAR_SOLVER_FATAL_ERROR;
#else
char uplo = 'L';
int n = num_rows;
@@ -77,17 +82,33 @@
double* lhs = const_cast<double*>(in_lhs);
dpotrf_(&uplo, &n, lhs, &n, &info);
- if (info != 0) {
- LOG(INFO) << "Cholesky factorization (dpotrf) failed: " << info;
- return info;
+ if (info < 0) {
+ LOG(FATAL) << "Congratulations, you found a bug in Ceres."
+ << "Please report it."
+ << "LAPACK::dpotrf fatal error."
+ << "Argument: " << -info << " is invalid.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ }
+
+ if (info > 0) {
+ *message =
+ StringPrintf(
+ "LAPACK::dpotrf numerical failure. "
+ "The leading minor of order %d is not positive definite.", info);
+ return LINEAR_SOLVER_FAILURE;
}
dpotrs_(&uplo, &n, &nrhs, lhs, &n, rhs_and_solution, &n, &info);
- if (info != 0) {
- LOG(INFO) << "Triangular solve (dpotrs) failed: " << info;
+ if (info < 0) {
+ LOG(FATAL) << "Congratulations, you found a bug in Ceres."
+ << "Please report it."
+ << "LAPACK::dpotrs fatal error."
+ << "Argument: " << -info << " is invalid.";
+ return LINEAR_SOLVER_FATAL_ERROR;
}
- return info;
+ *message = "Success";
+ return LINEAR_SOLVER_SUCCESS;
#endif
};
@@ -113,20 +134,27 @@
&lwork,
&info);
- CHECK_EQ(info, 0);
- return work;
+ if (info < 0) {
+ LOG(FATAL) << "Congratulations, you found a bug in Ceres."
+ << "Please report it."
+ << "LAPACK::dgels fatal error."
+ << "Argument: " << -info << " is invalid.";
+ }
+ return static_cast<int>(work);
#endif
}
-int LAPACK::SolveUsingQR(int num_rows,
- int num_cols,
- const double* in_lhs,
- int work_size,
- double* work,
- double* rhs_and_solution) {
+LinearSolverTerminationType LAPACK::SolveInPlaceUsingQR(
+ int num_rows,
+ int num_cols,
+ const double* in_lhs,
+ int work_size,
+ double* work,
+ double* rhs_and_solution,
+ string* message) {
#ifdef CERES_NO_LAPACK
LOG(FATAL) << "Ceres was built without a LAPACK library.";
- return -1;
+ return LINEAR_SOLVER_FATAL_ERROR;
#else
char trans = 'N';
int m = num_rows;
@@ -149,7 +177,15 @@
&work_size,
&info);
- return info;
+ if (info < 0) {
+ LOG(FATAL) << "Congratulations, you found a bug in Ceres."
+ << "Please report it."
+ << "LAPACK::dgels fatal error."
+ << "Argument: " << -info << " is invalid.";
+ }
+
+ *message = "Success.";
+ return LINEAR_SOLVER_SUCCESS;
#endif
}
diff --git a/internal/ceres/lapack.h b/internal/ceres/lapack.h
index 4f3a88c..8933c2c 100644
--- a/internal/ceres/lapack.h
+++ b/internal/ceres/lapack.h
@@ -31,6 +31,10 @@
#ifndef CERES_INTERNAL_LAPACK_H_
#define CERES_INTERNAL_LAPACK_H_
+#include <string>
+#include "ceres/internal/port.h"
+#include "ceres/linear_solver.h"
+
namespace ceres {
namespace internal {
@@ -47,10 +51,14 @@
//
// This function uses the LAPACK dpotrf and dpotrs routines.
//
- // The return value is zero if the solve is successful.
- static int SolveInPlaceUsingCholesky(int num_rows,
- const double* lhs,
- double* rhs_and_solution);
+ // The return value and the message string together describe whether
+ // the solver terminated successfully or not and if so, what was the
+ // reason for failure.
+ static LinearSolverTerminationType SolveInPlaceUsingCholesky(
+ int num_rows,
+ const double* lhs,
+ double* rhs_and_solution,
+ string* message);
// The SolveUsingQR function requires a buffer for its temporary
// computation. This function given the size of the lhs matrix will
@@ -73,13 +81,17 @@
//
// This function uses the LAPACK dgels routine.
//
- // The return value is zero if the solve is successful.
- static int SolveUsingQR(int num_rows,
- int num_cols,
- const double* lhs,
- int work_size,
- double* work,
- double* rhs_and_solution);
+ // The return value and the message string together describe whether
+ // the solver terminated successfully or not and if so, what was the
+ // reason for failure.
+ static LinearSolverTerminationType SolveInPlaceUsingQR(
+ int num_rows,
+ int num_cols,
+ const double* lhs,
+ int work_size,
+ double* work,
+ double* rhs_and_solution,
+ string* message);
};
} // namespace internal
diff --git a/internal/ceres/levenberg_marquardt_strategy.cc b/internal/ceres/levenberg_marquardt_strategy.cc
index fad7c1f..ce3b69a 100644
--- a/internal/ceres/levenberg_marquardt_strategy.cc
+++ b/internal/ceres/levenberg_marquardt_strategy.cc
@@ -105,10 +105,13 @@
// do not need to be modified.
LinearSolver::Summary linear_solver_summary =
linear_solver_->Solve(jacobian, residuals, solve_options, step);
- if (linear_solver_summary.termination_type == FAILURE ||
- !IsArrayValid(num_parameters, step)) {
+
+ if (linear_solver_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) {
+ LOG(WARNING) << "Linear solver fatal error.";
+ } else if (linear_solver_summary.termination_type == LINEAR_SOLVER_FAILURE ||
+ !IsArrayValid(num_parameters, step)) {
LOG(WARNING) << "Linear solver failure. Failed to compute a finite step.";
- linear_solver_summary.termination_type = FAILURE;
+ linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE;
} else {
VectorRef(step, num_parameters) *= -1.0;
}
diff --git a/internal/ceres/levenberg_marquardt_strategy_test.cc b/internal/ceres/levenberg_marquardt_strategy_test.cc
index 86302b7..ac7ddbc 100644
--- a/internal/ceres/levenberg_marquardt_strategy_test.cc
+++ b/internal/ceres/levenberg_marquardt_strategy_test.cc
@@ -150,7 +150,7 @@
TrustRegionStrategy::Summary summary =
lms.ComputeStep(pso, &dsm, &residual, x);
- EXPECT_EQ(summary.termination_type, FAILURE);
+ EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_FAILURE);
}
}
diff --git a/internal/ceres/line_search.cc b/internal/ceres/line_search.cc
index 8323896..7ff1164 100644
--- a/internal/ceres/line_search.cc
+++ b/internal/ceres/line_search.cc
@@ -28,7 +28,9 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
+#include <iomanip>
+#include <iostream> // NOLINT
+
#include "ceres/line_search.h"
#include "ceres/fpclassify.h"
@@ -41,6 +43,8 @@
namespace ceres {
namespace internal {
namespace {
+// Precision used for floating point values in error message output.
+const int kErrorMessageNumericPrecision = 8;
FunctionSample ValueSample(const double x, const double value) {
FunctionSample sample;
@@ -64,13 +68,12 @@
} // namespace
+
+std::ostream& operator<<(std::ostream &os, const FunctionSample& sample);
+
// Convenience stream operator for pushing FunctionSamples into log messages.
-std::ostream& operator<<(std::ostream &os,
- const FunctionSample& sample) {
- os << "[x: " << sample.x << ", value: " << sample.value
- << ", gradient: " << sample.gradient << ", value_is_valid: "
- << std::boolalpha << sample.value_is_valid << ", gradient_is_valid: "
- << std::boolalpha << sample.gradient_is_valid << "]";
+std::ostream& operator<<(std::ostream &os, const FunctionSample& sample) {
+ os << sample.ToDebugString();
return os;
}
@@ -170,6 +173,7 @@
// to avoid replicating current.value_is_valid == false
// behaviour in WolfeLineSearch.
CHECK(lowerbound.value_is_valid)
+ << std::scientific << std::setprecision(kErrorMessageNumericPrecision)
<< "Ceres bug: lower-bound sample for interpolation is invalid, "
<< "please contact the developers!, interpolation_type: "
<< LineSearchInterpolationTypeToString(interpolation_type)
@@ -237,20 +241,26 @@
FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0);
current.value_is_valid = false;
- const bool interpolation_uses_gradients =
+ // As the Armijo line search algorithm always uses the initial point, for
+ // which both the function value and derivative are known, when fitting a
+ // minimizing polynomial, we can fit up to a quadratic without requiring the
+ // gradient at the current query point.
+ const bool interpolation_uses_gradient_at_current_sample =
options().interpolation_type == CUBIC;
const double descent_direction_max_norm =
static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm();
++summary->num_function_evaluations;
- if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; }
+ if (interpolation_uses_gradient_at_current_sample) {
+ ++summary->num_gradient_evaluations;
+ }
current.value_is_valid =
function->Evaluate(current.x,
¤t.value,
- interpolation_uses_gradients
+ interpolation_uses_gradient_at_current_sample
? ¤t.gradient : NULL);
current.gradient_is_valid =
- interpolation_uses_gradients && current.value_is_valid;
+ interpolation_uses_gradient_at_current_sample && current.value_is_valid;
while (!current.value_is_valid ||
current.value > (initial_cost
+ options().sufficient_decrease
@@ -265,7 +275,7 @@
"satisfying the sufficient decrease condition within "
"specified max_num_iterations: %d.",
options().max_num_iterations);
- LOG(WARNING) << summary->error;
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
return;
}
@@ -283,7 +293,7 @@
StringPrintf("Line search failed: step_size too small: %.5e "
"with descent_direction_max_norm: %.5e.", step_size,
descent_direction_max_norm);
- LOG(WARNING) << summary->error;
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
return;
}
@@ -291,14 +301,16 @@
current.x = step_size;
++summary->num_function_evaluations;
- if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; }
+ if (interpolation_uses_gradient_at_current_sample) {
+ ++summary->num_gradient_evaluations;
+ }
current.value_is_valid =
function->Evaluate(current.x,
¤t.value,
- interpolation_uses_gradients
+ interpolation_uses_gradient_at_current_sample
? ¤t.gradient : NULL);
current.gradient_is_valid =
- interpolation_uses_gradients && current.value_is_valid;
+ interpolation_uses_gradient_at_current_sample && current.value_is_valid;
}
summary->optimal_step_size = current.x;
@@ -350,33 +362,36 @@
&bracket_low,
&bracket_high,
&do_zoom_search,
- summary) &&
- summary->num_iterations < options().max_num_iterations) {
- // Failed to find either a valid point or a valid bracket, but we did not
- // run out of iterations.
+ summary)) {
+ // Failed to find either a valid point, a valid bracket satisfying the Wolfe
+ // conditions, or even a step size > minimum tolerance satisfying the Armijo
+ // condition.
return;
}
+
if (!do_zoom_search) {
// Either: Bracketing phase already found a point satisfying the strong
// Wolfe conditions, thus no Zoom required.
//
// Or: Bracketing failed to find a valid bracket or a point satisfying the
- // strong Wolfe conditions within max_num_iterations. As this is an
- // 'artificial' constraint, and we would otherwise fail to produce a valid
- // point when ArmijoLineSearch would succeed, we return the lowest point
- // found thus far which satsifies the Armijo condition (but not the Wolfe
- // conditions).
- CHECK(bracket_low.value_is_valid)
- << "Ceres bug: Bracketing produced an invalid bracket_low, please "
- << "contact the developers!, bracket_low: " << bracket_low
- << ", bracket_high: " << bracket_high << ", num_iterations: "
- << summary->num_iterations << ", max_num_iterations: "
- << options().max_num_iterations;
+ // strong Wolfe conditions within max_num_iterations, or whilst searching
+ // shrank the bracket width until it was below our minimum tolerance.
+ // As these are 'artificial' constraints, and we would otherwise fail to
+ // produce a valid point when ArmijoLineSearch would succeed, we return the
+ // point with the lowest cost found thus far which satsifies the Armijo
+ // condition (but not the Wolfe conditions).
summary->optimal_step_size = bracket_low.x;
summary->success = true;
return;
}
+ VLOG(3) << std::scientific << std::setprecision(kErrorMessageNumericPrecision)
+ << "Starting line search zoom phase with bracket_low: "
+ << bracket_low << ", bracket_high: " << bracket_high
+ << ", bracket width: " << fabs(bracket_low.x - bracket_high.x)
+ << ", bracket abs delta cost: "
+ << fabs(bracket_low.value - bracket_high.value);
+
// Wolfe Zoom phase: Called when the Bracketing phase finds an interval of
// non-zero, finite width that should bracket step sizes which satisfy the
// (strong) Wolfe conditions (before finding a step size that satisfies the
@@ -419,11 +434,22 @@
summary->success = true;
}
-// Returns true iff bracket_low & bracket_high bound a bracket that contains
-// points which satisfy the strong Wolfe conditions. Otherwise, on return false,
-// if we stopped searching due to the 'artificial' condition of reaching
-// max_num_iterations, bracket_low is the step size amongst all those
-// tested, which satisfied the Armijo decrease condition and minimized f().
+// Returns true if either:
+//
+// A termination condition satisfying the (strong) Wolfe bracketing conditions
+// is found:
+//
+// - A valid point, defined as a bracket of zero width [zoom not required].
+// - A valid bracket (of width > tolerance), [zoom required].
+//
+// Or, searching was stopped due to an 'artificial' constraint, i.e. not
+// a condition imposed / required by the underlying algorithm, but instead an
+// engineering / implementation consideration. But a step which exceeds the
+// minimum step size, and satsifies the Armijo condition was still found,
+// and should thus be used [zoom not required].
+//
+// Returns false if no step size > minimum step size was found which
+// satisfies at least the Armijo condition.
bool WolfeLineSearch::BracketingPhase(
const FunctionSample& initial_position,
const double step_size_estimate,
@@ -437,23 +463,28 @@
FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0);
current.value_is_valid = false;
- const bool interpolation_uses_gradients =
- options().interpolation_type == CUBIC;
const double descent_direction_max_norm =
static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm();
*do_zoom_search = false;
*bracket_low = initial_position;
+ // As we require the gradient to evaluate the Wolfe condition, we always
+ // calculate it together with the value, irrespective of the interpolation
+ // type. As opposed to only calculating the gradient after the Armijo
+ // condition is satisifed, as the computational saving from this approach
+ // would be slight (perhaps even negative due to the extra call). Also,
+ // always calculating the value & gradient together protects against us
+ // reporting invalid solutions if the cost function returns slightly different
+ // function values when evaluated with / without gradients (due to numerical
+ // issues).
++summary->num_function_evaluations;
- if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; }
+ ++summary->num_gradient_evaluations;
current.value_is_valid =
function->Evaluate(current.x,
¤t.value,
- interpolation_uses_gradients
- ? ¤t.gradient : NULL);
- current.gradient_is_valid =
- interpolation_uses_gradients && current.value_is_valid;
+ ¤t.gradient);
+ current.gradient_is_valid = current.value_is_valid;
while (true) {
++summary->num_iterations;
@@ -470,22 +501,14 @@
*do_zoom_search = true;
*bracket_low = previous;
*bracket_high = current;
+ VLOG(3) << std::scientific
+ << std::setprecision(kErrorMessageNumericPrecision)
+ << "Bracket found: current step (" << current.x
+ << ") violates Armijo sufficient condition, or has passed an "
+ << "inflection point of f() based on value.";
break;
}
- // Irrespective of the interpolation type we are using, we now need the
- // gradient at the current point (which satisfies the Armijo condition)
- // in order to check the strong Wolfe conditions.
- if (!interpolation_uses_gradients) {
- ++summary->num_function_evaluations;
- ++summary->num_gradient_evaluations;
- current.value_is_valid =
- function->Evaluate(current.x,
- ¤t.value,
- ¤t.gradient);
- current.gradient_is_valid = current.value_is_valid;
- }
-
if (current.value_is_valid &&
fabs(current.gradient) <=
-options().sufficient_curvature_decrease * initial_position.gradient) {
@@ -493,6 +516,11 @@
// valid termination point, therefore a Zoom not required.
*bracket_low = current;
*bracket_high = current;
+ VLOG(3) << std::scientific
+ << std::setprecision(kErrorMessageNumericPrecision)
+ << "Bracketing phase found step size: " << current.x
+ << ", satisfying strong Wolfe conditions, initial_position: "
+ << initial_position << ", current: " << current;
break;
} else if (current.value_is_valid && current.gradient >= 0) {
@@ -505,6 +533,30 @@
// Note inverse ordering from first bracket case.
*bracket_low = current;
*bracket_high = previous;
+ VLOG(3) << "Bracket found: current step (" << current.x
+ << ") satisfies Armijo, but has gradient >= 0, thus have passed "
+ << "an inflection point of f().";
+ break;
+
+ } else if (current.value_is_valid &&
+ fabs(current.x - previous.x) * descent_direction_max_norm
+ < options().min_step_size) {
+ // We have shrunk the search bracket to a width less than our tolerance,
+ // and still not found either a point satisfying the strong Wolfe
+ // conditions, or a valid bracket containing such a point. Stop searching
+ // and set bracket_low to the size size amongst all those tested which
+ // minimizes f() and satisfies the Armijo condition.
+ LOG_IF(WARNING, !options().is_silent)
+ << "Line search failed: Wolfe bracketing phase shrank "
+ << "bracket width: " << fabs(current.x - previous.x)
+ << ", to < tolerance: " << options().min_step_size
+ << ", with descent_direction_max_norm: "
+ << descent_direction_max_norm << ", and failed to find "
+ << "a point satisfying the strong Wolfe conditions or a "
+ << "bracketing containing such a point. Accepting "
+ << "point found satisfying Armijo condition only, to "
+ << "allow continuation.";
+ *bracket_low = current;
break;
} else if (summary->num_iterations >= options().max_num_iterations) {
@@ -516,14 +568,14 @@
"find a point satisfying strong Wolfe conditions, or a "
"bracket containing such a point within specified "
"max_num_iterations: %d", options().max_num_iterations);
- LOG(WARNING) << summary->error;
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
// Ensure that bracket_low is always set to the step size amongst all
// those tested which minimizes f() and satisfies the Armijo condition
// when we terminate due to the 'artificial' max_num_iterations condition.
*bracket_low =
current.value_is_valid && current.value < bracket_low->value
? current : *bracket_low;
- return false;
+ break;
}
// Either: f(current) is invalid; or, f(current) is valid, but does not
// satisfy the strong Wolfe conditions itself, or the conditions for
@@ -555,7 +607,7 @@
StringPrintf("Line search failed: step_size too small: %.5e "
"with descent_direction_max_norm: %.5e", step_size,
descent_direction_max_norm);
- LOG(WARNING) << summary->error;
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
return false;
}
@@ -563,17 +615,22 @@
current.x = step_size;
++summary->num_function_evaluations;
- if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; }
+ ++summary->num_gradient_evaluations;
current.value_is_valid =
function->Evaluate(current.x,
¤t.value,
- interpolation_uses_gradients
- ? ¤t.gradient : NULL);
- current.gradient_is_valid =
- interpolation_uses_gradients && current.value_is_valid;
+ ¤t.gradient);
+ current.gradient_is_valid = current.value_is_valid;
}
- // Either we have a valid point, defined as a bracket of zero width, in which
- // case no zoom is required, or a valid bracket in which to zoom.
+
+ // Ensure that even if a valid bracket was found, we will only mark a zoom
+ // as required if the bracket's width is greater than our minimum tolerance.
+ if (*do_zoom_search &&
+ fabs(bracket_high->x - bracket_low->x) * descent_direction_max_norm
+ < options().min_step_size) {
+ *do_zoom_search = false;
+ }
+
return true;
}
@@ -589,6 +646,7 @@
Function* function = options().function;
CHECK(bracket_low.value_is_valid && bracket_low.gradient_is_valid)
+ << std::scientific << std::setprecision(kErrorMessageNumericPrecision)
<< "Ceres bug: f_low input to Wolfe Zoom invalid, please contact "
<< "the developers!, initial_position: " << initial_position
<< ", bracket_low: " << bracket_low
@@ -599,22 +657,46 @@
// not have been calculated (if bracket_high.value does not satisfy the
// Armijo sufficient decrease condition and interpolation method does not
// require it).
+ //
+ // We also do not require that: bracket_low.value < bracket_high.value,
+ // although this is typical. This is to deal with the case when
+ // bracket_low = initial_position, bracket_high is the first sample,
+ // and bracket_high does not satisfy the Armijo condition, but still has
+ // bracket_high.value < initial_position.value.
CHECK(bracket_high.value_is_valid)
+ << std::scientific << std::setprecision(kErrorMessageNumericPrecision)
<< "Ceres bug: f_high input to Wolfe Zoom invalid, please "
<< "contact the developers!, initial_position: " << initial_position
<< ", bracket_low: " << bracket_low
<< ", bracket_high: "<< bracket_high;
- CHECK_LT(bracket_low.gradient *
- (bracket_high.x - bracket_low.x), 0.0)
- << "Ceres bug: f_high input to Wolfe Zoom does not satisfy gradient "
- << "condition combined with f_low, please contact the developers!"
- << ", initial_position: " << initial_position
- << ", bracket_low: " << bracket_low
- << ", bracket_high: "<< bracket_high;
+
+ if (bracket_low.gradient * (bracket_high.x - bracket_low.x) >= 0) {
+ // The third condition for a valid initial bracket:
+ //
+ // 3. bracket_high is chosen after bracket_low, s.t.
+ // bracket_low.gradient * (bracket_high.x - bracket_low.x) < 0.
+ //
+ // is not satisfied. As this can happen when the users' cost function
+ // returns inconsistent gradient values relative to the function values,
+ // we do not CHECK_LT(), but we do stop processing and return an invalid
+ // value.
+ summary->error =
+ StringPrintf("Line search failed: Wolfe zoom phase passed a bracket "
+ "which does not satisfy: bracket_low.gradient * "
+ "(bracket_high.x - bracket_low.x) < 0 [%.8e !< 0] "
+ "with initial_position: %s, bracket_low: %s, bracket_high:"
+ " %s, the most likely cause of which is the cost function "
+ "returning inconsistent gradient & function values.",
+ bracket_low.gradient * (bracket_high.x - bracket_low.x),
+ initial_position.ToDebugString().c_str(),
+ bracket_low.ToDebugString().c_str(),
+ bracket_high.ToDebugString().c_str());
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
+ solution->value_is_valid = false;
+ return false;
+ }
const int num_bracketing_iterations = summary->num_iterations;
- const bool interpolation_uses_gradients =
- options().interpolation_type == CUBIC;
const double descent_direction_max_norm =
static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm();
@@ -630,7 +712,7 @@
"within specified max_num_iterations: %d, "
"(num iterations taken for bracketing: %d).",
options().max_num_iterations, num_bracketing_iterations);
- LOG(WARNING) << summary->error;
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
return false;
}
if (fabs(bracket_high.x - bracket_low.x) * descent_direction_max_norm
@@ -642,7 +724,7 @@
"too small with descent_direction_max_norm: %.5e.",
fabs(bracket_high.x - bracket_low.x),
descent_direction_max_norm);
- LOG(WARNING) << summary->error;
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
return false;
}
@@ -669,15 +751,23 @@
upper_bound_step.x);
// No check on magnitude of step size being too small here as it is
// lower-bounded by the initial bracket start point, which was valid.
+ //
+ // As we require the gradient to evaluate the Wolfe condition, we always
+ // calculate it together with the value, irrespective of the interpolation
+ // type. As opposed to only calculating the gradient after the Armijo
+ // condition is satisifed, as the computational saving from this approach
+ // would be slight (perhaps even negative due to the extra call). Also,
+ // always calculating the value & gradient together protects against us
+ // reporting invalid solutions if the cost function returns slightly
+ // different function values when evaluated with / without gradients (due
+ // to numerical issues).
++summary->num_function_evaluations;
- if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; }
+ ++summary->num_gradient_evaluations;
solution->value_is_valid =
function->Evaluate(solution->x,
&solution->value,
- interpolation_uses_gradients
- ? &solution->gradient : NULL);
- solution->gradient_is_valid =
- interpolation_uses_gradients && solution->value_is_valid;
+ &solution->gradient);
+ solution->gradient_is_valid = solution->value_is_valid;
if (!solution->value_is_valid) {
summary->error =
StringPrintf("Line search failed: Wolfe Zoom phase found "
@@ -685,10 +775,16 @@
"between low_step: %.5e and high_step: %.5e "
"at which function is valid.",
solution->x, bracket_low.x, bracket_high.x);
- LOG(WARNING) << summary->error;
+ LOG_IF(WARNING, !options().is_silent) << summary->error;
return false;
}
+ VLOG(3) << "Zoom iteration: "
+ << summary->num_iterations - num_bracketing_iterations
+ << ", bracket_low: " << bracket_low
+ << ", bracket_high: " << bracket_high
+ << ", minimizing solution: " << *solution;
+
if ((solution->value > (initial_position.value
+ options().sufficient_decrease
* initial_position.gradient
@@ -701,31 +797,13 @@
}
// Armijo sufficient decrease satisfied, check strong Wolfe condition.
- if (!interpolation_uses_gradients) {
- // Irrespective of the interpolation type we are using, we now need the
- // gradient at the current point (which satisfies the Armijo condition)
- // in order to check the strong Wolfe conditions.
- ++summary->num_function_evaluations;
- ++summary->num_gradient_evaluations;
- solution->value_is_valid =
- function->Evaluate(solution->x,
- &solution->value,
- &solution->gradient);
- solution->gradient_is_valid = solution->value_is_valid;
- if (!solution->value_is_valid) {
- summary->error =
- StringPrintf("Line search failed: Wolfe Zoom phase found "
- "step_size: %.5e, for which function is invalid, "
- "between low_step: %.5e and high_step: %.5e "
- "at which function is valid.",
- solution->x, bracket_low.x, bracket_high.x);
- LOG(WARNING) << summary->error;
- return false;
- }
- }
if (fabs(solution->gradient) <=
-options().sufficient_curvature_decrease * initial_position.gradient) {
// Found a valid termination point satisfying strong Wolfe conditions.
+ VLOG(3) << std::scientific
+ << std::setprecision(kErrorMessageNumericPrecision)
+ << "Zoom phase found step size: " << solution->x
+ << ", satisfying strong Wolfe conditions.";
break;
} else if (solution->gradient * (bracket_high.x - bracket_low.x) >= 0) {
@@ -741,5 +819,3 @@
} // namespace internal
} // namespace ceres
-
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
diff --git a/internal/ceres/line_search.h b/internal/ceres/line_search.h
index 5f24e9f..97b9bc6 100644
--- a/internal/ceres/line_search.h
+++ b/internal/ceres/line_search.h
@@ -33,8 +33,6 @@
#ifndef CERES_INTERNAL_LINE_SEARCH_H_
#define CERES_INTERNAL_LINE_SEARCH_H_
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
-
#include <string>
#include <vector>
#include "ceres/internal/eigen.h"
@@ -71,6 +69,7 @@
max_num_iterations(20),
sufficient_curvature_decrease(0.9),
max_step_expansion(10.0),
+ is_silent(false),
function(NULL) {}
// Degree of the polynomial used to approximate the objective
@@ -144,6 +143,8 @@
// By definition for expansion, max_step_expansion > 1.0.
double max_step_expansion;
+ bool is_silent;
+
// The one dimensional function that the line search algorithm
// minimizes.
Function* function;
@@ -295,5 +296,4 @@
} // namespace internal
} // namespace ceres
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
#endif // CERES_INTERNAL_LINE_SEARCH_H_
diff --git a/internal/ceres/line_search_direction.cc b/internal/ceres/line_search_direction.cc
index 8ded823..dddcecd 100644
--- a/internal/ceres/line_search_direction.cc
+++ b/internal/ceres/line_search_direction.cc
@@ -28,8 +28,6 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
-
#include "ceres/line_search_direction.h"
#include "ceres/line_search_minimizer.h"
#include "ceres/low_rank_inverse_hessian.h"
@@ -67,7 +65,7 @@
case FLETCHER_REEVES:
beta = current.gradient_squared_norm / previous.gradient_squared_norm;
break;
- case POLAK_RIBIRERE:
+ case POLAK_RIBIERE:
gradient_change = current.gradient - previous.gradient;
beta = (current.gradient.dot(gradient_change) /
previous.gradient_squared_norm);
@@ -121,6 +119,7 @@
low_rank_inverse_hessian_.Update(
previous.search_direction * previous.step_size,
current.gradient - previous.gradient);
+
search_direction->setZero();
low_rank_inverse_hessian_.RightMultiply(current.gradient.data(),
search_direction->data());
@@ -176,9 +175,46 @@
const Vector delta_gradient = current.gradient - previous.gradient;
const double delta_x_dot_delta_gradient = delta_x.dot(delta_gradient);
- if (delta_x_dot_delta_gradient <= 1e-10) {
+ // The (L)BFGS algorithm explicitly requires that the secant equation:
+ //
+ // B_{k+1} * s_k = y_k
+ //
+ // Is satisfied at each iteration, where B_{k+1} is the approximated
+ // Hessian at the k+1-th iteration, s_k = (x_{k+1} - x_{k}) and
+ // y_k = (grad_{k+1} - grad_{k}). As the approximated Hessian must be
+ // positive definite, this is equivalent to the condition:
+ //
+ // s_k^T * y_k > 0 [s_k^T * B_{k+1} * s_k = s_k^T * y_k > 0]
+ //
+ // This condition would always be satisfied if the function was strictly
+ // convex, alternatively, it is always satisfied provided that a Wolfe line
+ // search is used (even if the function is not strictly convex). See [1]
+ // (p138) for a proof.
+ //
+ // Although Ceres will always use a Wolfe line search when using (L)BFGS,
+ // practical implementation considerations mean that the line search
+ // may return a point that satisfies only the Armijo condition, and thus
+ // could violate the Secant equation. As such, we will only use a step
+ // to update the Hessian approximation if:
+ //
+ // s_k^T * y_k > tolerance
+ //
+ // It is important that tolerance is very small (and >=0), as otherwise we
+ // might skip the update too often and fail to capture important curvature
+ // information in the Hessian. For example going from 1e-10 -> 1e-14
+ // improves the NIST benchmark score from 43/54 to 53/54.
+ //
+ // [1] Nocedal J, Wright S, Numerical Optimization, 2nd Ed. Springer, 1999.
+ //
+ // TODO(alexs.mac): Consider using Damped BFGS update instead of
+ // skipping update.
+ const double kBFGSSecantConditionHessianUpdateTolerance = 1e-14;
+ if (delta_x_dot_delta_gradient <=
+ kBFGSSecantConditionHessianUpdateTolerance) {
VLOG(2) << "Skipping BFGS Update, delta_x_dot_delta_gradient too "
- << "small: " << delta_x_dot_delta_gradient;
+ << "small: " << delta_x_dot_delta_gradient << ", tolerance: "
+ << kBFGSSecantConditionHessianUpdateTolerance
+ << " (Secant condition).";
} else {
// Update dense inverse Hessian approximation.
@@ -214,8 +250,13 @@
// Part II: Implementation and experiments, Management Science,
// 20(5), 863-874, 1974.
// [2] Nocedal J., Wright S., Numerical Optimization, Springer, 1999.
- inverse_hessian_ *=
+ const double approximate_eigenvalue_scale =
delta_x_dot_delta_gradient / delta_gradient.dot(delta_gradient);
+ inverse_hessian_ *= approximate_eigenvalue_scale;
+
+ VLOG(4) << "Applying approximate_eigenvalue_scale: "
+ << approximate_eigenvalue_scale << " to initial inverse "
+ << "Hessian approximation.";
}
initialized_ = true;
@@ -329,5 +370,3 @@
} // namespace internal
} // namespace ceres
-
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
diff --git a/internal/ceres/line_search_direction.h b/internal/ceres/line_search_direction.h
index 0857cb0..c77fdc8 100644
--- a/internal/ceres/line_search_direction.h
+++ b/internal/ceres/line_search_direction.h
@@ -31,8 +31,6 @@
#ifndef CERES_INTERNAL_LINE_SEARCH_DIRECTION_H_
#define CERES_INTERNAL_LINE_SEARCH_DIRECTION_H_
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
-
#include "ceres/internal/eigen.h"
#include "ceres/line_search_minimizer.h"
#include "ceres/types.h"
@@ -71,5 +69,4 @@
} // namespace internal
} // namespace ceres
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
#endif // CERES_INTERNAL_LINE_SEARCH_DIRECTION_H_
diff --git a/internal/ceres/line_search_minimizer.cc b/internal/ceres/line_search_minimizer.cc
index 2cc89fa..ae77a73 100644
--- a/internal/ceres/line_search_minimizer.cc
+++ b/internal/ceres/line_search_minimizer.cc
@@ -38,8 +38,6 @@
// For details on the theory and implementation see "Numerical
// Optimization" by Nocedal & Wright.
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
-
#include "ceres/line_search_minimizer.h"
#include <algorithm>
@@ -64,25 +62,36 @@
namespace ceres {
namespace internal {
namespace {
-// Small constant for various floating point issues.
-// TODO(sameeragarwal): Change to a better name if this has only one
-// use.
-const double kEpsilon = 1e-12;
+// TODO(sameeragarwal): I think there is a small bug here, in that if
+// the evaluation fails, then the state can contain garbage. Look at
+// this more carefully.
bool Evaluate(Evaluator* evaluator,
const Vector& x,
- LineSearchMinimizer::State* state) {
- const bool status = evaluator->Evaluate(x.data(),
- &(state->cost),
- NULL,
- state->gradient.data(),
- NULL);
- if (status) {
- state->gradient_squared_norm = state->gradient.squaredNorm();
- state->gradient_max_norm = state->gradient.lpNorm<Eigen::Infinity>();
+ LineSearchMinimizer::State* state,
+ string* message) {
+ if (!evaluator->Evaluate(x.data(),
+ &(state->cost),
+ NULL,
+ state->gradient.data(),
+ NULL)) {
+ *message = "Gradient evaluation failed.";
+ return false;
}
- return status;
+ Vector negative_gradient = -state->gradient;
+ Vector projected_gradient_step(x.size());
+ if (!evaluator->Plus(x.data(),
+ negative_gradient.data(),
+ projected_gradient_step.data())) {
+ *message = "projected_gradient_step = Plus(x, -gradient) failed.";
+ return false;
+ }
+
+ state->gradient_squared_norm = (x - projected_gradient_step).squaredNorm();
+ state->gradient_max_norm =
+ (x - projected_gradient_step).lpNorm<Eigen::Infinity>();
+ return true;
}
} // namespace
@@ -90,6 +99,7 @@
void LineSearchMinimizer::Minimize(const Minimizer::Options& options,
double* parameters,
Solver::Summary* summary) {
+ const bool is_not_silent = !options.is_silent;
double start_time = WallTimeInSeconds();
double iteration_start_time = start_time;
@@ -115,14 +125,17 @@
iteration_summary.step_is_successful = false;
iteration_summary.cost_change = 0.0;
iteration_summary.gradient_max_norm = 0.0;
+ iteration_summary.gradient_norm = 0.0;
iteration_summary.step_norm = 0.0;
iteration_summary.linear_solver_iterations = 0;
iteration_summary.step_solver_time_in_seconds = 0;
// Do initial cost and Jacobian evaluation.
- if (!Evaluate(evaluator, x, ¤t_state)) {
- LOG(WARNING) << "Terminating: Cost and gradient evaluation failed.";
- summary->termination_type = NUMERICAL_FAILURE;
+ if (!Evaluate(evaluator, x, ¤t_state, &summary->message)) {
+ summary->termination_type = FAILURE;
+ summary->message = "Initial cost and jacobian evaluation failed. "
+ "More details: " + summary->message;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
return;
}
@@ -130,20 +143,15 @@
iteration_summary.cost = current_state.cost + summary->fixed_cost;
iteration_summary.gradient_max_norm = current_state.gradient_max_norm;
+ iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm);
- // The initial gradient max_norm is bounded from below so that we do
- // not divide by zero.
- const double initial_gradient_max_norm =
- max(iteration_summary.gradient_max_norm, kEpsilon);
- const double absolute_gradient_tolerance =
- options.gradient_tolerance * initial_gradient_max_norm;
-
- if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
- summary->termination_type = GRADIENT_TOLERANCE;
- VLOG(1) << "Terminating: Gradient tolerance reached."
- << "Relative gradient max norm: "
- << iteration_summary.gradient_max_norm / initial_gradient_max_norm
- << " <= " << options.gradient_tolerance;
+ if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) {
+ summary->message = StringPrintf("Gradient tolerance reached. "
+ "Gradient max norm: %e <= %e",
+ iteration_summary.gradient_max_norm,
+ options.gradient_tolerance);
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
return;
}
@@ -188,11 +196,10 @@
scoped_ptr<LineSearch>
line_search(LineSearch::Create(options.line_search_type,
line_search_options,
- &summary->error));
+ &summary->message));
if (line_search.get() == NULL) {
- LOG(ERROR) << "Ceres bug: Unable to create a LineSearch object, please "
- << "contact the developers!, error: " << summary->error;
- summary->termination_type = DID_NOT_RUN;
+ summary->termination_type = FAILURE;
+ LOG_IF(ERROR, is_not_silent) << "Terminating: " << summary->message;
return;
}
@@ -200,22 +207,24 @@
int num_line_search_direction_restarts = 0;
while (true) {
- if (!RunCallbacks(options.callbacks, iteration_summary, summary)) {
- return;
+ if (!RunCallbacks(options, iteration_summary, summary)) {
+ break;
}
iteration_start_time = WallTimeInSeconds();
if (iteration_summary.iteration >= options.max_num_iterations) {
+ summary->message = "Maximum number of iterations reached.";
summary->termination_type = NO_CONVERGENCE;
- VLOG(1) << "Terminating: Maximum number of iterations reached.";
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
break;
}
const double total_solver_time = iteration_start_time - start_time +
summary->preprocessor_time_in_seconds;
if (total_solver_time >= options.max_solver_time_in_seconds) {
+ summary->message = "Maximum solver time reached.";
summary->termination_type = NO_CONVERGENCE;
- VLOG(1) << "Terminating: Maximum solver time reached.";
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
break;
}
@@ -240,14 +249,13 @@
// Line search direction failed to generate a new direction, and we
// have already reached our specified maximum number of restarts,
// terminate optimization.
- summary->error =
+ summary->message =
StringPrintf("Line search direction failure: specified "
"max_num_line_search_direction_restarts: %d reached.",
options.max_num_line_search_direction_restarts);
- LOG(WARNING) << summary->error << " terminating optimization.";
- summary->termination_type = NUMERICAL_FAILURE;
+ summary->termination_type = FAILURE;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
break;
-
} else if (!line_search_status) {
// Restart line search direction with gradient descent on first iteration
// as we have not yet reached our maximum number of restarts.
@@ -255,13 +263,16 @@
options.max_num_line_search_direction_restarts);
++num_line_search_direction_restarts;
- LOG(WARNING)
+ LOG_IF(WARNING, is_not_silent)
<< "Line search direction algorithm: "
- << LineSearchDirectionTypeToString(options.line_search_direction_type)
- << ", failed to produce a valid new direction at iteration: "
- << iteration_summary.iteration << ". Restarting, number of "
- << "restarts: " << num_line_search_direction_restarts << " / "
- << options.max_num_line_search_direction_restarts << " [max].";
+ << LineSearchDirectionTypeToString(
+ options.line_search_direction_type)
+ << ", failed to produce a valid new direction at "
+ << "iteration: " << iteration_summary.iteration
+ << ". Restarting, number of restarts: "
+ << num_line_search_direction_restarts << " / "
+ << options.max_num_line_search_direction_restarts
+ << " [max].";
line_search_direction.reset(
LineSearchDirection::Create(line_search_direction_options));
current_state.search_direction = -current_state.gradient;
@@ -286,14 +297,14 @@
// direction in a line search, most likely cause for this being violated
// would be a numerical failure in the line search direction calculation.
if (initial_step_size < 0.0) {
- summary->error =
+ summary->message =
StringPrintf("Numerical failure in line search, initial_step_size is "
"negative: %.5e, directional_derivative: %.5e, "
"(current_cost - previous_cost): %.5e",
initial_step_size, current_state.directional_derivative,
(current_state.cost - previous_state.cost));
- LOG(WARNING) << summary->error;
- summary->termination_type = NUMERICAL_FAILURE;
+ summary->termination_type = FAILURE;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
break;
}
@@ -301,6 +312,18 @@
current_state.cost,
current_state.directional_derivative,
&line_search_summary);
+ if (!line_search_summary.success) {
+ summary->message =
+ StringPrintf("Numerical failure in line search, failed to find "
+ "a valid step size, (did not run out of iterations) "
+ "using initial_step_size: %.5e, initial_cost: %.5e, "
+ "initial_gradient: %.5e.",
+ initial_step_size, current_state.cost,
+ current_state.directional_derivative);
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
+ summary->termination_type = FAILURE;
+ break;
+ }
current_state.step_size = line_search_summary.optimal_step_size;
delta = current_state.step_size * current_state.search_direction;
@@ -309,36 +332,31 @@
iteration_summary.step_solver_time_in_seconds =
WallTimeInSeconds() - iteration_start_time;
- // TODO(sameeragarwal): Collect stats.
- if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data()) ||
- !Evaluate(evaluator, x_plus_delta, ¤t_state)) {
- LOG(WARNING) << "Evaluation failed.";
+ if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+ summary->termination_type = FAILURE;
+ summary->message =
+ "x_plus_delta = Plus(x, delta) failed. This should not happen "
+ "as the step was valid when it was selected by the line search.";
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
+ break;
+ } else if (!Evaluate(evaluator,
+ x_plus_delta,
+ ¤t_state,
+ &summary->message)) {
+ summary->termination_type = FAILURE;
+ summary->message =
+ "Step failed to evaluate. This should not happen as the step was "
+ "valid when it was selected by the line search. More details: " +
+ summary->message;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
+ break;
} else {
x = x_plus_delta;
}
iteration_summary.gradient_max_norm = current_state.gradient_max_norm;
- if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
- summary->termination_type = GRADIENT_TOLERANCE;
- VLOG(1) << "Terminating: Gradient tolerance reached."
- << "Relative gradient max norm: "
- << iteration_summary.gradient_max_norm / initial_gradient_max_norm
- << " <= " << options.gradient_tolerance;
- break;
- }
-
+ iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm);
iteration_summary.cost_change = previous_state.cost - current_state.cost;
- const double absolute_function_tolerance =
- options.function_tolerance * previous_state.cost;
- if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) {
- VLOG(1) << "Terminating. Function tolerance reached. "
- << "|cost_change|/cost: "
- << fabs(iteration_summary.cost_change) / previous_state.cost
- << " <= " << options.function_tolerance;
- summary->termination_type = FUNCTION_TOLERANCE;
- return;
- }
-
iteration_summary.cost = current_state.cost + summary->fixed_cost;
iteration_summary.step_norm = delta.norm();
iteration_summary.step_is_valid = true;
@@ -359,10 +377,32 @@
summary->iterations.push_back(iteration_summary);
++summary->num_successful_steps;
+
+ if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) {
+ summary->message = StringPrintf("Gradient tolerance reached. "
+ "Gradient max norm: %e <= %e",
+ iteration_summary.gradient_max_norm,
+ options.gradient_tolerance);
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
+ break;
+ }
+
+ const double absolute_function_tolerance =
+ options.function_tolerance * previous_state.cost;
+ if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) {
+ summary->message =
+ StringPrintf("Function tolerance reached. "
+ "|cost_change|/cost: %e <= %e",
+ fabs(iteration_summary.cost_change) /
+ previous_state.cost,
+ options.function_tolerance);
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
+ break;
+ }
}
}
} // namespace internal
} // namespace ceres
-
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
diff --git a/internal/ceres/line_search_minimizer.h b/internal/ceres/line_search_minimizer.h
index 59f5c3f..f82f139 100644
--- a/internal/ceres/line_search_minimizer.h
+++ b/internal/ceres/line_search_minimizer.h
@@ -31,8 +31,6 @@
#ifndef CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_
#define CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
-
#include "ceres/minimizer.h"
#include "ceres/solver.h"
#include "ceres/types.h"
@@ -76,5 +74,4 @@
} // namespace internal
} // namespace ceres
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
#endif // CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_
diff --git a/internal/ceres/linear_solver.cc b/internal/ceres/linear_solver.cc
index 08c3ba1..e983e2c 100644
--- a/internal/ceres/linear_solver.cc
+++ b/internal/ceres/linear_solver.cc
@@ -45,6 +45,30 @@
LinearSolver::~LinearSolver() {
}
+LinearSolverType LinearSolver::LinearSolverForZeroEBlocks(
+ LinearSolverType linear_solver_type) {
+ if (!IsSchurType(linear_solver_type)) {
+ return linear_solver_type;
+ }
+
+ if (linear_solver_type == SPARSE_SCHUR) {
+ return SPARSE_NORMAL_CHOLESKY;
+ }
+
+ if (linear_solver_type == DENSE_SCHUR) {
+ // TODO(sameeragarwal): This is probably not a great choice.
+ // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can take
+ // a BlockSparseMatrix as input.
+ return DENSE_QR;
+ }
+
+ if (linear_solver_type == ITERATIVE_SCHUR) {
+ return CGNR;
+ }
+
+ return linear_solver_type;
+}
+
LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) {
switch (options.type) {
case CGNR:
@@ -52,9 +76,6 @@
case SPARSE_NORMAL_CHOLESKY:
#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
- LOG(WARNING) << "SPARSE_NORMAL_CHOLESKY is not available. Please "
- << "build Ceres with SuiteSparse or CXSparse. "
- << "Returning NULL.";
return NULL;
#else
return new SparseNormalCholeskySolver(options);
@@ -62,9 +83,6 @@
case SPARSE_SCHUR:
#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
- LOG(WARNING) << "SPARSE_SCHUR is not available. Please "
- << "build Ceres with SuiteSparse or CXSparse. "
- << "Returning NULL.";
return NULL;
#else
return new SparseSchurComplementSolver(options);
diff --git a/internal/ceres/linear_solver.h b/internal/ceres/linear_solver.h
index 22691b3..58b9044 100644
--- a/internal/ceres/linear_solver.h
+++ b/internal/ceres/linear_solver.h
@@ -50,6 +50,26 @@
namespace ceres {
namespace internal {
+enum LinearSolverTerminationType {
+ // Termination criterion was met.
+ LINEAR_SOLVER_SUCCESS,
+
+ // Solver ran for max_num_iterations and terminated before the
+ // termination tolerance could be satisfied.
+ LINEAR_SOLVER_NO_CONVERGENCE,
+
+ // Solver was terminated due to numerical problems, generally due to
+ // the linear system being poorly conditioned.
+ LINEAR_SOLVER_FAILURE,
+
+ // Solver failed with a fatal error that cannot be recovered from,
+ // e.g. CHOLMOD ran out of memory when computing the symbolic or
+ // numeric factorization or an underlying library was called with
+ // the wrong arguments.
+ LINEAR_SOLVER_FATAL_ERROR
+};
+
+
class LinearOperator;
// Abstract base class for objects that implement algorithms for
@@ -74,9 +94,11 @@
Options()
: type(SPARSE_NORMAL_CHOLESKY),
preconditioner_type(JACOBI),
+ visibility_clustering_type(CANONICAL_VIEWS),
dense_linear_algebra_library_type(EIGEN),
sparse_linear_algebra_library_type(SUITE_SPARSE),
use_postordering(false),
+ dynamic_sparsity(false),
min_num_iterations(1),
max_num_iterations(1),
num_threads(1),
@@ -87,14 +109,14 @@
}
LinearSolverType type;
-
PreconditionerType preconditioner_type;
-
+ VisibilityClusteringType visibility_clustering_type;
DenseLinearAlgebraLibraryType dense_linear_algebra_library_type;
SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type;
// See solver.h for information about this flag.
bool use_postordering;
+ bool dynamic_sparsity;
// Number of internal iterations that the solver uses. This
// parameter only makes sense for iterative solvers like CG.
@@ -243,14 +265,23 @@
Summary()
: residual_norm(0.0),
num_iterations(-1),
- termination_type(FAILURE) {
+ termination_type(LINEAR_SOLVER_FAILURE) {
}
double residual_norm;
int num_iterations;
LinearSolverTerminationType termination_type;
+ string message;
};
+ // If the optimization problem is such that there are no remaining
+ // e-blocks, a Schur type linear solver cannot be used. If the
+ // linear solver is of Schur type, this function implements a policy
+ // to select an alternate nearest linear solver to the one selected
+ // by the user. The input linear_solver_type is returned otherwise.
+ static LinearSolverType LinearSolverForZeroEBlocks(
+ LinearSolverType linear_solver_type);
+
virtual ~LinearSolver();
// Solve Ax = b.
diff --git a/internal/ceres/loss_function.cc b/internal/ceres/loss_function.cc
index b948f28..4ad01e3 100644
--- a/internal/ceres/loss_function.cc
+++ b/internal/ceres/loss_function.cc
@@ -39,8 +39,8 @@
void TrivialLoss::Evaluate(double s, double rho[3]) const {
rho[0] = s;
- rho[1] = 1;
- rho[2] = 0;
+ rho[1] = 1.0;
+ rho[2] = 0.0;
}
void HuberLoss::Evaluate(double s, double rho[3]) const {
@@ -48,32 +48,32 @@
// Outlier region.
// 'r' is always positive.
const double r = sqrt(s);
- rho[0] = 2 * a_ * r - b_;
- rho[1] = a_ / r;
- rho[2] = - rho[1] / (2 * s);
+ rho[0] = 2.0 * a_ * r - b_;
+ rho[1] = std::max(std::numeric_limits<double>::min(), a_ / r);
+ rho[2] = - rho[1] / (2.0 * s);
} else {
// Inlier region.
rho[0] = s;
- rho[1] = 1;
- rho[2] = 0;
+ rho[1] = 1.0;
+ rho[2] = 0.0;
}
}
void SoftLOneLoss::Evaluate(double s, double rho[3]) const {
- const double sum = 1 + s * c_;
+ const double sum = 1.0 + s * c_;
const double tmp = sqrt(sum);
// 'sum' and 'tmp' are always positive, assuming that 's' is.
- rho[0] = 2 * b_ * (tmp - 1);
- rho[1] = 1 / tmp;
- rho[2] = - (c_ * rho[1]) / (2 * sum);
+ rho[0] = 2.0 * b_ * (tmp - 1.0);
+ rho[1] = std::max(std::numeric_limits<double>::min(), 1.0 / tmp);
+ rho[2] = - (c_ * rho[1]) / (2.0 * sum);
}
void CauchyLoss::Evaluate(double s, double rho[3]) const {
- const double sum = 1 + s * c_;
- const double inv = 1 / sum;
+ const double sum = 1.0 + s * c_;
+ const double inv = 1.0 / sum;
// 'sum' and 'inv' are always positive, assuming that 's' is.
rho[0] = b_ * log(sum);
- rho[1] = inv;
+ rho[1] = std::max(std::numeric_limits<double>::min(), inv);
rho[2] = - c_ * (inv * inv);
}
@@ -82,8 +82,8 @@
const double inv = 1 / sum;
// 'sum' and 'inv' are always positive.
rho[0] = a_ * atan2(s, a_);
- rho[1] = inv;
- rho[2] = -2 * s * b_ * (inv * inv);
+ rho[1] = std::max(std::numeric_limits<double>::min(), inv);
+ rho[2] = -2.0 * s * b_ * (inv * inv);
}
TolerantLoss::TolerantLoss(double a, double b)
@@ -108,7 +108,7 @@
} else {
const double e_x = exp(x);
rho[0] = b_ * log(1.0 + e_x) - c_;
- rho[1] = e_x / (1.0 + e_x);
+ rho[1] = std::max(std::numeric_limits<double>::min(), e_x / (1.0 + e_x));
rho[2] = 0.5 / (b_ * (1.0 + cosh(x)));
}
}
diff --git a/internal/ceres/low_rank_inverse_hessian.cc b/internal/ceres/low_rank_inverse_hessian.cc
index 372165f..4816e3c 100644
--- a/internal/ceres/low_rank_inverse_hessian.cc
+++ b/internal/ceres/low_rank_inverse_hessian.cc
@@ -28,6 +28,8 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+#include <list>
+
#include "ceres/internal/eigen.h"
#include "ceres/low_rank_inverse_hessian.h"
#include "glog/logging.h"
@@ -35,6 +37,41 @@
namespace ceres {
namespace internal {
+// The (L)BFGS algorithm explicitly requires that the secant equation:
+//
+// B_{k+1} * s_k = y_k
+//
+// Is satisfied at each iteration, where B_{k+1} is the approximated
+// Hessian at the k+1-th iteration, s_k = (x_{k+1} - x_{k}) and
+// y_k = (grad_{k+1} - grad_{k}). As the approximated Hessian must be
+// positive definite, this is equivalent to the condition:
+//
+// s_k^T * y_k > 0 [s_k^T * B_{k+1} * s_k = s_k^T * y_k > 0]
+//
+// This condition would always be satisfied if the function was strictly
+// convex, alternatively, it is always satisfied provided that a Wolfe line
+// search is used (even if the function is not strictly convex). See [1]
+// (p138) for a proof.
+//
+// Although Ceres will always use a Wolfe line search when using (L)BFGS,
+// practical implementation considerations mean that the line search
+// may return a point that satisfies only the Armijo condition, and thus
+// could violate the Secant equation. As such, we will only use a step
+// to update the Hessian approximation if:
+//
+// s_k^T * y_k > tolerance
+//
+// It is important that tolerance is very small (and >=0), as otherwise we
+// might skip the update too often and fail to capture important curvature
+// information in the Hessian. For example going from 1e-10 -> 1e-14 improves
+// the NIST benchmark score from 43/54 to 53/54.
+//
+// [1] Nocedal J., Wright S., Numerical Optimization, 2nd Ed. Springer, 1999.
+//
+// TODO(alexs.mac): Consider using Damped BFGS update instead of
+// skipping update.
+const double kLBFGSSecantConditionHessianUpdateTolerance = 1e-14;
+
LowRankInverseHessian::LowRankInverseHessian(
int num_parameters,
int max_num_corrections,
@@ -42,7 +79,6 @@
: num_parameters_(num_parameters),
max_num_corrections_(max_num_corrections),
use_approximate_eigenvalue_scaling_(use_approximate_eigenvalue_scaling),
- num_corrections_(0),
approximate_eigenvalue_scale_(1.0),
delta_x_history_(num_parameters, max_num_corrections),
delta_gradient_history_(num_parameters, max_num_corrections),
@@ -52,35 +88,29 @@
bool LowRankInverseHessian::Update(const Vector& delta_x,
const Vector& delta_gradient) {
const double delta_x_dot_delta_gradient = delta_x.dot(delta_gradient);
- if (delta_x_dot_delta_gradient <= 1e-10) {
- VLOG(2) << "Skipping LBFGS Update, delta_x_dot_delta_gradient too small: "
- << delta_x_dot_delta_gradient;
+ if (delta_x_dot_delta_gradient <=
+ kLBFGSSecantConditionHessianUpdateTolerance) {
+ VLOG(2) << "Skipping L-BFGS Update, delta_x_dot_delta_gradient too "
+ << "small: " << delta_x_dot_delta_gradient << ", tolerance: "
+ << kLBFGSSecantConditionHessianUpdateTolerance
+ << " (Secant condition).";
return false;
}
- if (num_corrections_ == max_num_corrections_) {
- // TODO(sameeragarwal): This can be done more efficiently using
- // a circular buffer/indexing scheme, but for simplicity we will
- // do the expensive copy for now.
- delta_x_history_.block(0, 0, num_parameters_, max_num_corrections_ - 1) =
- delta_x_history_
- .block(0, 1, num_parameters_, max_num_corrections_ - 1);
- delta_gradient_history_
- .block(0, 0, num_parameters_, max_num_corrections_ - 1) =
- delta_gradient_history_
- .block(0, 1, num_parameters_, max_num_corrections_ - 1);
-
- delta_x_dot_delta_gradient_.head(num_corrections_ - 1) =
- delta_x_dot_delta_gradient_.tail(num_corrections_ - 1);
- } else {
- ++num_corrections_;
+ int next = indices_.size();
+ // Once the size of the list reaches max_num_corrections_, simulate
+ // a circular buffer by removing the first element of the list and
+ // making it the next position where the LBFGS history is stored.
+ if (next == max_num_corrections_) {
+ next = indices_.front();
+ indices_.pop_front();
}
- delta_x_history_.col(num_corrections_ - 1) = delta_x;
- delta_gradient_history_.col(num_corrections_ - 1) = delta_gradient;
- delta_x_dot_delta_gradient_(num_corrections_ - 1) =
- delta_x_dot_delta_gradient;
+ indices_.push_back(next);
+ delta_x_history_.col(next) = delta_x;
+ delta_gradient_history_.col(next) = delta_gradient;
+ delta_x_dot_delta_gradient_(next) = delta_x_dot_delta_gradient;
approximate_eigenvalue_scale_ =
delta_x_dot_delta_gradient / delta_gradient.squaredNorm();
return true;
@@ -93,12 +123,16 @@
search_direction = gradient;
- Vector alpha(num_corrections_);
+ const int num_corrections = indices_.size();
+ Vector alpha(num_corrections);
- for (int i = num_corrections_ - 1; i >= 0; --i) {
- alpha(i) = delta_x_history_.col(i).dot(search_direction) /
- delta_x_dot_delta_gradient_(i);
- search_direction -= alpha(i) * delta_gradient_history_.col(i);
+ for (std::list<int>::const_reverse_iterator it = indices_.rbegin();
+ it != indices_.rend();
+ ++it) {
+ const double alpha_i = delta_x_history_.col(*it).dot(search_direction) /
+ delta_x_dot_delta_gradient_(*it);
+ search_direction -= alpha_i * delta_gradient_history_.col(*it);
+ alpha(*it) = alpha_i;
}
if (use_approximate_eigenvalue_scaling_) {
@@ -133,12 +167,18 @@
// 20(5), 863-874, 1974.
// [2] Nocedal J., Wright S., Numerical Optimization, Springer, 1999.
search_direction *= approximate_eigenvalue_scale_;
+
+ VLOG(4) << "Applying approximate_eigenvalue_scale: "
+ << approximate_eigenvalue_scale_ << " to initial inverse Hessian "
+ << "approximation.";
}
- for (int i = 0; i < num_corrections_; ++i) {
- const double beta = delta_gradient_history_.col(i).dot(search_direction) /
- delta_x_dot_delta_gradient_(i);
- search_direction += delta_x_history_.col(i) * (alpha(i) - beta);
+ for (std::list<int>::const_iterator it = indices_.begin();
+ it != indices_.end();
+ ++it) {
+ const double beta = delta_gradient_history_.col(*it).dot(search_direction) /
+ delta_x_dot_delta_gradient_(*it);
+ search_direction += delta_x_history_.col(*it) * (alpha(*it) - beta);
}
}
diff --git a/internal/ceres/low_rank_inverse_hessian.h b/internal/ceres/low_rank_inverse_hessian.h
index 7d293d0..19ab760 100644
--- a/internal/ceres/low_rank_inverse_hessian.h
+++ b/internal/ceres/low_rank_inverse_hessian.h
@@ -34,6 +34,8 @@
#ifndef CERES_INTERNAL_LOW_RANK_INVERSE_HESSIAN_H_
#define CERES_INTERNAL_LOW_RANK_INVERSE_HESSIAN_H_
+#include <list>
+
#include "ceres/internal/eigen.h"
#include "ceres/linear_operator.h"
@@ -93,11 +95,11 @@
const int num_parameters_;
const int max_num_corrections_;
const bool use_approximate_eigenvalue_scaling_;
- int num_corrections_;
double approximate_eigenvalue_scale_;
- Matrix delta_x_history_;
- Matrix delta_gradient_history_;
+ ColMajorMatrix delta_x_history_;
+ ColMajorMatrix delta_gradient_history_;
Vector delta_x_dot_delta_gradient_;
+ std::list<int> indices_;
};
} // namespace internal
diff --git a/internal/ceres/miniglog/glog/logging.cc b/internal/ceres/miniglog/glog/logging.cc
new file mode 100644
index 0000000..32a78ce
--- /dev/null
+++ b/internal/ceres/miniglog/glog/logging.cc
@@ -0,0 +1,39 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+
+#include "glog/logging.h"
+
+namespace google {
+
+// This is the set of log sinks. This must be in a separate library to ensure
+// that there is only one instance of this across the entire program.
+std::set<google::LogSink *> log_sinks_global;
+
+} // namespace ceres
diff --git a/internal/ceres/miniglog/glog/logging.h b/internal/ceres/miniglog/glog/logging.h
index bab3191..e9c0dff 100644
--- a/internal/ceres/miniglog/glog/logging.h
+++ b/internal/ceres/miniglog/glog/logging.h
@@ -1,83 +1,114 @@
-// Copyright 2011 Google Inc. All Rights Reserved.
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
// Author: settinger@google.com (Scott Ettinger)
-
-// Simplified Google3 style logging with Android support.
-// Supported macros are : LOG(INFO), LOG(WARNING), LOG(ERROR), LOG(FATAL),
-// and VLOG(n).
+// mierle@gmail.com (Keir Mierle)
//
-// Portions of this code are taken from the GLOG package. This code
-// is only a small subset of the GLOG functionality. And like GLOG,
-// higher levels are more verbose.
+// Simplified Glog style logging with Android support. Supported macros in
+// decreasing severity level per line:
//
-// Notable differences from GLOG :
+// VLOG(2), VLOG(N)
+// VLOG(1),
+// LOG(INFO), VLOG(0), LG
+// LOG(WARNING),
+// LOG(ERROR),
+// LOG(FATAL),
//
-// 1. lack of support for displaying unprintable characters and lack
-// of stack trace information upon failure of the CHECK macros.
-// 2. All output is tagged with the string "native".
-// 3. While there is no runtime flag filtering logs (-v, -vmodule), the
-// compile time define MAX_LOG_LEVEL can be used to silence any
-// logging above the given level.
+// With VLOG(n), the output is directed to one of the 5 Android log levels:
//
-// -------------------------------- Usage ------------------------------------
-// Basic usage :
-// LOG(<severity level>) acts as a c++ stream to the Android logcat output.
-// e.g. LOG(INFO) << "Value of counter = " << counter;
+// 2 - Verbose
+// 1 - Debug
+// 0 - Info
+// -1 - Warning
+// -2 - Error
+// -3 - Fatal
//
-// Valid severity levels include INFO, WARNING, ERROR, FATAL.
-// The various severity levels are routed to the corresponding Android logcat
-// output.
-// LOG(FATAL) outputs to the log and then terminates.
+// Any logging of level 2 and above is directed to the Verbose level. All
+// Android log output is tagged with the string "native".
//
-// VLOG(<severity level>) can also be used.
-// VLOG(n) output is directed to the Android logcat levels as follows :
-// >=2 - Verbose
-// 1 - Debug
-// 0 - Info
-// -1 - Warning
-// -2 - Error
-// <=-3 - Fatal
-// Note that VLOG(FATAL) will terminate the program.
+// If the symbol ANDROID is not defined, all output goes to std::cerr.
+// This allows code to be built on a different system for debug.
//
-// CHECK macros are defined to test for conditions within code. Any CHECK
-// that fails will log the failure and terminate the application.
+// Portions of this code are taken from the GLOG package. This code is only a
+// small subset of the GLOG functionality. Notable differences from GLOG
+// behavior include lack of support for displaying unprintable characters and
+// lack of stack trace information upon failure of the CHECK macros. On
+// non-Android systems, log output goes to std::cerr and is not written to a
+// file.
+//
+// CHECK macros are defined to test for conditions within code. Any CHECK that
+// fails will log the failure and terminate the application.
// e.g. CHECK_GE(3, 2) will pass while CHECK_GE(3, 4) will fail after logging
// "Check failed 3 >= 4".
-// The following CHECK macros are defined :
//
-// CHECK(condition) - fails if condition is false and logs condition.
-// CHECK_NOTNULL(variable) - fails if the variable is NULL.
+// The following CHECK macros are defined:
+//
+// CHECK(condition) - fails if condition is false and logs condition.
+// CHECK_NOTNULL(variable) - fails if the variable is NULL.
//
// The following binary check macros are also defined :
-// Macro operator applied
-// ------------------------------------------
-// CHECK_EQ(val1, val2) val1 == val2
-// CHECK_NE(val1, val2) val1 != val2
-// CHECK_GT(val1, val2) val1 > val2
-// CHECK_GE(val1, val2) val1 >= val2
-// CHECK_LT(val1, val2) val1 < val2
-// CHECK_LE(val1, val2) val1 <= val2
+//
+// Macro Operator equivalent
+// -------------------- -------------------
+// CHECK_EQ(val1, val2) val1 == val2
+// CHECK_NE(val1, val2) val1 != val2
+// CHECK_GT(val1, val2) val1 > val2
+// CHECK_GE(val1, val2) val1 >= val2
+// CHECK_LT(val1, val2) val1 < val2
+// CHECK_LE(val1, val2) val1 <= val2
//
// Debug only versions of all of the check macros are also defined. These
// macros generate no code in a release build, but avoid unused variable
// warnings / errors.
-// To use the debug only versions, Prepend a D to the normal check macros.
-// e.g. DCHECK_EQ(a, b);
+//
+// To use the debug only versions, prepend a D to the normal check macros, e.g.
+// DCHECK_EQ(a, b).
-#ifndef MOBILE_BASE_LOGGING_H_
-#define MOBILE_BASE_LOGGING_H_
+#ifndef CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_
+#define CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_
-// Definitions for building on an Android system.
-#include <android/log.h>
-#include <time.h>
+#ifdef ANDROID
+# include <android/log.h>
+#endif // ANDROID
#include <algorithm>
-#include <iostream>
-#include <string>
+#include <ctime>
#include <fstream>
+#include <iostream>
#include <set>
#include <sstream>
+#include <string>
#include <vector>
+// For appropriate definition of CERES_EXPORT macro.
+#include "ceres/internal/port.h"
+#include "ceres/internal/disable_warnings.h"
+
// Log severity level constants.
const int FATAL = -3;
const int ERROR = -2;
@@ -94,26 +125,29 @@
const int ERROR = ::ERROR;
const int FATAL = ::FATAL;
-#ifdef ENABLE_LOG_SINKS
-
-// Sink class used for integration with mock and test functions.
-// If sinks are added, all log output is also sent to each sink through
-// the send function. In this implementation, WaitTillSent() is called
-// immediately after the send.
+// Sink class used for integration with mock and test functions. If sinks are
+// added, all log output is also sent to each sink through the send function.
+// In this implementation, WaitTillSent() is called immediately after the send.
// This implementation is not thread safe.
-class LogSink {
+class CERES_EXPORT LogSink {
public:
virtual ~LogSink() {}
- virtual void send(LogSeverity severity, const char* full_filename,
- const char* base_filename, int line,
+ virtual void send(LogSeverity severity,
+ const char* full_filename,
+ const char* base_filename,
+ int line,
const struct tm* tm_time,
- const char* message, size_t message_len) = 0;
+ const char* message,
+ size_t message_len) = 0;
virtual void WaitTillSent() = 0;
};
-// Global set of log sinks.
-// TODO(settinger): Move this into a .cc file.
-static std::set<LogSink *> log_sinks_global;
+// Global set of log sinks. The actual object is defined in logging.cc.
+extern CERES_EXPORT std::set<LogSink *> log_sinks_global;
+
+inline void InitGoogleLogging(char *argv) {
+ // Do nothing; this is ignored.
+}
// Note: the Log sink functions are not thread safe.
inline void AddLogSink(LogSink *sink) {
@@ -124,20 +158,17 @@
log_sinks_global.erase(sink);
}
-#endif // #ifdef ENABLE_LOG_SINKS
-
-inline void InitGoogleLogging(char *argv) {}
-
} // namespace google
// ---------------------------- Logger Class --------------------------------
// Class created for each use of the logging macros.
// The logger acts as a stream and routes the final stream contents to the
-// Android logcat output at the proper filter level. This class should not
+// Android logcat output at the proper filter level. If ANDROID is not
+// defined, output is directed to std::cerr. This class should not
// be directly instantiated in code, rather it should be invoked through the
-// use of the log macros LOG, or VLOG.
-class MessageLogger {
+// use of the log macros LG, LOG, or VLOG.
+class CERES_EXPORT MessageLogger {
public:
MessageLogger(const char *file, int line, const char *tag, int severity)
: file_(file), line_(line), tag_(tag), severity_(severity) {
@@ -148,17 +179,14 @@
// Output the contents of the stream to the proper channel on destruction.
~MessageLogger() {
-#ifdef MAX_LOG_LEVEL
- if (severity_ > MAX_LOG_LEVEL && severity_ > FATAL) {
- return;
- }
-#endif
stream_ << "\n";
+
+#ifdef ANDROID
static const int android_log_levels[] = {
ANDROID_LOG_FATAL, // LOG(FATAL)
ANDROID_LOG_ERROR, // LOG(ERROR)
ANDROID_LOG_WARN, // LOG(WARNING)
- ANDROID_LOG_INFO, // LOG(INFO), VLOG(0)
+ ANDROID_LOG_INFO, // LOG(INFO), LG, VLOG(0)
ANDROID_LOG_DEBUG, // VLOG(1)
ANDROID_LOG_VERBOSE, // VLOG(2) .. VLOG(N)
};
@@ -178,14 +206,14 @@
tag_.c_str(),
"terminating.\n");
}
-
-#ifdef ENABLE_LOG_SINKS
+#else
+ // If not building on Android, log all output to std::cerr.
+ std::cerr << stream_.str();
+#endif // ANDROID
LogToSinks(severity_);
WaitForSinks();
-#endif // #ifdef ENABLE_LOG_SINKS
-
// Android logging at level FATAL does not terminate execution, so abort()
// is still required to stop the program.
if (severity_ == FATAL) {
@@ -197,41 +225,49 @@
std::stringstream &stream() { return stream_; }
private:
-#ifdef ENABLE_LOG_SINKS
-
void LogToSinks(int severity) {
time_t rawtime;
- struct tm * timeinfo;
+ time (&rawtime);
- time ( &rawtime );
- timeinfo = localtime ( &rawtime );
- std::set<google::LogSink *>::iterator iter;
+ struct tm* timeinfo;
+#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__)
+ // On Windows, use secure localtime_s not localtime.
+ struct tm windows_timeinfo;
+ timeinfo = &windows_timeinfo;
+ localtime_s(timeinfo, &rawtime);
+#else
+ timeinfo = localtime(&rawtime);
+#endif
+
+ std::set<google::LogSink*>::iterator iter;
// Send the log message to all sinks.
for (iter = google::log_sinks_global.begin();
- iter != google::log_sinks_global.end(); ++iter)
+ iter != google::log_sinks_global.end(); ++iter) {
(*iter)->send(severity, file_.c_str(), filename_only_.c_str(), line_,
timeinfo, stream_.str().c_str(), stream_.str().size());
+ }
}
void WaitForSinks() {
- // TODO(settinger): add locks for thread safety.
+ // TODO(settinger): Add locks for thread safety.
std::set<google::LogSink *>::iterator iter;
+
// Call WaitTillSent() for all sinks.
for (iter = google::log_sinks_global.begin();
- iter != google::log_sinks_global.end(); ++iter)
+ iter != google::log_sinks_global.end(); ++iter) {
(*iter)->WaitTillSent();
+ }
}
-#endif // #ifdef ENABLE_LOG_SINKS
-
void StripBasename(const std::string &full_path, std::string *filename) {
- // TODO(settinger): add support for OS with different path separators.
+ // TODO(settinger): Add support for OSs with different path separators.
const char kSeparator = '/';
size_t pos = full_path.rfind(kSeparator);
- if (pos != std::string::npos)
+ if (pos != std::string::npos) {
*filename = full_path.substr(pos + 1, std::string::npos);
- else
+ } else {
*filename = full_path;
+ }
}
std::string file_;
@@ -247,7 +283,7 @@
// This class is used to explicitly ignore values in the conditional
// logging macros. This avoids compiler warnings like "value computed
// is not used" and "statement has no effect".
-class LoggerVoidify {
+class CERES_EXPORT LoggerVoidify {
public:
LoggerVoidify() { }
// This has to be an operator with a precedence lower than << but
@@ -257,8 +293,8 @@
// Log only if condition is met. Otherwise evaluates to void.
#define LOG_IF(severity, condition) \
- !(condition) ? (void) 0 : LoggerVoidify() & \
- MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream()
+ !(condition) ? (void) 0 : LoggerVoidify() & \
+ MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream()
// Log only if condition is NOT met. Otherwise evaluates to void.
#define LOG_IF_FALSE(severity, condition) LOG_IF(severity, !(condition))
@@ -267,30 +303,31 @@
// google3 code is discouraged and the following shortcut exists for
// backward compatibility with existing code.
#ifdef MAX_LOG_LEVEL
-#define LOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL)
-#define VLOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL)
-#define LG LOG_IF(INFO, INFO <= MAX_LOG_LEVEL)
+# define LOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL)
+# define VLOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL)
+# define LG LOG_IF(INFO, INFO <= MAX_LOG_LEVEL)
+# define VLOG_IF(n, condition) LOG_IF(n, (n <= MAX_LOG_LEVEL) && condition)
#else
-#define LOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream()
-#define VLOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream()
-#define LG MessageLogger((char *)__FILE__, __LINE__, "native", INFO).stream()
+# define LOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream() // NOLINT
+# define VLOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream() // NOLINT
+# define LG MessageLogger((char *)__FILE__, __LINE__, "native", INFO).stream() // NOLINT
+# define VLOG_IF(n, condition) LOG_IF(n, condition)
#endif
// Currently, VLOG is always on for levels below MAX_LOG_LEVEL.
#ifndef MAX_LOG_LEVEL
-#define VLOG_IS_ON(x) (1)
+# define VLOG_IS_ON(x) (1)
#else
-#define VLOG_IS_ON(x) (x <= MAX_LOG_LEVEL)
+# define VLOG_IS_ON(x) (x <= MAX_LOG_LEVEL)
#endif
#ifndef NDEBUG
-#define DLOG LOG
+# define DLOG LOG
#else
-#define DLOG(severity) true ? (void) 0 : LoggerVoidify() & \
- MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream()
+# define DLOG(severity) true ? (void) 0 : LoggerVoidify() & \
+ MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream()
#endif
-// ---------------------------- CHECK helpers --------------------------------
// Log a message and terminate.
template<class T>
@@ -307,19 +344,19 @@
#ifndef NDEBUG
// Debug only version of CHECK
-#define DCHECK(condition) LOG_IF_FALSE(FATAL, condition) \
- << "Check failed: " #condition " "
+# define DCHECK(condition) LOG_IF_FALSE(FATAL, condition) \
+ << "Check failed: " #condition " "
#else
// Optimized version - generates no code.
-#define DCHECK(condition) if (false) LOG_IF_FALSE(FATAL, condition) \
- << "Check failed: " #condition " "
+# define DCHECK(condition) if (false) LOG_IF_FALSE(FATAL, condition) \
+ << "Check failed: " #condition " "
#endif // NDEBUG
// ------------------------- CHECK_OP macros ---------------------------------
// Generic binary operator check macro. This should not be directly invoked,
// instead use the binary comparison macros defined below.
-#define CHECK_OP(val1, val2, op) LOG_IF_FALSE(FATAL, (val1 op val2)) \
+#define CHECK_OP(val1, val2, op) LOG_IF_FALSE(FATAL, ((val1) op (val2))) \
<< "Check failed: " #val1 " " #op " " #val2 " "
// Check_op macro definitions
@@ -332,20 +369,20 @@
#ifndef NDEBUG
// Debug only versions of CHECK_OP macros.
-#define DCHECK_EQ(val1, val2) CHECK_OP(val1, val2, ==)
-#define DCHECK_NE(val1, val2) CHECK_OP(val1, val2, !=)
-#define DCHECK_LE(val1, val2) CHECK_OP(val1, val2, <=)
-#define DCHECK_LT(val1, val2) CHECK_OP(val1, val2, <)
-#define DCHECK_GE(val1, val2) CHECK_OP(val1, val2, >=)
-#define DCHECK_GT(val1, val2) CHECK_OP(val1, val2, >)
+# define DCHECK_EQ(val1, val2) CHECK_OP(val1, val2, ==)
+# define DCHECK_NE(val1, val2) CHECK_OP(val1, val2, !=)
+# define DCHECK_LE(val1, val2) CHECK_OP(val1, val2, <=)
+# define DCHECK_LT(val1, val2) CHECK_OP(val1, val2, <)
+# define DCHECK_GE(val1, val2) CHECK_OP(val1, val2, >=)
+# define DCHECK_GT(val1, val2) CHECK_OP(val1, val2, >)
#else
// These versions generate no code in optimized mode.
-#define DCHECK_EQ(val1, val2) if (false) CHECK_OP(val1, val2, ==)
-#define DCHECK_NE(val1, val2) if (false) CHECK_OP(val1, val2, !=)
-#define DCHECK_LE(val1, val2) if (false) CHECK_OP(val1, val2, <=)
-#define DCHECK_LT(val1, val2) if (false) CHECK_OP(val1, val2, <)
-#define DCHECK_GE(val1, val2) if (false) CHECK_OP(val1, val2, >=)
-#define DCHECK_GT(val1, val2) if (false) CHECK_OP(val1, val2, >)
+# define DCHECK_EQ(val1, val2) if (false) CHECK_OP(val1, val2, ==)
+# define DCHECK_NE(val1, val2) if (false) CHECK_OP(val1, val2, !=)
+# define DCHECK_LE(val1, val2) if (false) CHECK_OP(val1, val2, <=)
+# define DCHECK_LT(val1, val2) if (false) CHECK_OP(val1, val2, <)
+# define DCHECK_GE(val1, val2) if (false) CHECK_OP(val1, val2, >=)
+# define DCHECK_GT(val1, val2) if (false) CHECK_OP(val1, val2, >)
#endif // NDEBUG
// ---------------------------CHECK_NOTNULL macros ---------------------------
@@ -384,8 +421,6 @@
CheckNotNull(__FILE__, __LINE__, "'" #val "' Must be non NULL", (val))
#endif // NDEBUG
-inline void PrintAndroid(const char *msg) {
- __android_log_write(ANDROID_LOG_VERBOSE, "native", msg);
-}
+#include "ceres/internal/reenable_warnings.h"
-#endif // MOBILE_BASE_LOGGING_H_
+#endif // CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_
diff --git a/internal/ceres/minimizer.cc b/internal/ceres/minimizer.cc
index 2e2c15a..6c3b68d 100644
--- a/internal/ceres/minimizer.cc
+++ b/internal/ceres/minimizer.cc
@@ -37,13 +37,14 @@
Minimizer::~Minimizer() {}
-bool Minimizer::RunCallbacks(const vector<IterationCallback*> callbacks,
+bool Minimizer::RunCallbacks(const Minimizer::Options& options,
const IterationSummary& iteration_summary,
Solver::Summary* summary) {
+ const bool is_not_silent = !options.is_silent;
CallbackReturnType status = SOLVER_CONTINUE;
int i = 0;
- while (status == SOLVER_CONTINUE && i < callbacks.size()) {
- status = (*callbacks[i])(iteration_summary);
+ while (status == SOLVER_CONTINUE && i < options.callbacks.size()) {
+ status = (*options.callbacks[i])(iteration_summary);
++i;
}
switch (status) {
@@ -51,11 +52,13 @@
return true;
case SOLVER_TERMINATE_SUCCESSFULLY:
summary->termination_type = USER_SUCCESS;
- VLOG(1) << "Terminating: User callback returned USER_SUCCESS.";
+ summary->message = "User callback returned SOLVER_TERMINATE_SUCCESSFULLY.";
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
return false;
case SOLVER_ABORT:
- summary->termination_type = USER_ABORT;
- VLOG(1) << "Terminating: User callback returned USER_ABORT.";
+ summary->termination_type = USER_FAILURE;
+ summary->message = "User callback returned SOLVER_ABORT.";
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
return false;
default:
LOG(FATAL) << "Unknown type of user callback status";
diff --git a/internal/ceres/minimizer.h b/internal/ceres/minimizer.h
index 622e9ce..f1da3f7 100644
--- a/internal/ceres/minimizer.h
+++ b/internal/ceres/minimizer.h
@@ -107,12 +107,14 @@
options.line_search_sufficient_curvature_decrease;
max_line_search_step_expansion =
options.max_line_search_step_expansion;
+ is_silent = (options.logging_type == SILENT);
evaluator = NULL;
trust_region_strategy = NULL;
jacobian = NULL;
callbacks = options.callbacks;
inner_iteration_minimizer = NULL;
inner_iteration_tolerance = options.inner_iteration_tolerance;
+ is_constrained = false;
}
int max_num_iterations;
@@ -153,6 +155,8 @@
double line_search_sufficient_curvature_decrease;
double max_line_search_step_expansion;
+ // If true, then all logging is disabled.
+ bool is_silent;
// List of callbacks that are executed by the Minimizer at the end
// of each iteration.
@@ -177,9 +181,12 @@
Minimizer* inner_iteration_minimizer;
double inner_iteration_tolerance;
+
+ // Use a bounds constrained optimization algorithm.
+ bool is_constrained;
};
- static bool RunCallbacks(const vector<IterationCallback*> callbacks,
+ static bool RunCallbacks(const Options& options,
const IterationSummary& iteration_summary,
Solver::Summary* summary);
diff --git a/internal/ceres/minimizer_test.cc b/internal/ceres/minimizer_test.cc
index 1058036..0d8b617 100644
--- a/internal/ceres/minimizer_test.cc
+++ b/internal/ceres/minimizer_test.cc
@@ -44,7 +44,7 @@
}
};
-TEST(MinimizerTest, InitializationCopiesCallbacks) {
+TEST(Minimizer, InitializationCopiesCallbacks) {
FakeIterationCallback callback0;
FakeIterationCallback callback1;
@@ -59,5 +59,42 @@
EXPECT_EQ(minimizer_options.callbacks[1], &callback1);
}
+class AbortingIterationCallback : public IterationCallback {
+ public:
+ virtual ~AbortingIterationCallback() {}
+ virtual CallbackReturnType operator()(const IterationSummary& summary) {
+ return SOLVER_ABORT;
+ }
+};
+
+TEST(Minimizer, UserAbortUpdatesSummaryMessage) {
+ AbortingIterationCallback callback;
+ Solver::Options solver_options;
+ solver_options.callbacks.push_back(&callback);
+ Minimizer::Options minimizer_options(solver_options);
+ Solver::Summary summary;
+ Minimizer::RunCallbacks(minimizer_options, IterationSummary(), &summary);
+ EXPECT_EQ(summary.message, "User callback returned SOLVER_ABORT.");
+}
+
+class SucceedingIterationCallback : public IterationCallback {
+ public:
+ virtual ~SucceedingIterationCallback() {}
+ virtual CallbackReturnType operator()(const IterationSummary& summary) {
+ return SOLVER_TERMINATE_SUCCESSFULLY;
+ }
+};
+
+TEST(Minimizer, UserSuccessUpdatesSummaryMessage) {
+ SucceedingIterationCallback callback;
+ Solver::Options solver_options;
+ solver_options.callbacks.push_back(&callback);
+ Minimizer::Options minimizer_options(solver_options);
+ Solver::Summary summary;
+ Minimizer::RunCallbacks(minimizer_options, IterationSummary(), &summary);
+ EXPECT_EQ(summary.message,
+ "User callback returned SOLVER_TERMINATE_SUCCESSFULLY.");
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/mutex.h b/internal/ceres/mutex.h
index 0c48ed3..97e2cd3 100644
--- a/internal/ceres/mutex.h
+++ b/internal/ceres/mutex.h
@@ -95,6 +95,8 @@
#ifndef CERES_INTERNAL_MUTEX_H_
#define CERES_INTERNAL_MUTEX_H_
+#include "ceres/internal/port.h"
+
#if defined(CERES_NO_THREADS)
typedef int MutexType; // to keep a lock-count
#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__)
@@ -112,7 +114,9 @@
// To avoid macro definition of ERROR.
# define NOGDI
// To avoid macro definition of min/max.
-# define NOMINMAX
+# ifndef NOMINMAX
+# define NOMINMAX
+# endif
# include <windows.h>
typedef CRITICAL_SECTION MutexType;
#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK)
diff --git a/internal/ceres/numeric_diff_cost_function_test.cc b/internal/ceres/numeric_diff_cost_function_test.cc
index 3953ded..422c712 100644
--- a/internal/ceres/numeric_diff_cost_function_test.cc
+++ b/internal/ceres/numeric_diff_cost_function_test.cc
@@ -184,5 +184,18 @@
new SizeTestingCostFunction<2,2>, ceres::TAKE_OWNERSHIP));
}
+TEST(NumericDiffCostFunction, EasyCaseFunctorCentralDifferencesAndDynamicNumResiduals) {
+ internal::scoped_ptr<CostFunction> cost_function;
+ cost_function.reset(
+ new NumericDiffCostFunction<EasyFunctor,
+ CENTRAL,
+ ceres::DYNAMIC,
+ 5, /* size of x1 */
+ 5 /* size of x2 */>(
+ new EasyFunctor, TAKE_OWNERSHIP, 3));
+ EasyFunctor functor;
+ functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/ordered_groups_test.cc b/internal/ceres/ordered_groups_test.cc
index 700e788..7719d35 100644
--- a/internal/ceres/ordered_groups_test.cc
+++ b/internal/ceres/ordered_groups_test.cc
@@ -38,7 +38,7 @@
namespace ceres {
namespace internal {
-TEST(OrderedGroup, EmptyOrderedGroupBehavesCorrectly) {
+TEST(OrderedGroups, EmptyOrderedGroupBehavesCorrectly) {
ParameterBlockOrdering ordering;
EXPECT_EQ(ordering.NumGroups(), 0);
EXPECT_EQ(ordering.NumElements(), 0);
@@ -48,7 +48,7 @@
EXPECT_FALSE(ordering.Remove(&x));
}
-TEST(OrderedGroup, EverythingInOneGroup) {
+TEST(OrderedGroups, EverythingInOneGroup) {
ParameterBlockOrdering ordering;
double x[3];
ordering.AddElementToGroup(x, 1);
@@ -75,7 +75,7 @@
EXPECT_EQ(ordering.GroupId(x + 2), 1);
}
-TEST(OrderedGroup, StartInOneGroupAndThenSplit) {
+TEST(OrderedGroups, StartInOneGroupAndThenSplit) {
ParameterBlockOrdering ordering;
double x[3];
ordering.AddElementToGroup(x, 1);
@@ -103,7 +103,7 @@
EXPECT_EQ(ordering.GroupId(x + 2), 1);
}
-TEST(OrderedGroup, AddAndRemoveEveryThingFromOneGroup) {
+TEST(OrderedGroups, AddAndRemoveEveryThingFromOneGroup) {
ParameterBlockOrdering ordering;
double x[3];
ordering.AddElementToGroup(x, 1);
@@ -133,7 +133,7 @@
EXPECT_EQ(ordering.GroupId(x + 2), 5);
}
-TEST(OrderedGroup, ReverseOrdering) {
+TEST(OrderedGroups, ReverseOrdering) {
ParameterBlockOrdering ordering;
double x[3];
ordering.AddElementToGroup(x, 1);
@@ -159,5 +159,61 @@
EXPECT_EQ(ordering.GroupId(x + 2), 2);
}
+TEST(OrderedGroups, BulkRemove) {
+ ParameterBlockOrdering ordering;
+ double x[3];
+ ordering.AddElementToGroup(x, 1);
+ ordering.AddElementToGroup(x + 1, 2);
+ ordering.AddElementToGroup(x + 2, 2);
+
+ vector<double*> elements_to_remove;
+ elements_to_remove.push_back(x);
+ elements_to_remove.push_back(x + 2);
+
+ EXPECT_EQ(ordering.Remove(elements_to_remove), 2);
+ EXPECT_EQ(ordering.NumElements(), 1);
+ EXPECT_EQ(ordering.GroupId(x), -1);
+ EXPECT_EQ(ordering.GroupId(x + 1), 2);
+ EXPECT_EQ(ordering.GroupId(x + 2), -1);
+}
+
+TEST(OrderedGroups, BulkRemoveWithNoElements) {
+ ParameterBlockOrdering ordering;
+
+ double x[3];
+ vector<double*> elements_to_remove;
+ elements_to_remove.push_back(x);
+ elements_to_remove.push_back(x + 2);
+
+ EXPECT_EQ(ordering.Remove(elements_to_remove), 0);
+
+ ordering.AddElementToGroup(x, 1);
+ ordering.AddElementToGroup(x + 1, 2);
+ ordering.AddElementToGroup(x + 2, 2);
+
+ elements_to_remove.clear();
+ EXPECT_EQ(ordering.Remove(elements_to_remove), 0);
+}
+
+TEST(OrderedGroups, MinNonZeroGroup) {
+ ParameterBlockOrdering ordering;
+ double x[3];
+
+ ordering.AddElementToGroup(x, 1);
+ ordering.AddElementToGroup(x + 1, 1);
+ ordering.AddElementToGroup(x + 2, 2);
+
+ EXPECT_EQ(ordering.MinNonZeroGroup(), 1);
+ ordering.Remove(x);
+
+ EXPECT_EQ(ordering.MinNonZeroGroup(), 1);
+ ordering.Remove(x + 1);
+
+ EXPECT_EQ(ordering.MinNonZeroGroup(), 2);
+ ordering.Remove(x + 2);
+
+ // No non-zero groups left.
+ EXPECT_DEATH_IF_SUPPORTED(ordering.MinNonZeroGroup(), "NumGroups()");
+}
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h
index 695fa6f..7bc823d 100644
--- a/internal/ceres/parameter_block.h
+++ b/internal/ceres/parameter_block.h
@@ -31,7 +31,9 @@
#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_
#define CERES_INTERNAL_PARAMETER_BLOCK_H_
+#include <algorithm>
#include <cstdlib>
+#include <limits>
#include <string>
#include "ceres/array_utils.h"
#include "ceres/collections_port.h"
@@ -180,16 +182,59 @@
}
}
+ void SetUpperBound(int index, double upper_bound) {
+ CHECK_LT(index, size_);
+
+ if (upper_bounds_.get() == NULL) {
+ upper_bounds_.reset(new double[size_]);
+ std::fill(upper_bounds_.get(),
+ upper_bounds_.get() + size_,
+ std::numeric_limits<double>::max());
+ }
+
+ upper_bounds_[index] = upper_bound;
+ };
+
+ void SetLowerBound(int index, double lower_bound) {
+ CHECK_LT(index, size_);
+
+ if (lower_bounds_.get() == NULL) {
+ lower_bounds_.reset(new double[size_]);
+ std::fill(lower_bounds_.get(),
+ lower_bounds_.get() + size_,
+ -std::numeric_limits<double>::max());
+ }
+
+ lower_bounds_[index] = lower_bound;
+ }
+
// Generalization of the addition operation. This is the same as
- // LocalParameterization::Plus() but uses the parameter's current state
- // instead of operating on a passed in pointer.
+ // LocalParameterization::Plus() followed by projection onto the
+ // hyper cube implied by the bounds constraints.
bool Plus(const double *x, const double* delta, double* x_plus_delta) {
- if (local_parameterization_ == NULL) {
+ if (local_parameterization_ != NULL) {
+ if (!local_parameterization_->Plus(x, delta, x_plus_delta)) {
+ return false;
+ }
+ } else {
VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) +
ConstVectorRef(delta, size_);
- return true;
}
- return local_parameterization_->Plus(x, delta, x_plus_delta);
+
+ // Project onto the box constraints.
+ if (lower_bounds_.get() != NULL) {
+ for (int i = 0; i < size_; ++i) {
+ x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]);
+ }
+ }
+
+ if (upper_bounds_.get() != NULL) {
+ for (int i = 0; i < size_; ++i) {
+ x_plus_delta[i] = std::min(x_plus_delta[i], upper_bounds_[i]);
+ }
+ }
+
+ return true;
}
string ToString() const {
@@ -234,6 +279,22 @@
return residual_blocks_.get();
}
+ double LowerBoundForParameter(int index) const {
+ if (lower_bounds_.get() == NULL) {
+ return -std::numeric_limits<double>::max();
+ } else {
+ return lower_bounds_[index];
+ }
+ }
+
+ double UpperBoundForParameter(int index) const {
+ if (upper_bounds_.get() == NULL) {
+ return std::numeric_limits<double>::max();
+ } else {
+ return upper_bounds_[index];
+ }
+ }
+
private:
void Init(double* user_state,
int size,
@@ -312,6 +373,20 @@
// If non-null, contains the residual blocks this parameter block is in.
scoped_ptr<ResidualBlockSet> residual_blocks_;
+ // Upper and lower bounds for the parameter block. SetUpperBound
+ // and SetLowerBound lazily initialize the upper_bounds_ and
+ // lower_bounds_ arrays. If they are never called, then memory for
+ // these arrays is never allocated. Thus for problems where there
+ // are no bounds, or only one sided bounds we do not pay the cost of
+ // allocating memory for the inactive bounds constraints.
+ //
+ // Upon initialization these arrays are initialized to
+ // std::numeric_limits<double>::max() and
+ // -std::numeric_limits<double>::max() respectively which correspond
+ // to the parameter block being unconstrained.
+ scoped_array<double> upper_bounds_;
+ scoped_array<double> lower_bounds_;
+
// Necessary so ProblemImpl can clean up the parameterizations.
friend class ProblemImpl;
};
diff --git a/internal/ceres/parameter_block_ordering.cc b/internal/ceres/parameter_block_ordering.cc
index 190715b..3032329 100644
--- a/internal/ceres/parameter_block_ordering.cc
+++ b/internal/ceres/parameter_block_ordering.cc
@@ -144,5 +144,21 @@
return graph;
}
+void OrderingToGroupSizes(const ParameterBlockOrdering* ordering,
+ vector<int>* group_sizes) {
+ CHECK_NOTNULL(group_sizes)->clear();
+ if (ordering == NULL) {
+ return;
+ }
+
+ const map<int, set<double*> >& group_to_elements =
+ ordering->group_to_elements();
+ for (map<int, set<double*> >::const_iterator it = group_to_elements.begin();
+ it != group_to_elements.end();
+ ++it) {
+ group_sizes->push_back(it->second.size());
+ }
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/parameter_block_ordering.h b/internal/ceres/parameter_block_ordering.h
index 4675cb8..5de9951 100644
--- a/internal/ceres/parameter_block_ordering.h
+++ b/internal/ceres/parameter_block_ordering.h
@@ -78,6 +78,11 @@
// parameter blocks, if they co-occur in a residual block.
Graph<ParameterBlock*>* CreateHessianGraph(const Program& program);
+// Iterate over each of the groups in order of their priority and fill
+// summary with their sizes.
+void OrderingToGroupSizes(const ParameterBlockOrdering* ordering,
+ vector<int>* group_sizes);
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/parameter_block_test.cc b/internal/ceres/parameter_block_test.cc
index 09156f8..5a2db3c 100644
--- a/internal/ceres/parameter_block_test.cc
+++ b/internal/ceres/parameter_block_test.cc
@@ -169,5 +169,45 @@
EXPECT_FALSE(parameter_block.SetState(&y));
}
+TEST(ParameterBlock, DefaultBounds) {
+ double x[2];
+ ParameterBlock parameter_block(x, 2, -1, NULL);
+ EXPECT_EQ(parameter_block.UpperBoundForParameter(0),
+ std::numeric_limits<double>::max());
+ EXPECT_EQ(parameter_block.UpperBoundForParameter(1),
+ std::numeric_limits<double>::max());
+ EXPECT_EQ(parameter_block.LowerBoundForParameter(0),
+ -std::numeric_limits<double>::max());
+ EXPECT_EQ(parameter_block.LowerBoundForParameter(1),
+ -std::numeric_limits<double>::max());
+}
+
+TEST(ParameterBlock, SetBounds) {
+ double x[2];
+ ParameterBlock parameter_block(x, 2, -1, NULL);
+ parameter_block.SetLowerBound(0, 1);
+ parameter_block.SetUpperBound(1, 1);
+
+ EXPECT_EQ(parameter_block.LowerBoundForParameter(0), 1.0);
+ EXPECT_EQ(parameter_block.LowerBoundForParameter(1),
+ -std::numeric_limits<double>::max());
+
+ EXPECT_EQ(parameter_block.UpperBoundForParameter(0),
+ std::numeric_limits<double>::max());
+ EXPECT_EQ(parameter_block.UpperBoundForParameter(1), 1.0);
+}
+
+TEST(ParameterBlock, PlusWithBoundsConstraints) {
+ double x[] = {1.0, 0.0};
+ double delta[] = {2.0, -10.0};
+ ParameterBlock parameter_block(x, 2, -1, NULL);
+ parameter_block.SetUpperBound(0, 2.0);
+ parameter_block.SetLowerBound(1, -1.0);
+ double x_plus_delta[2];
+ parameter_block.Plus(x, delta, x_plus_delta);
+ EXPECT_EQ(x_plus_delta[0], 2.0);
+ EXPECT_EQ(x_plus_delta[1], -1.0);
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/partitioned_matrix_view.cc b/internal/ceres/partitioned_matrix_view.cc
index 59eaff8..d745a9b 100644
--- a/internal/ceres/partitioned_matrix_view.cc
+++ b/internal/ceres/partitioned_matrix_view.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2013 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -27,277 +27,153 @@
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Template specialization of PartitionedMatrixView.
+//
+// ========================================
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
+//=========================================
+//
+// This file is generated using generate_partitioned_matrix_view_specializations.py.
+// Editing it manually is not recommended.
-#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10
-
+#include "ceres/linear_solver.h"
#include "ceres/partitioned_matrix_view.h"
-
-#include <algorithm>
-#include <cstring>
-#include <vector>
-#include "ceres/block_sparse_matrix.h"
-#include "ceres/block_structure.h"
#include "ceres/internal/eigen.h"
-#include "ceres/small_blas.h"
-#include "glog/logging.h"
namespace ceres {
namespace internal {
-PartitionedMatrixView::PartitionedMatrixView(
- const BlockSparseMatrix& matrix,
- int num_col_blocks_a)
- : matrix_(matrix),
- num_col_blocks_e_(num_col_blocks_a) {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
- CHECK_NOTNULL(bs);
-
- num_col_blocks_f_ = bs->cols.size() - num_col_blocks_a;
-
- // Compute the number of row blocks in E. The number of row blocks
- // in E maybe less than the number of row blocks in the input matrix
- // as some of the row blocks at the bottom may not have any
- // e_blocks. For a definition of what an e_block is, please see
- // explicit_schur_complement_solver.h
- num_row_blocks_e_ = 0;
- for (int r = 0; r < bs->rows.size(); ++r) {
- const vector<Cell>& cells = bs->rows[r].cells;
- if (cells[0].block_id < num_col_blocks_a) {
- ++num_row_blocks_e_;
- }
+PartitionedMatrixViewBase*
+PartitionedMatrixViewBase::Create(const LinearSolver::Options& options,
+ const BlockSparseMatrix& matrix) {
+#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 2) &&
+ (options.f_block_size == 2)) {
+ return new PartitionedMatrixView<2, 2, 2>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 2) &&
+ (options.f_block_size == 3)) {
+ return new PartitionedMatrixView<2, 2, 3>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 2) &&
+ (options.f_block_size == 4)) {
+ return new PartitionedMatrixView<2, 2, 4>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 2) &&
+ (options.f_block_size == Eigen::Dynamic)) {
+ return new PartitionedMatrixView<2, 2, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 3) &&
+ (options.f_block_size == 3)) {
+ return new PartitionedMatrixView<2, 3, 3>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 3) &&
+ (options.f_block_size == 4)) {
+ return new PartitionedMatrixView<2, 3, 4>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 3) &&
+ (options.f_block_size == 9)) {
+ return new PartitionedMatrixView<2, 3, 9>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 3) &&
+ (options.f_block_size == Eigen::Dynamic)) {
+ return new PartitionedMatrixView<2, 3, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 3)) {
+ return new PartitionedMatrixView<2, 4, 3>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 4)) {
+ return new PartitionedMatrixView<2, 4, 4>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 8)) {
+ return new PartitionedMatrixView<2, 4, 8>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 9)) {
+ return new PartitionedMatrixView<2, 4, 9>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == Eigen::Dynamic)) {
+ return new PartitionedMatrixView<2, 4, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == Eigen::Dynamic) &&
+ (options.f_block_size == Eigen::Dynamic)) {
+ return new PartitionedMatrixView<2, Eigen::Dynamic, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 4) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 2)) {
+ return new PartitionedMatrixView<4, 4, 2>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 4) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 3)) {
+ return new PartitionedMatrixView<4, 4, 3>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 4) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 4)) {
+ return new PartitionedMatrixView<4, 4, 4>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == 4) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == Eigen::Dynamic)) {
+ return new PartitionedMatrixView<4, 4, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
+ }
+ if ((options.row_block_size == Eigen::Dynamic) &&
+ (options.e_block_size == Eigen::Dynamic) &&
+ (options.f_block_size == Eigen::Dynamic)) {
+ return new PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
}
- // Compute the number of columns in E and F.
- num_cols_e_ = 0;
- num_cols_f_ = 0;
-
- for (int c = 0; c < bs->cols.size(); ++c) {
- const Block& block = bs->cols[c];
- if (c < num_col_blocks_a) {
- num_cols_e_ += block.size;
- } else {
- num_cols_f_ += block.size;
- }
- }
-
- CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols());
-}
-
-PartitionedMatrixView::~PartitionedMatrixView() {
-}
-
-// The next four methods don't seem to be particularly cache
-// friendly. This is an artifact of how the BlockStructure of the
-// input matrix is constructed. These methods will benefit from
-// multithreading as well as improved data layout.
-
-void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
-
- // Iterate over the first num_row_blocks_e_ row blocks, and multiply
- // by the first cell in each row block.
- const double* values = matrix_.values();
- for (int r = 0; r < num_row_blocks_e_; ++r) {
- const Cell& cell = bs->rows[r].cells[0];
- const int row_block_pos = bs->rows[r].block.position;
- const int row_block_size = bs->rows[r].block.size;
- const int col_block_id = cell.block_id;
- const int col_block_pos = bs->cols[col_block_id].position;
- const int col_block_size = bs->cols[col_block_id].size;
- MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
- values + cell.position, row_block_size, col_block_size,
- x + col_block_pos,
- y + row_block_pos);
- }
-}
-
-void PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
-
- // Iterate over row blocks, and if the row block is in E, then
- // multiply by all the cells except the first one which is of type
- // E. If the row block is not in E (i.e its in the bottom
- // num_row_blocks - num_row_blocks_e row blocks), then all the cells
- // are of type F and multiply by them all.
- const double* values = matrix_.values();
- for (int r = 0; r < bs->rows.size(); ++r) {
- const int row_block_pos = bs->rows[r].block.position;
- const int row_block_size = bs->rows[r].block.size;
- const vector<Cell>& cells = bs->rows[r].cells;
- for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) {
- const int col_block_id = cells[c].block_id;
- const int col_block_pos = bs->cols[col_block_id].position;
- const int col_block_size = bs->cols[col_block_id].size;
- MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
- values + cells[c].position, row_block_size, col_block_size,
- x + col_block_pos - num_cols_e(),
- y + row_block_pos);
- }
- }
-}
-
-void PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
-
- // Iterate over the first num_row_blocks_e_ row blocks, and multiply
- // by the first cell in each row block.
- const double* values = matrix_.values();
- for (int r = 0; r < num_row_blocks_e_; ++r) {
- const Cell& cell = bs->rows[r].cells[0];
- const int row_block_pos = bs->rows[r].block.position;
- const int row_block_size = bs->rows[r].block.size;
- const int col_block_id = cell.block_id;
- const int col_block_pos = bs->cols[col_block_id].position;
- const int col_block_size = bs->cols[col_block_id].size;
- MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
- values + cell.position, row_block_size, col_block_size,
- x + row_block_pos,
- y + col_block_pos);
- }
-}
-
-void PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
-
- // Iterate over row blocks, and if the row block is in E, then
- // multiply by all the cells except the first one which is of type
- // E. If the row block is not in E (i.e its in the bottom
- // num_row_blocks - num_row_blocks_e row blocks), then all the cells
- // are of type F and multiply by them all.
- const double* values = matrix_.values();
- for (int r = 0; r < bs->rows.size(); ++r) {
- const int row_block_pos = bs->rows[r].block.position;
- const int row_block_size = bs->rows[r].block.size;
- const vector<Cell>& cells = bs->rows[r].cells;
- for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) {
- const int col_block_id = cells[c].block_id;
- const int col_block_pos = bs->cols[col_block_id].position;
- const int col_block_size = bs->cols[col_block_id].size;
- MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
- values + cells[c].position, row_block_size, col_block_size,
- x + row_block_pos,
- y + col_block_pos - num_cols_e());
- }
- }
-}
-
-// Given a range of columns blocks of a matrix m, compute the block
-// structure of the block diagonal of the matrix m(:,
-// start_col_block:end_col_block)'m(:, start_col_block:end_col_block)
-// and return a BlockSparseMatrix with the this block structure. The
-// caller owns the result.
-BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalMatrixLayout(
- int start_col_block, int end_col_block) const {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
- CompressedRowBlockStructure* block_diagonal_structure =
- new CompressedRowBlockStructure;
-
- int block_position = 0;
- int diagonal_cell_position = 0;
-
- // Iterate over the column blocks, creating a new diagonal block for
- // each column block.
- for (int c = start_col_block; c < end_col_block; ++c) {
- const Block& block = bs->cols[c];
- block_diagonal_structure->cols.push_back(Block());
- Block& diagonal_block = block_diagonal_structure->cols.back();
- diagonal_block.size = block.size;
- diagonal_block.position = block_position;
-
- block_diagonal_structure->rows.push_back(CompressedRow());
- CompressedRow& row = block_diagonal_structure->rows.back();
- row.block = diagonal_block;
-
- row.cells.push_back(Cell());
- Cell& cell = row.cells.back();
- cell.block_id = c - start_col_block;
- cell.position = diagonal_cell_position;
-
- block_position += block.size;
- diagonal_cell_position += block.size * block.size;
- }
-
- // Build a BlockSparseMatrix with the just computed block
- // structure.
- return new BlockSparseMatrix(block_diagonal_structure);
-}
-
-BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalEtE() const {
- BlockSparseMatrix* block_diagonal =
- CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_);
- UpdateBlockDiagonalEtE(block_diagonal);
- return block_diagonal;
-}
-
-BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalFtF() const {
- BlockSparseMatrix* block_diagonal =
- CreateBlockDiagonalMatrixLayout(
- num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_);
- UpdateBlockDiagonalFtF(block_diagonal);
- return block_diagonal;
-}
-
-// Similar to the code in RightMultiplyE, except instead of the matrix
-// vector multiply its an outer product.
-//
-// block_diagonal = block_diagonal(E'E)
-void PartitionedMatrixView::UpdateBlockDiagonalEtE(
- BlockSparseMatrix* block_diagonal) const {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
- const CompressedRowBlockStructure* block_diagonal_structure =
- block_diagonal->block_structure();
-
- block_diagonal->SetZero();
- const double* values = matrix_.values();
- for (int r = 0; r < num_row_blocks_e_ ; ++r) {
- const Cell& cell = bs->rows[r].cells[0];
- const int row_block_size = bs->rows[r].block.size;
- const int block_id = cell.block_id;
- const int col_block_size = bs->cols[block_id].size;
- const int cell_position =
- block_diagonal_structure->rows[block_id].cells[0].position;
-
- MatrixTransposeMatrixMultiply
- <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>(
- values + cell.position, row_block_size, col_block_size,
- values + cell.position, row_block_size, col_block_size,
- block_diagonal->mutable_values() + cell_position,
- 0, 0, col_block_size, col_block_size);
- }
-}
-
-// Similar to the code in RightMultiplyF, except instead of the matrix
-// vector multiply its an outer product.
-//
-// block_diagonal = block_diagonal(F'F)
-//
-void PartitionedMatrixView::UpdateBlockDiagonalFtF(
- BlockSparseMatrix* block_diagonal) const {
- const CompressedRowBlockStructure* bs = matrix_.block_structure();
- const CompressedRowBlockStructure* block_diagonal_structure =
- block_diagonal->block_structure();
-
- block_diagonal->SetZero();
- const double* values = matrix_.values();
- for (int r = 0; r < bs->rows.size(); ++r) {
- const int row_block_size = bs->rows[r].block.size;
- const vector<Cell>& cells = bs->rows[r].cells;
- for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) {
- const int col_block_id = cells[c].block_id;
- const int col_block_size = bs->cols[col_block_id].size;
- const int diagonal_block_id = col_block_id - num_col_blocks_e_;
- const int cell_position =
- block_diagonal_structure->rows[diagonal_block_id].cells[0].position;
-
- MatrixTransposeMatrixMultiply
- <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>(
- values + cells[c].position, row_block_size, col_block_size,
- values + cells[c].position, row_block_size, col_block_size,
- block_diagonal->mutable_values() + cell_position,
- 0, 0, col_block_size, col_block_size);
- }
- }
-}
+#endif
+ VLOG(1) << "Template specializations not found for <"
+ << options.row_block_size << ","
+ << options.e_block_size << ","
+ << options.f_block_size << ">";
+ return new PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(
+ matrix, options.elimination_groups[0]);
+};
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/partitioned_matrix_view.h b/internal/ceres/partitioned_matrix_view.h
index ebfbe40..661252d 100644
--- a/internal/ceres/partitioned_matrix_view.h
+++ b/internal/ceres/partitioned_matrix_view.h
@@ -36,7 +36,15 @@
#ifndef CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_
#define CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_
-#include "ceres/block_sparse_matrix.h"
+#include <algorithm>
+#include <cstring>
+#include <vector>
+
+#include "ceres/block_structure.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/linear_solver.h"
+#include "ceres/small_blas.h"
+#include "glog/logging.h"
namespace ceres {
namespace internal {
@@ -51,57 +59,80 @@
// block structure of the matrix does not satisfy the requirements of
// the Schur complement solver it will result in unpredictable and
// wrong output.
-//
-// This class lives in the internal name space as its a utility class
-// to be used by the IterativeSchurComplementSolver class, found in
-// iterative_schur_complement_solver.h, and is not meant for general
-// consumption.
-class PartitionedMatrixView {
+class PartitionedMatrixViewBase {
public:
- // matrix = [E F], where the matrix E contains the first
- // num_col_blocks_a column blocks.
- PartitionedMatrixView(const BlockSparseMatrix& matrix,
- int num_col_blocks_a);
- ~PartitionedMatrixView();
+ virtual ~PartitionedMatrixViewBase() {}
// y += E'x
- void LeftMultiplyE(const double* x, double* y) const;
+ virtual void LeftMultiplyE(const double* x, double* y) const = 0;
// y += F'x
- void LeftMultiplyF(const double* x, double* y) const;
+ virtual void LeftMultiplyF(const double* x, double* y) const = 0;
// y += Ex
- void RightMultiplyE(const double* x, double* y) const;
+ virtual void RightMultiplyE(const double* x, double* y) const = 0;
// y += Fx
- void RightMultiplyF(const double* x, double* y) const;
+ virtual void RightMultiplyF(const double* x, double* y) const = 0;
// Create and return the block diagonal of the matrix E'E.
- BlockSparseMatrix* CreateBlockDiagonalEtE() const;
+ virtual BlockSparseMatrix* CreateBlockDiagonalEtE() const = 0;
- // Create and return the block diagonal of the matrix F'F.
- BlockSparseMatrix* CreateBlockDiagonalFtF() const;
+ // Create and return the block diagonal of the matrix F'F. Caller
+ // owns the result.
+ virtual BlockSparseMatrix* CreateBlockDiagonalFtF() const = 0;
// Compute the block diagonal of the matrix E'E and store it in
// block_diagonal. The matrix block_diagonal is expected to have a
// BlockStructure (preferably created using
// CreateBlockDiagonalMatrixEtE) which is has the same structure as
// the block diagonal of E'E.
- void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const;
+ virtual void UpdateBlockDiagonalEtE(
+ BlockSparseMatrix* block_diagonal) const = 0;
// Compute the block diagonal of the matrix F'F and store it in
// block_diagonal. The matrix block_diagonal is expected to have a
// BlockStructure (preferably created using
// CreateBlockDiagonalMatrixFtF) which is has the same structure as
// the block diagonal of F'F.
- void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const;
+ virtual void UpdateBlockDiagonalFtF(
+ BlockSparseMatrix* block_diagonal) const = 0;
- int num_col_blocks_e() const { return num_col_blocks_e_; }
- int num_col_blocks_f() const { return num_col_blocks_f_; }
- int num_cols_e() const { return num_cols_e_; }
- int num_cols_f() const { return num_cols_f_; }
- int num_rows() const { return matrix_.num_rows(); }
- int num_cols() const { return matrix_.num_cols(); }
+ virtual int num_col_blocks_e() const = 0;
+ virtual int num_col_blocks_f() const = 0;
+ virtual int num_cols_e() const = 0;
+ virtual int num_cols_f() const = 0;
+ virtual int num_rows() const = 0;
+ virtual int num_cols() const = 0;
+
+ static PartitionedMatrixViewBase* Create(const LinearSolver::Options& options,
+ const BlockSparseMatrix& matrix);
+};
+
+template <int kRowBlockSize = Eigen::Dynamic,
+ int kEBlockSize = Eigen::Dynamic,
+ int kFBlockSize = Eigen::Dynamic >
+class PartitionedMatrixView : public PartitionedMatrixViewBase {
+ public:
+ // matrix = [E F], where the matrix E contains the first
+ // num_col_blocks_a column blocks.
+ PartitionedMatrixView(const BlockSparseMatrix& matrix, int num_col_blocks_e);
+
+ virtual ~PartitionedMatrixView();
+ virtual void LeftMultiplyE(const double* x, double* y) const;
+ virtual void LeftMultiplyF(const double* x, double* y) const;
+ virtual void RightMultiplyE(const double* x, double* y) const;
+ virtual void RightMultiplyF(const double* x, double* y) const;
+ virtual BlockSparseMatrix* CreateBlockDiagonalEtE() const;
+ virtual BlockSparseMatrix* CreateBlockDiagonalFtF() const;
+ virtual void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const;
+ virtual void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const;
+ virtual int num_col_blocks_e() const { return num_col_blocks_e_; }
+ virtual int num_col_blocks_f() const { return num_col_blocks_f_; }
+ virtual int num_cols_e() const { return num_cols_e_; }
+ virtual int num_cols_f() const { return num_cols_f_; }
+ virtual int num_rows() const { return matrix_.num_rows(); }
+ virtual int num_cols() const { return matrix_.num_cols(); }
private:
BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block,
diff --git a/internal/ceres/partitioned_matrix_view_impl.h b/internal/ceres/partitioned_matrix_view_impl.h
new file mode 100644
index 0000000..ae7f776
--- /dev/null
+++ b/internal/ceres/partitioned_matrix_view_impl.h
@@ -0,0 +1,380 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/partitioned_matrix_view.h"
+
+#include <algorithm>
+#include <cstring>
+#include <vector>
+#include "ceres/block_sparse_matrix.h"
+#include "ceres/block_structure.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/small_blas.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+PartitionedMatrixView(
+ const BlockSparseMatrix& matrix,
+ int num_col_blocks_e)
+ : matrix_(matrix),
+ num_col_blocks_e_(num_col_blocks_e) {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+ CHECK_NOTNULL(bs);
+
+ num_col_blocks_f_ = bs->cols.size() - num_col_blocks_e_;
+
+ // Compute the number of row blocks in E. The number of row blocks
+ // in E maybe less than the number of row blocks in the input matrix
+ // as some of the row blocks at the bottom may not have any
+ // e_blocks. For a definition of what an e_block is, please see
+ // explicit_schur_complement_solver.h
+ num_row_blocks_e_ = 0;
+ for (int r = 0; r < bs->rows.size(); ++r) {
+ const vector<Cell>& cells = bs->rows[r].cells;
+ if (cells[0].block_id < num_col_blocks_e_) {
+ ++num_row_blocks_e_;
+ }
+ }
+
+ // Compute the number of columns in E and F.
+ num_cols_e_ = 0;
+ num_cols_f_ = 0;
+
+ for (int c = 0; c < bs->cols.size(); ++c) {
+ const Block& block = bs->cols[c];
+ if (c < num_col_blocks_e_) {
+ num_cols_e_ += block.size;
+ } else {
+ num_cols_f_ += block.size;
+ }
+ }
+
+ CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols());
+}
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+~PartitionedMatrixView() {
+}
+
+// The next four methods don't seem to be particularly cache
+// friendly. This is an artifact of how the BlockStructure of the
+// input matrix is constructed. These methods will benefit from
+// multithreading as well as improved data layout.
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+void
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+RightMultiplyE(const double* x, double* y) const {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+
+ // Iterate over the first num_row_blocks_e_ row blocks, and multiply
+ // by the first cell in each row block.
+ const double* values = matrix_.values();
+ for (int r = 0; r < num_row_blocks_e_; ++r) {
+ const Cell& cell = bs->rows[r].cells[0];
+ const int row_block_pos = bs->rows[r].block.position;
+ const int row_block_size = bs->rows[r].block.size;
+ const int col_block_id = cell.block_id;
+ const int col_block_pos = bs->cols[col_block_id].position;
+ const int col_block_size = bs->cols[col_block_id].size;
+ MatrixVectorMultiply<kRowBlockSize, kEBlockSize, 1>(
+ values + cell.position, row_block_size, col_block_size,
+ x + col_block_pos,
+ y + row_block_pos);
+ }
+}
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+void
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+RightMultiplyF(const double* x, double* y) const {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+
+ // Iterate over row blocks, and if the row block is in E, then
+ // multiply by all the cells except the first one which is of type
+ // E. If the row block is not in E (i.e its in the bottom
+ // num_row_blocks - num_row_blocks_e row blocks), then all the cells
+ // are of type F and multiply by them all.
+ const double* values = matrix_.values();
+ for (int r = 0; r < num_row_blocks_e_; ++r) {
+ const int row_block_pos = bs->rows[r].block.position;
+ const int row_block_size = bs->rows[r].block.size;
+ const vector<Cell>& cells = bs->rows[r].cells;
+ for (int c = 1; c < cells.size(); ++c) {
+ const int col_block_id = cells[c].block_id;
+ const int col_block_pos = bs->cols[col_block_id].position;
+ const int col_block_size = bs->cols[col_block_id].size;
+ MatrixVectorMultiply<kRowBlockSize, kFBlockSize, 1>(
+ values + cells[c].position, row_block_size, col_block_size,
+ x + col_block_pos - num_cols_e_,
+ y + row_block_pos);
+ }
+ }
+
+ for (int r = num_row_blocks_e_; r < bs->rows.size(); ++r) {
+ const int row_block_pos = bs->rows[r].block.position;
+ const int row_block_size = bs->rows[r].block.size;
+ const vector<Cell>& cells = bs->rows[r].cells;
+ for (int c = 0; c < cells.size(); ++c) {
+ const int col_block_id = cells[c].block_id;
+ const int col_block_pos = bs->cols[col_block_id].position;
+ const int col_block_size = bs->cols[col_block_id].size;
+ MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
+ values + cells[c].position, row_block_size, col_block_size,
+ x + col_block_pos - num_cols_e_,
+ y + row_block_pos);
+ }
+ }
+}
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+void
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+LeftMultiplyE(const double* x, double* y) const {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+
+ // Iterate over the first num_row_blocks_e_ row blocks, and multiply
+ // by the first cell in each row block.
+ const double* values = matrix_.values();
+ for (int r = 0; r < num_row_blocks_e_; ++r) {
+ const Cell& cell = bs->rows[r].cells[0];
+ const int row_block_pos = bs->rows[r].block.position;
+ const int row_block_size = bs->rows[r].block.size;
+ const int col_block_id = cell.block_id;
+ const int col_block_pos = bs->cols[col_block_id].position;
+ const int col_block_size = bs->cols[col_block_id].size;
+ MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>(
+ values + cell.position, row_block_size, col_block_size,
+ x + row_block_pos,
+ y + col_block_pos);
+ }
+}
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+void
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+LeftMultiplyF(const double* x, double* y) const {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+
+ // Iterate over row blocks, and if the row block is in E, then
+ // multiply by all the cells except the first one which is of type
+ // E. If the row block is not in E (i.e its in the bottom
+ // num_row_blocks - num_row_blocks_e row blocks), then all the cells
+ // are of type F and multiply by them all.
+ const double* values = matrix_.values();
+ for (int r = 0; r < num_row_blocks_e_; ++r) {
+ const int row_block_pos = bs->rows[r].block.position;
+ const int row_block_size = bs->rows[r].block.size;
+ const vector<Cell>& cells = bs->rows[r].cells;
+ for (int c = 1; c < cells.size(); ++c) {
+ const int col_block_id = cells[c].block_id;
+ const int col_block_pos = bs->cols[col_block_id].position;
+ const int col_block_size = bs->cols[col_block_id].size;
+ MatrixTransposeVectorMultiply<kRowBlockSize, kFBlockSize, 1>(
+ values + cells[c].position, row_block_size, col_block_size,
+ x + row_block_pos,
+ y + col_block_pos - num_cols_e_);
+ }
+ }
+
+ for (int r = num_row_blocks_e_; r < bs->rows.size(); ++r) {
+ const int row_block_pos = bs->rows[r].block.position;
+ const int row_block_size = bs->rows[r].block.size;
+ const vector<Cell>& cells = bs->rows[r].cells;
+ for (int c = 0; c < cells.size(); ++c) {
+ const int col_block_id = cells[c].block_id;
+ const int col_block_pos = bs->cols[col_block_id].position;
+ const int col_block_size = bs->cols[col_block_id].size;
+ MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
+ values + cells[c].position, row_block_size, col_block_size,
+ x + row_block_pos,
+ y + col_block_pos - num_cols_e_);
+ }
+ }
+}
+
+// Given a range of columns blocks of a matrix m, compute the block
+// structure of the block diagonal of the matrix m(:,
+// start_col_block:end_col_block)'m(:, start_col_block:end_col_block)
+// and return a BlockSparseMatrix with the this block structure. The
+// caller owns the result.
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+BlockSparseMatrix*
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+CreateBlockDiagonalMatrixLayout(int start_col_block, int end_col_block) const {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+ CompressedRowBlockStructure* block_diagonal_structure =
+ new CompressedRowBlockStructure;
+
+ int block_position = 0;
+ int diagonal_cell_position = 0;
+
+ // Iterate over the column blocks, creating a new diagonal block for
+ // each column block.
+ for (int c = start_col_block; c < end_col_block; ++c) {
+ const Block& block = bs->cols[c];
+ block_diagonal_structure->cols.push_back(Block());
+ Block& diagonal_block = block_diagonal_structure->cols.back();
+ diagonal_block.size = block.size;
+ diagonal_block.position = block_position;
+
+ block_diagonal_structure->rows.push_back(CompressedRow());
+ CompressedRow& row = block_diagonal_structure->rows.back();
+ row.block = diagonal_block;
+
+ row.cells.push_back(Cell());
+ Cell& cell = row.cells.back();
+ cell.block_id = c - start_col_block;
+ cell.position = diagonal_cell_position;
+
+ block_position += block.size;
+ diagonal_cell_position += block.size * block.size;
+ }
+
+ // Build a BlockSparseMatrix with the just computed block
+ // structure.
+ return new BlockSparseMatrix(block_diagonal_structure);
+}
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+BlockSparseMatrix*
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+CreateBlockDiagonalEtE() const {
+ BlockSparseMatrix* block_diagonal =
+ CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_);
+ UpdateBlockDiagonalEtE(block_diagonal);
+ return block_diagonal;
+}
+
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+BlockSparseMatrix*
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+CreateBlockDiagonalFtF() const {
+ BlockSparseMatrix* block_diagonal =
+ CreateBlockDiagonalMatrixLayout(
+ num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_);
+ UpdateBlockDiagonalFtF(block_diagonal);
+ return block_diagonal;
+}
+
+// Similar to the code in RightMultiplyE, except instead of the matrix
+// vector multiply its an outer product.
+//
+// block_diagonal = block_diagonal(E'E)
+//
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+void
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+UpdateBlockDiagonalEtE(
+ BlockSparseMatrix* block_diagonal) const {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+ const CompressedRowBlockStructure* block_diagonal_structure =
+ block_diagonal->block_structure();
+
+ block_diagonal->SetZero();
+ const double* values = matrix_.values();
+ for (int r = 0; r < num_row_blocks_e_ ; ++r) {
+ const Cell& cell = bs->rows[r].cells[0];
+ const int row_block_size = bs->rows[r].block.size;
+ const int block_id = cell.block_id;
+ const int col_block_size = bs->cols[block_id].size;
+ const int cell_position =
+ block_diagonal_structure->rows[block_id].cells[0].position;
+
+ MatrixTransposeMatrixMultiply
+ <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>(
+ values + cell.position, row_block_size, col_block_size,
+ values + cell.position, row_block_size, col_block_size,
+ block_diagonal->mutable_values() + cell_position,
+ 0, 0, col_block_size, col_block_size);
+ }
+}
+
+// Similar to the code in RightMultiplyF, except instead of the matrix
+// vector multiply its an outer product.
+//
+// block_diagonal = block_diagonal(F'F)
+//
+template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
+void
+PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>::
+UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const {
+ const CompressedRowBlockStructure* bs = matrix_.block_structure();
+ const CompressedRowBlockStructure* block_diagonal_structure =
+ block_diagonal->block_structure();
+
+ block_diagonal->SetZero();
+ const double* values = matrix_.values();
+ for (int r = 0; r < num_row_blocks_e_; ++r) {
+ const int row_block_size = bs->rows[r].block.size;
+ const vector<Cell>& cells = bs->rows[r].cells;
+ for (int c = 1; c < cells.size(); ++c) {
+ const int col_block_id = cells[c].block_id;
+ const int col_block_size = bs->cols[col_block_id].size;
+ const int diagonal_block_id = col_block_id - num_col_blocks_e_;
+ const int cell_position =
+ block_diagonal_structure->rows[diagonal_block_id].cells[0].position;
+
+ MatrixTransposeMatrixMultiply
+ <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>(
+ values + cells[c].position, row_block_size, col_block_size,
+ values + cells[c].position, row_block_size, col_block_size,
+ block_diagonal->mutable_values() + cell_position,
+ 0, 0, col_block_size, col_block_size);
+ }
+ }
+
+ for (int r = num_row_blocks_e_; r < bs->rows.size(); ++r) {
+ const int row_block_size = bs->rows[r].block.size;
+ const vector<Cell>& cells = bs->rows[r].cells;
+ for (int c = 0; c < cells.size(); ++c) {
+ const int col_block_id = cells[c].block_id;
+ const int col_block_size = bs->cols[col_block_id].size;
+ const int diagonal_block_id = col_block_id - num_col_blocks_e_;
+ const int cell_position =
+ block_diagonal_structure->rows[diagonal_block_id].cells[0].position;
+
+ MatrixTransposeMatrixMultiply
+ <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>(
+ values + cells[c].position, row_block_size, col_block_size,
+ values + cells[c].position, row_block_size, col_block_size,
+ block_diagonal->mutable_values() + cell_position,
+ 0, 0, col_block_size, col_block_size);
+ }
+ }
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/partitioned_matrix_view_test.cc b/internal/ceres/partitioned_matrix_view_test.cc
index 48f7d24..ef5dae9 100644
--- a/internal/ceres/partitioned_matrix_view_test.cc
+++ b/internal/ceres/partitioned_matrix_view_test.cc
@@ -49,6 +49,7 @@
class PartitionedMatrixViewTest : public ::testing::Test {
protected :
virtual void SetUp() {
+ srand(5);
scoped_ptr<LinearLeastSquaresProblem> problem(
CreateLinearLeastSquaresProblemFromId(2));
CHECK_NOTNULL(problem.get());
@@ -57,108 +58,93 @@
num_cols_ = A_->num_cols();
num_rows_ = A_->num_rows();
num_eliminate_blocks_ = problem->num_eliminate_blocks;
+ LinearSolver::Options options;
+ options.elimination_groups.push_back(num_eliminate_blocks_);
+ pmv_.reset(PartitionedMatrixViewBase::Create(
+ options,
+ *down_cast<BlockSparseMatrix*>(A_.get())));
}
int num_rows_;
int num_cols_;
int num_eliminate_blocks_;
-
scoped_ptr<SparseMatrix> A_;
+ scoped_ptr<PartitionedMatrixViewBase> pmv_;
};
TEST_F(PartitionedMatrixViewTest, DimensionsTest) {
- PartitionedMatrixView m(*down_cast<BlockSparseMatrix*>(A_.get()),
- num_eliminate_blocks_);
- EXPECT_EQ(m.num_col_blocks_e(), num_eliminate_blocks_);
- EXPECT_EQ(m.num_col_blocks_f(), num_cols_ - num_eliminate_blocks_);
- EXPECT_EQ(m.num_cols_e(), num_eliminate_blocks_);
- EXPECT_EQ(m.num_cols_f(), num_cols_ - num_eliminate_blocks_);
- EXPECT_EQ(m.num_cols(), A_->num_cols());
- EXPECT_EQ(m.num_rows(), A_->num_rows());
+ EXPECT_EQ(pmv_->num_col_blocks_e(), num_eliminate_blocks_);
+ EXPECT_EQ(pmv_->num_col_blocks_f(), num_cols_ - num_eliminate_blocks_);
+ EXPECT_EQ(pmv_->num_cols_e(), num_eliminate_blocks_);
+ EXPECT_EQ(pmv_->num_cols_f(), num_cols_ - num_eliminate_blocks_);
+ EXPECT_EQ(pmv_->num_cols(), A_->num_cols());
+ EXPECT_EQ(pmv_->num_rows(), A_->num_rows());
}
TEST_F(PartitionedMatrixViewTest, RightMultiplyE) {
- PartitionedMatrixView m(*down_cast<BlockSparseMatrix*>(A_.get()),
- num_eliminate_blocks_);
-
- srand(5);
-
- Vector x1(m.num_cols_e());
- Vector x2(m.num_cols());
+ Vector x1(pmv_->num_cols_e());
+ Vector x2(pmv_->num_cols());
x2.setZero();
- for (int i = 0; i < m.num_cols_e(); ++i) {
+ for (int i = 0; i < pmv_->num_cols_e(); ++i) {
x1(i) = x2(i) = RandDouble();
}
- Vector y1 = Vector::Zero(m.num_rows());
- m.RightMultiplyE(x1.data(), y1.data());
+ Vector y1 = Vector::Zero(pmv_->num_rows());
+ pmv_->RightMultiplyE(x1.data(), y1.data());
- Vector y2 = Vector::Zero(m.num_rows());
+ Vector y2 = Vector::Zero(pmv_->num_rows());
A_->RightMultiply(x2.data(), y2.data());
- for (int i = 0; i < m.num_rows(); ++i) {
+ for (int i = 0; i < pmv_->num_rows(); ++i) {
EXPECT_NEAR(y1(i), y2(i), kEpsilon);
}
}
TEST_F(PartitionedMatrixViewTest, RightMultiplyF) {
- PartitionedMatrixView m(*down_cast<BlockSparseMatrix*>(A_.get()),
- num_eliminate_blocks_);
+ Vector x1(pmv_->num_cols_f());
+ Vector x2 = Vector::Zero(pmv_->num_cols());
- srand(5);
-
- Vector x1(m.num_cols_f());
- Vector x2 = Vector::Zero(m.num_cols());
-
- for (int i = 0; i < m.num_cols_f(); ++i) {
+ for (int i = 0; i < pmv_->num_cols_f(); ++i) {
x1(i) = RandDouble();
- x2(i + m.num_cols_e()) = x1(i);
+ x2(i + pmv_->num_cols_e()) = x1(i);
}
- Vector y1 = Vector::Zero(m.num_rows());
- m.RightMultiplyF(x1.data(), y1.data());
+ Vector y1 = Vector::Zero(pmv_->num_rows());
+ pmv_->RightMultiplyF(x1.data(), y1.data());
- Vector y2 = Vector::Zero(m.num_rows());
+ Vector y2 = Vector::Zero(pmv_->num_rows());
A_->RightMultiply(x2.data(), y2.data());
- for (int i = 0; i < m.num_rows(); ++i) {
+ for (int i = 0; i < pmv_->num_rows(); ++i) {
EXPECT_NEAR(y1(i), y2(i), kEpsilon);
}
}
TEST_F(PartitionedMatrixViewTest, LeftMultiply) {
- PartitionedMatrixView m(*down_cast<BlockSparseMatrix*>(A_.get()),
- num_eliminate_blocks_);
-
- srand(5);
-
- Vector x = Vector::Zero(m.num_rows());
- for (int i = 0; i < m.num_rows(); ++i) {
+ Vector x = Vector::Zero(pmv_->num_rows());
+ for (int i = 0; i < pmv_->num_rows(); ++i) {
x(i) = RandDouble();
}
- Vector y = Vector::Zero(m.num_cols());
- Vector y1 = Vector::Zero(m.num_cols_e());
- Vector y2 = Vector::Zero(m.num_cols_f());
+ Vector y = Vector::Zero(pmv_->num_cols());
+ Vector y1 = Vector::Zero(pmv_->num_cols_e());
+ Vector y2 = Vector::Zero(pmv_->num_cols_f());
A_->LeftMultiply(x.data(), y.data());
- m.LeftMultiplyE(x.data(), y1.data());
- m.LeftMultiplyF(x.data(), y2.data());
+ pmv_->LeftMultiplyE(x.data(), y1.data());
+ pmv_->LeftMultiplyF(x.data(), y2.data());
- for (int i = 0; i < m.num_cols(); ++i) {
+ for (int i = 0; i < pmv_->num_cols(); ++i) {
EXPECT_NEAR(y(i),
- (i < m.num_cols_e()) ? y1(i) : y2(i - m.num_cols_e()),
+ (i < pmv_->num_cols_e()) ? y1(i) : y2(i - pmv_->num_cols_e()),
kEpsilon);
}
}
TEST_F(PartitionedMatrixViewTest, BlockDiagonalEtE) {
- PartitionedMatrixView m(*down_cast<BlockSparseMatrix*>(A_.get()),
- num_eliminate_blocks_);
-
scoped_ptr<BlockSparseMatrix>
- block_diagonal_ee(m.CreateBlockDiagonalEtE());
+ block_diagonal_ee(pmv_->CreateBlockDiagonalEtE());
const CompressedRowBlockStructure* bs = block_diagonal_ee->block_structure();
EXPECT_EQ(block_diagonal_ee->num_rows(), 2);
@@ -171,11 +157,8 @@
}
TEST_F(PartitionedMatrixViewTest, BlockDiagonalFtF) {
- PartitionedMatrixView m(*down_cast<BlockSparseMatrix*>(A_.get()),
- num_eliminate_blocks_);
-
scoped_ptr<BlockSparseMatrix>
- block_diagonal_ff(m.CreateBlockDiagonalFtF());
+ block_diagonal_ff(pmv_->CreateBlockDiagonalFtF());
const CompressedRowBlockStructure* bs = block_diagonal_ff->block_structure();
EXPECT_EQ(block_diagonal_ff->num_rows(), 3);
diff --git a/internal/ceres/polynomial.cc b/internal/ceres/polynomial.cc
index 3238b89..75f43de 100644
--- a/internal/ceres/polynomial.cc
+++ b/internal/ceres/polynomial.cc
@@ -37,6 +37,7 @@
#include "Eigen/Dense"
#include "ceres/internal/port.h"
+#include "ceres/stringprintf.h"
#include "glog/logging.h"
namespace ceres {
@@ -119,6 +120,63 @@
}
return polynomial_in.tail(polynomial_in.size() - i);
}
+
+void FindLinearPolynomialRoots(const Vector& polynomial,
+ Vector* real,
+ Vector* imaginary) {
+ CHECK_EQ(polynomial.size(), 2);
+ if (real != NULL) {
+ real->resize(1);
+ (*real)(0) = -polynomial(1) / polynomial(0);
+ }
+
+ if (imaginary != NULL) {
+ imaginary->setZero(1);
+ }
+}
+
+void FindQuadraticPolynomialRoots(const Vector& polynomial,
+ Vector* real,
+ Vector* imaginary) {
+ CHECK_EQ(polynomial.size(), 3);
+ const double a = polynomial(0);
+ const double b = polynomial(1);
+ const double c = polynomial(2);
+ const double D = b * b - 4 * a * c;
+ const double sqrt_D = sqrt(fabs(D));
+ if (real != NULL) {
+ real->setZero(2);
+ }
+ if (imaginary != NULL) {
+ imaginary->setZero(2);
+ }
+
+ // Real roots.
+ if (D >= 0) {
+ if (real != NULL) {
+ // Stable quadratic roots according to BKP Horn.
+ // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
+ if (b >= 0) {
+ (*real)(0) = (-b - sqrt_D) / (2.0 * a);
+ (*real)(1) = (2.0 * c) / (-b - sqrt_D);
+ } else {
+ (*real)(0) = (2.0 * c) / (-b + sqrt_D);
+ (*real)(1) = (-b + sqrt_D) / (2.0 * a);
+ }
+ }
+ return;
+ }
+
+ // Use the normal quadratic formula for the complex case.
+ if (real != NULL) {
+ (*real)(0) = -b / (2.0 * a);
+ (*real)(1) = -b / (2.0 * a);
+ }
+ if (imaginary != NULL) {
+ (*imaginary)(0) = sqrt_D / (2.0 * a);
+ (*imaginary)(1) = -sqrt_D / (2.0 * a);
+ }
+}
} // namespace
bool FindPolynomialRoots(const Vector& polynomial_in,
@@ -132,30 +190,40 @@
Vector polynomial = RemoveLeadingZeros(polynomial_in);
const int degree = polynomial.size() - 1;
+ VLOG(3) << "Input polynomial: " << polynomial_in.transpose();
+ if (polynomial.size() != polynomial_in.size()) {
+ VLOG(3) << "Trimmed polynomial: " << polynomial.transpose();
+ }
+
// Is the polynomial constant?
if (degree == 0) {
LOG(WARNING) << "Trying to extract roots from a constant "
<< "polynomial in FindPolynomialRoots";
+ // We return true with no roots, not false, as if the polynomial is constant
+ // it is correct that there are no roots. It is not the case that they were
+ // there, but that we have failed to extract them.
return true;
}
+ // Linear
+ if (degree == 1) {
+ FindLinearPolynomialRoots(polynomial, real, imaginary);
+ return true;
+ }
+
+ // Quadratic
+ if (degree == 2) {
+ FindQuadraticPolynomialRoots(polynomial, real, imaginary);
+ return true;
+ }
+
+ // The degree is now known to be at least 3. For cubic or higher
+ // roots we use the method of companion matrices.
+
// Divide by leading term
const double leading_term = polynomial(0);
polynomial /= leading_term;
- // Separately handle linear polynomials.
- if (degree == 1) {
- if (real != NULL) {
- real->resize(1);
- (*real)(0) = -polynomial(1);
- }
- if (imaginary != NULL) {
- imaginary->resize(1);
- imaginary->setZero();
- }
- }
-
- // The degree is now known to be at least 2.
// Build and balance the companion matrix to the polynomial.
Matrix companion_matrix(degree, degree);
BuildCompanionMatrix(polynomial, &companion_matrix);
@@ -255,6 +323,12 @@
}
}
+string FunctionSample::ToDebugString() const {
+ return StringPrintf("[x: %.8e, value: %.8e, gradient: %.8e, "
+ "value_is_valid: %d, gradient_is_valid: %d]",
+ x, value, gradient, value_is_valid, gradient_is_valid);
+}
+
Vector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) {
const int num_samples = samples.size();
int num_constraints = 0;
@@ -268,6 +342,7 @@
}
const int degree = num_constraints - 1;
+
Matrix lhs = Matrix::Zero(num_constraints, num_constraints);
Vector rhs = Vector::Zero(num_constraints);
diff --git a/internal/ceres/polynomial.h b/internal/ceres/polynomial.h
index 42ffdcb..80ce77e 100644
--- a/internal/ceres/polynomial.h
+++ b/internal/ceres/polynomial.h
@@ -95,6 +95,7 @@
gradient(0.0),
gradient_is_valid(false) {
}
+ string ToDebugString() const;
double x;
double value; // value = f(x)
diff --git a/internal/ceres/preconditioner.cc b/internal/ceres/preconditioner.cc
index 505a47d..062347f 100644
--- a/internal/ceres/preconditioner.cc
+++ b/internal/ceres/preconditioner.cc
@@ -37,6 +37,16 @@
Preconditioner::~Preconditioner() {
}
+PreconditionerType Preconditioner::PreconditionerForZeroEBlocks(
+ PreconditionerType preconditioner_type) {
+ if (preconditioner_type == SCHUR_JACOBI ||
+ preconditioner_type == CLUSTER_JACOBI ||
+ preconditioner_type == CLUSTER_TRIDIAGONAL) {
+ return JACOBI;
+ }
+ return preconditioner_type;
+}
+
SparseMatrixPreconditionerWrapper::SparseMatrixPreconditionerWrapper(
const SparseMatrix* matrix)
: matrix_(CHECK_NOTNULL(matrix)) {
diff --git a/internal/ceres/preconditioner.h b/internal/ceres/preconditioner.h
index af64e3c..e8d5994 100644
--- a/internal/ceres/preconditioner.h
+++ b/internal/ceres/preconditioner.h
@@ -36,6 +36,7 @@
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/linear_operator.h"
#include "ceres/sparse_matrix.h"
+#include "ceres/types.h"
namespace ceres {
namespace internal {
@@ -48,6 +49,7 @@
struct Options {
Options()
: type(JACOBI),
+ visibility_clustering_type(CANONICAL_VIEWS),
sparse_linear_algebra_library_type(SUITE_SPARSE),
num_threads(1),
row_block_size(Eigen::Dynamic),
@@ -56,7 +58,7 @@
}
PreconditionerType type;
-
+ VisibilityClusteringType visibility_clustering_type;
SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type;
// If possible, how many threads the preconditioner can use.
@@ -94,6 +96,14 @@
int f_block_size;
};
+ // If the optimization problem is such that there are no remaining
+ // e-blocks, ITERATIVE_SCHUR with a Schur type preconditioner cannot
+ // be used. This function returns JACOBI if a preconditioner for
+ // ITERATIVE_SCHUR is used. The input preconditioner_type is
+ // returned otherwise.
+ static PreconditionerType PreconditionerForZeroEBlocks(
+ PreconditionerType preconditioner_type);
+
virtual ~Preconditioner();
// Update the numerical value of the preconditioner for the linear
diff --git a/internal/ceres/problem.cc b/internal/ceres/problem.cc
index 403e96a..674694d 100644
--- a/internal/ceres/problem.cc
+++ b/internal/ceres/problem.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2013 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -178,6 +178,23 @@
problem_impl_->SetParameterization(values, local_parameterization);
}
+const LocalParameterization* Problem::GetParameterization(
+ double* values) const {
+ return problem_impl_->GetParameterization(values);
+}
+
+void Problem::SetParameterLowerBound(double* values,
+ int index,
+ double lower_bound) {
+ problem_impl_->SetParameterLowerBound(values, index, lower_bound);
+}
+
+void Problem::SetParameterUpperBound(double* values,
+ int index,
+ double upper_bound) {
+ problem_impl_->SetParameterUpperBound(values, index, upper_bound);
+}
+
bool Problem::Evaluate(const EvaluateOptions& evaluate_options,
double* cost,
vector<double>* residuals,
@@ -214,8 +231,31 @@
return problem_impl_->ParameterBlockLocalSize(parameter_block);
};
+bool Problem::HasParameterBlock(const double* values) const {
+ return problem_impl_->HasParameterBlock(values);
+}
+
void Problem::GetParameterBlocks(vector<double*>* parameter_blocks) const {
problem_impl_->GetParameterBlocks(parameter_blocks);
}
+void Problem::GetResidualBlocks(
+ vector<ResidualBlockId>* residual_blocks) const {
+ problem_impl_->GetResidualBlocks(residual_blocks);
+}
+
+void Problem::GetParameterBlocksForResidualBlock(
+ const ResidualBlockId residual_block,
+ vector<double*>* parameter_blocks) const {
+ problem_impl_->GetParameterBlocksForResidualBlock(residual_block,
+ parameter_blocks);
+}
+
+void Problem::GetResidualBlocksForParameterBlock(
+ const double* values,
+ vector<ResidualBlockId>* residual_blocks) const {
+ problem_impl_->GetResidualBlocksForParameterBlock(values,
+ residual_blocks);
+}
+
} // namespace ceres
diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index 8302702..7c86efb 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2013 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -27,7 +27,7 @@
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
-// keir@google.com (Keir Mierle)
+// mierle@gmail.com (Keir Mierle)
#include "ceres/problem_impl.h"
@@ -142,7 +142,7 @@
// For dynamic problems, add the list of dependent residual blocks, which is
// empty to start.
- if (options_.enable_fast_parameter_block_removal) {
+ if (options_.enable_fast_removal) {
new_parameter_block->EnableResidualBlockDependencies();
}
parameter_block_map_[values] = new_parameter_block;
@@ -150,6 +150,26 @@
return new_parameter_block;
}
+void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) {
+ CHECK_NOTNULL(residual_block);
+ // Perform no check on the validity of residual_block, that is handled in
+ // the public method: RemoveResidualBlock().
+
+ // If needed, remove the parameter dependencies on this residual block.
+ if (options_.enable_fast_removal) {
+ const int num_parameter_blocks_for_residual =
+ residual_block->NumParameterBlocks();
+ for (int i = 0; i < num_parameter_blocks_for_residual; ++i) {
+ residual_block->parameter_blocks()[i]
+ ->RemoveResidualBlock(residual_block);
+ }
+
+ ResidualBlockSet::iterator it = residual_block_set_.find(residual_block);
+ residual_block_set_.erase(it);
+ }
+ DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block);
+}
+
// Deletes the residual block in question, assuming there are no other
// references to it inside the problem (e.g. by another parameter). Referenced
// cost and loss functions are tucked away for future deletion, since it is not
@@ -224,7 +244,7 @@
cost_function->parameter_block_sizes().size());
// Check the sizes match.
- const vector<int16>& parameter_block_sizes =
+ const vector<int32>& parameter_block_sizes =
cost_function->parameter_block_sizes();
if (!options_.disable_all_safety_checks) {
@@ -278,13 +298,18 @@
program_->residual_blocks_.size());
// Add dependencies on the residual to the parameter blocks.
- if (options_.enable_fast_parameter_block_removal) {
+ if (options_.enable_fast_removal) {
for (int i = 0; i < parameter_blocks.size(); ++i) {
parameter_block_ptrs[i]->AddResidualBlock(new_residual_block);
}
}
program_->residual_blocks_.push_back(new_residual_block);
+
+ if (options_.enable_fast_removal) {
+ residual_block_set_.insert(new_residual_block);
+ }
+
return new_residual_block;
}
@@ -452,7 +477,11 @@
void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks,
Block* block_to_remove) {
CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove)
- << "You found a Ceres bug! Block: " << block_to_remove->ToString();
+ << "You found a Ceres bug! \n"
+ << "Block requested: "
+ << block_to_remove->ToString() << "\n"
+ << "Block present: "
+ << (*mutable_blocks)[block_to_remove->index()]->ToString();
// Prepare the to-be-moved block for the new, lower-in-index position by
// setting the index to the blocks final location.
@@ -471,30 +500,46 @@
void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) {
CHECK_NOTNULL(residual_block);
- // If needed, remove the parameter dependencies on this residual block.
- if (options_.enable_fast_parameter_block_removal) {
- const int num_parameter_blocks_for_residual =
- residual_block->NumParameterBlocks();
- for (int i = 0; i < num_parameter_blocks_for_residual; ++i) {
- residual_block->parameter_blocks()[i]
- ->RemoveResidualBlock(residual_block);
- }
+ // Verify that residual_block identifies a residual in the current problem.
+ const string residual_not_found_message =
+ StringPrintf("Residual block to remove: %p not found. This usually means "
+ "one of three things have happened:\n"
+ " 1) residual_block is uninitialised and points to a random "
+ "area in memory.\n"
+ " 2) residual_block represented a residual that was added to"
+ " the problem, but referred to a parameter block which has "
+ "since been removed, which removes all residuals which "
+ "depend on that parameter block, and was thus removed.\n"
+ " 3) residual_block referred to a residual that has already "
+ "been removed from the problem (by the user).",
+ residual_block);
+ if (options_.enable_fast_removal) {
+ CHECK(residual_block_set_.find(residual_block) !=
+ residual_block_set_.end())
+ << residual_not_found_message;
+ } else {
+ // Perform a full search over all current residuals.
+ CHECK(std::find(program_->residual_blocks().begin(),
+ program_->residual_blocks().end(),
+ residual_block) != program_->residual_blocks().end())
+ << residual_not_found_message;
}
- DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block);
+
+ InternalRemoveResidualBlock(residual_block);
}
void ProblemImpl::RemoveParameterBlock(double* values) {
ParameterBlock* parameter_block =
FindParameterBlockOrDie(parameter_block_map_, values);
- if (options_.enable_fast_parameter_block_removal) {
+ if (options_.enable_fast_removal) {
// Copy the dependent residuals from the parameter block because the set of
// dependents will change after each call to RemoveResidualBlock().
vector<ResidualBlock*> residual_blocks_to_remove(
parameter_block->mutable_residual_blocks()->begin(),
parameter_block->mutable_residual_blocks()->end());
for (int i = 0; i < residual_blocks_to_remove.size(); ++i) {
- RemoveResidualBlock(residual_blocks_to_remove[i]);
+ InternalRemoveResidualBlock(residual_blocks_to_remove[i]);
}
} else {
// Scan all the residual blocks to remove ones that depend on the parameter
@@ -506,7 +551,7 @@
const int num_parameter_blocks = residual_block->NumParameterBlocks();
for (int j = 0; j < num_parameter_blocks; ++j) {
if (residual_block->parameter_blocks()[j] == parameter_block) {
- RemoveResidualBlock(residual_block);
+ InternalRemoveResidualBlock(residual_block);
// The parameter blocks are guaranteed unique.
break;
}
@@ -531,6 +576,26 @@
->SetParameterization(local_parameterization);
}
+const LocalParameterization* ProblemImpl::GetParameterization(
+ double* values) const {
+ return FindParameterBlockOrDie(parameter_block_map_, values)
+ ->local_parameterization();
+}
+
+void ProblemImpl::SetParameterLowerBound(double* values,
+ int index,
+ double lower_bound) {
+ FindParameterBlockOrDie(parameter_block_map_, values)
+ ->SetLowerBound(index, lower_bound);
+}
+
+void ProblemImpl::SetParameterUpperBound(double* values,
+ int index,
+ double upper_bound) {
+ FindParameterBlockOrDie(parameter_block_map_, values)
+ ->SetUpperBound(index, upper_bound);
+}
+
bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
double* cost,
vector<double>* residuals,
@@ -634,6 +699,9 @@
for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
variable_parameter_blocks[i]->SetVarying();
}
+
+ program_->SetParameterBlockStatePtrsToUserStatePtrs();
+ program_->SetParameterOffsetsAndIndex();
return false;
}
@@ -692,6 +760,8 @@
}
}
+ program_->SetParameterBlockStatePtrsToUserStatePtrs();
+ program_->SetParameterOffsetsAndIndex();
return status;
}
@@ -721,6 +791,11 @@
parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize();
};
+bool ProblemImpl::HasParameterBlock(const double* parameter_block) const {
+ return (parameter_block_map_.find(const_cast<double*>(parameter_block)) !=
+ parameter_block_map_.end());
+}
+
void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const {
CHECK_NOTNULL(parameter_blocks);
parameter_blocks->resize(0);
@@ -731,6 +806,57 @@
}
}
+void ProblemImpl::GetResidualBlocks(
+ vector<ResidualBlockId>* residual_blocks) const {
+ CHECK_NOTNULL(residual_blocks);
+ *residual_blocks = program().residual_blocks();
+}
+
+void ProblemImpl::GetParameterBlocksForResidualBlock(
+ const ResidualBlockId residual_block,
+ vector<double*>* parameter_blocks) const {
+ int num_parameter_blocks = residual_block->NumParameterBlocks();
+ CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks);
+ for (int i = 0; i < num_parameter_blocks; ++i) {
+ (*parameter_blocks)[i] =
+ residual_block->parameter_blocks()[i]->mutable_user_state();
+ }
+}
+
+void ProblemImpl::GetResidualBlocksForParameterBlock(
+ const double* values,
+ vector<ResidualBlockId>* residual_blocks) const {
+ ParameterBlock* parameter_block =
+ FindParameterBlockOrDie(parameter_block_map_,
+ const_cast<double*>(values));
+
+ if (options_.enable_fast_removal) {
+ // In this case the residual blocks that depend on the parameter block are
+ // stored in the parameter block already, so just copy them out.
+ CHECK_NOTNULL(residual_blocks)->resize(
+ parameter_block->mutable_residual_blocks()->size());
+ std::copy(parameter_block->mutable_residual_blocks()->begin(),
+ parameter_block->mutable_residual_blocks()->end(),
+ residual_blocks->begin());
+ return;
+ }
+
+ // Find residual blocks that depend on the parameter block.
+ CHECK_NOTNULL(residual_blocks)->clear();
+ const int num_residual_blocks = NumResidualBlocks();
+ for (int i = 0; i < num_residual_blocks; ++i) {
+ ResidualBlock* residual_block =
+ (*(program_->mutable_residual_blocks()))[i];
+ const int num_parameter_blocks = residual_block->NumParameterBlocks();
+ for (int j = 0; j < num_parameter_blocks; ++j) {
+ if (residual_block->parameter_blocks()[j] == parameter_block) {
+ residual_blocks->push_back(residual_block);
+ // The parameter blocks are guaranteed unique.
+ break;
+ }
+ }
+ }
+}
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h
index ace27f5..7b5547b 100644
--- a/internal/ceres/problem_impl.h
+++ b/internal/ceres/problem_impl.h
@@ -45,6 +45,7 @@
#include "ceres/internal/macros.h"
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
+#include "ceres/collections_port.h"
#include "ceres/problem.h"
#include "ceres/types.h"
@@ -63,6 +64,7 @@
class ProblemImpl {
public:
typedef map<double*, ParameterBlock*> ParameterMap;
+ typedef HashSet<ResidualBlock*> ResidualBlockSet;
ProblemImpl();
explicit ProblemImpl(const Problem::Options& options);
@@ -127,6 +129,10 @@
void SetParameterBlockVariable(double* values);
void SetParameterization(double* values,
LocalParameterization* local_parameterization);
+ const LocalParameterization* GetParameterization(double* values) const;
+
+ void SetParameterLowerBound(double* values, int index, double lower_bound);
+ void SetParameterUpperBound(double* values, int index, double upper_bound);
bool Evaluate(const Problem::EvaluateOptions& options,
double* cost,
@@ -141,15 +147,33 @@
int ParameterBlockSize(const double* parameter_block) const;
int ParameterBlockLocalSize(const double* parameter_block) const;
+
+ bool HasParameterBlock(const double* parameter_block) const;
+
void GetParameterBlocks(vector<double*>* parameter_blocks) const;
+ void GetResidualBlocks(vector<ResidualBlockId>* residual_blocks) const;
+
+ void GetParameterBlocksForResidualBlock(
+ const ResidualBlockId residual_block,
+ vector<double*>* parameter_blocks) const;
+
+ void GetResidualBlocksForParameterBlock(
+ const double* values,
+ vector<ResidualBlockId>* residual_blocks) const;
const Program& program() const { return *program_; }
Program* mutable_program() { return program_.get(); }
const ParameterMap& parameter_map() const { return parameter_block_map_; }
+ const ResidualBlockSet& residual_block_set() const {
+ CHECK(options_.enable_fast_removal)
+ << "Fast removal not enabled, residual_block_set is not maintained.";
+ return residual_block_set_;
+ }
private:
ParameterBlock* InternalAddParameterBlock(double* values, int size);
+ void InternalRemoveResidualBlock(ResidualBlock* residual_block);
bool InternalEvaluate(Program* program,
double* cost,
@@ -171,6 +195,9 @@
// The mapping from user pointers to parameter blocks.
map<double*, ParameterBlock*> parameter_block_map_;
+ // Iff enable_fast_removal is enabled, contains the current residual blocks.
+ ResidualBlockSet residual_block_set_;
+
// The actual parameter and residual blocks.
internal::scoped_ptr<internal::Program> program_;
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index 0944d3f..db082ec 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2013 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -56,7 +56,7 @@
// Trivial cost function that accepts a single argument.
class UnaryCostFunction : public CostFunction {
public:
- UnaryCostFunction(int num_residuals, int16 parameter_block_size) {
+ UnaryCostFunction(int num_residuals, int32 parameter_block_size) {
set_num_residuals(num_residuals);
mutable_parameter_block_sizes()->push_back(parameter_block_size);
}
@@ -76,8 +76,8 @@
class BinaryCostFunction: public CostFunction {
public:
BinaryCostFunction(int num_residuals,
- int16 parameter_block1_size,
- int16 parameter_block2_size) {
+ int32 parameter_block1_size,
+ int32 parameter_block2_size) {
set_num_residuals(num_residuals);
mutable_parameter_block_sizes()->push_back(parameter_block1_size);
mutable_parameter_block_sizes()->push_back(parameter_block2_size);
@@ -97,9 +97,9 @@
class TernaryCostFunction: public CostFunction {
public:
TernaryCostFunction(int num_residuals,
- int16 parameter_block1_size,
- int16 parameter_block2_size,
- int16 parameter_block3_size) {
+ int32 parameter_block1_size,
+ int32 parameter_block2_size,
+ int32 parameter_block3_size) {
set_num_residuals(num_residuals);
mutable_parameter_block_sizes()->push_back(parameter_block1_size);
mutable_parameter_block_sizes()->push_back(parameter_block2_size);
@@ -139,7 +139,7 @@
// UnaryCostFunction takes only one parameter, but two are passed.
EXPECT_DEATH_IF_SUPPORTED(
problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x, y),
- "parameter_blocks.size()");
+ "parameter_blocks.size");
}
TEST(Problem, AddResidualWithDifferentSizesOnTheSameVariableDies) {
@@ -378,7 +378,7 @@
struct DynamicProblem : public ::testing::TestWithParam<bool> {
DynamicProblem() {
Problem::Options options;
- options.enable_fast_parameter_block_removal = GetParam();
+ options.enable_fast_removal = GetParam();
problem.reset(new ProblemImpl(options));
}
@@ -390,9 +390,26 @@
}
bool HasResidualBlock(ResidualBlock* residual_block) {
- return find(problem->program().residual_blocks().begin(),
- problem->program().residual_blocks().end(),
- residual_block) != problem->program().residual_blocks().end();
+ bool have_residual_block = true;
+ if (GetParam()) {
+ have_residual_block &=
+ (problem->residual_block_set().find(residual_block) !=
+ problem->residual_block_set().end());
+ }
+ have_residual_block &=
+ find(problem->program().residual_blocks().begin(),
+ problem->program().residual_blocks().end(),
+ residual_block) != problem->program().residual_blocks().end();
+ return have_residual_block;
+ }
+
+ int NumResidualBlocks() {
+ // Verify that the hash set of residuals is maintained consistently.
+ if (GetParam()) {
+ EXPECT_EQ(problem->residual_block_set().size(),
+ problem->NumResidualBlocks());
+ }
+ return problem->NumResidualBlocks();
}
// The next block of functions until the end are only for testing the
@@ -502,6 +519,20 @@
problem.RemoveParameterBlock(y), "Parameter block not found:");
}
+TEST(Problem, GetParameterization) {
+ double x[3];
+ double y[2];
+
+ Problem problem;
+ problem.AddParameterBlock(x, 3);
+ problem.AddParameterBlock(y, 2);
+
+ LocalParameterization* parameterization = new IdentityParameterization(3);
+ problem.SetParameterization(x, parameterization);
+ EXPECT_EQ(problem.GetParameterization(x), parameterization);
+ EXPECT_TRUE(problem.GetParameterization(y) == NULL);
+}
+
TEST(Problem, ParameterBlockQueryTest) {
double x[3];
double y[4];
@@ -525,7 +556,9 @@
EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y);
EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y);
+ EXPECT_TRUE(problem.HasParameterBlock(x));
problem.RemoveParameterBlock(x);
+ EXPECT_FALSE(problem.HasParameterBlock(x));
problem.GetParameterBlocks(¶meter_blocks);
EXPECT_EQ(parameter_blocks.size(), 1);
EXPECT_TRUE(parameter_blocks[0] == y);
@@ -536,7 +569,7 @@
problem->AddParameterBlock(z, 5);
problem->AddParameterBlock(w, 3);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(y, GetParameterBlock(0)->user_state());
EXPECT_EQ(z, GetParameterBlock(1)->user_state());
EXPECT_EQ(w, GetParameterBlock(2)->user_state());
@@ -545,12 +578,12 @@
// removing it.
problem->RemoveParameterBlock(w);
ASSERT_EQ(2, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(y, GetParameterBlock(0)->user_state());
EXPECT_EQ(z, GetParameterBlock(1)->user_state());
problem->AddParameterBlock(w, 3);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(y, GetParameterBlock(0)->user_state());
EXPECT_EQ(z, GetParameterBlock(1)->user_state());
EXPECT_EQ(w, GetParameterBlock(2)->user_state());
@@ -558,12 +591,12 @@
// Now remove z, which is in the middle, and add it back.
problem->RemoveParameterBlock(z);
ASSERT_EQ(2, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(y, GetParameterBlock(0)->user_state());
EXPECT_EQ(w, GetParameterBlock(1)->user_state());
problem->AddParameterBlock(z, 5);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(y, GetParameterBlock(0)->user_state());
EXPECT_EQ(w, GetParameterBlock(1)->user_state());
EXPECT_EQ(z, GetParameterBlock(2)->user_state());
@@ -572,20 +605,20 @@
// y
problem->RemoveParameterBlock(y);
ASSERT_EQ(2, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(z, GetParameterBlock(0)->user_state());
EXPECT_EQ(w, GetParameterBlock(1)->user_state());
// z
problem->RemoveParameterBlock(z);
ASSERT_EQ(1, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(w, GetParameterBlock(0)->user_state());
// w
problem->RemoveParameterBlock(w);
EXPECT_EQ(0, problem->NumParameterBlocks());
- EXPECT_EQ(0, problem->NumResidualBlocks());
+ EXPECT_EQ(0, NumResidualBlocks());
}
TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) {
@@ -593,7 +626,7 @@
problem->AddParameterBlock(z, 5);
problem->AddParameterBlock(w, 3);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
EXPECT_EQ(y, GetParameterBlock(0)->user_state());
EXPECT_EQ(z, GetParameterBlock(1)->user_state());
EXPECT_EQ(w, GetParameterBlock(2)->user_state());
@@ -616,12 +649,12 @@
ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w);
EXPECT_EQ(3, problem->NumParameterBlocks());
- EXPECT_EQ(7, problem->NumResidualBlocks());
+ EXPECT_EQ(7, NumResidualBlocks());
// Remove w, which should remove r_yzw, r_yw, r_zw, r_w.
problem->RemoveParameterBlock(w);
ASSERT_EQ(2, problem->NumParameterBlocks());
- ASSERT_EQ(3, problem->NumResidualBlocks());
+ ASSERT_EQ(3, NumResidualBlocks());
ASSERT_FALSE(HasResidualBlock(r_yzw));
ASSERT_TRUE (HasResidualBlock(r_yz ));
@@ -634,7 +667,7 @@
// Remove z, which will remove almost everything else.
problem->RemoveParameterBlock(z);
ASSERT_EQ(1, problem->NumParameterBlocks());
- ASSERT_EQ(1, problem->NumResidualBlocks());
+ ASSERT_EQ(1, NumResidualBlocks());
ASSERT_FALSE(HasResidualBlock(r_yzw));
ASSERT_FALSE(HasResidualBlock(r_yz ));
@@ -647,7 +680,7 @@
// Remove y; all gone.
problem->RemoveParameterBlock(y);
EXPECT_EQ(0, problem->NumParameterBlocks());
- EXPECT_EQ(0, problem->NumResidualBlocks());
+ EXPECT_EQ(0, NumResidualBlocks());
}
TEST_P(DynamicProblem, RemoveResidualBlock) {
@@ -685,14 +718,14 @@
EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL);
}
EXPECT_EQ(3, problem->NumParameterBlocks());
- EXPECT_EQ(7, problem->NumResidualBlocks());
+ EXPECT_EQ(7, NumResidualBlocks());
// Remove each residual and check the state after each removal.
// Remove r_yzw.
problem->RemoveResidualBlock(r_yzw);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(6, problem->NumResidualBlocks());
+ ASSERT_EQ(6, NumResidualBlocks());
if (GetParam()) {
ExpectParameterBlockContains(y, r_yz, r_yw, r_y);
ExpectParameterBlockContains(z, r_yz, r_zw, r_z);
@@ -708,7 +741,7 @@
// Remove r_yw.
problem->RemoveResidualBlock(r_yw);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(5, problem->NumResidualBlocks());
+ ASSERT_EQ(5, NumResidualBlocks());
if (GetParam()) {
ExpectParameterBlockContains(y, r_yz, r_y);
ExpectParameterBlockContains(z, r_yz, r_zw, r_z);
@@ -723,7 +756,7 @@
// Remove r_zw.
problem->RemoveResidualBlock(r_zw);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(4, problem->NumResidualBlocks());
+ ASSERT_EQ(4, NumResidualBlocks());
if (GetParam()) {
ExpectParameterBlockContains(y, r_yz, r_y);
ExpectParameterBlockContains(z, r_yz, r_z);
@@ -737,7 +770,7 @@
// Remove r_w.
problem->RemoveResidualBlock(r_w);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(3, problem->NumResidualBlocks());
+ ASSERT_EQ(3, NumResidualBlocks());
if (GetParam()) {
ExpectParameterBlockContains(y, r_yz, r_y);
ExpectParameterBlockContains(z, r_yz, r_z);
@@ -750,7 +783,7 @@
// Remove r_yz.
problem->RemoveResidualBlock(r_yz);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(2, problem->NumResidualBlocks());
+ ASSERT_EQ(2, NumResidualBlocks());
if (GetParam()) {
ExpectParameterBlockContains(y, r_y);
ExpectParameterBlockContains(z, r_z);
@@ -763,7 +796,7 @@
problem->RemoveResidualBlock(r_z);
problem->RemoveResidualBlock(r_y);
ASSERT_EQ(3, problem->NumParameterBlocks());
- ASSERT_EQ(0, problem->NumResidualBlocks());
+ ASSERT_EQ(0, NumResidualBlocks());
if (GetParam()) {
ExpectParameterBlockContains(y);
ExpectParameterBlockContains(z);
@@ -771,6 +804,191 @@
}
}
+TEST_P(DynamicProblem, RemoveInvalidResidualBlockDies) {
+ problem->AddParameterBlock(y, 4);
+ problem->AddParameterBlock(z, 5);
+ problem->AddParameterBlock(w, 3);
+
+ // Add all combinations of cost functions.
+ CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
+ CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5);
+ CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3);
+ CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3);
+ CostFunction* cost_y = new UnaryCostFunction (1, 4);
+ CostFunction* cost_z = new UnaryCostFunction (1, 5);
+ CostFunction* cost_w = new UnaryCostFunction (1, 3);
+
+ ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
+ ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z);
+ ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w);
+ ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w);
+ ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y);
+ ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z);
+ ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w);
+
+ // Remove r_yzw.
+ problem->RemoveResidualBlock(r_yzw);
+ ASSERT_EQ(3, problem->NumParameterBlocks());
+ ASSERT_EQ(6, NumResidualBlocks());
+ // Attempt to remove r_yzw again.
+ EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yzw), "not found");
+
+ // Attempt to remove a cast pointer never added as a residual.
+ int trash_memory = 1234;
+ ResidualBlock* invalid_residual =
+ reinterpret_cast<ResidualBlock*>(&trash_memory);
+ EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(invalid_residual),
+ "not found");
+
+ // Remove a parameter block, which in turn removes the dependent residuals
+ // then attempt to remove them directly.
+ problem->RemoveParameterBlock(z);
+ ASSERT_EQ(2, problem->NumParameterBlocks());
+ ASSERT_EQ(3, NumResidualBlocks());
+ EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yz), "not found");
+ EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_zw), "not found");
+ EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_z), "not found");
+
+ problem->RemoveResidualBlock(r_yw);
+ problem->RemoveResidualBlock(r_w);
+ problem->RemoveResidualBlock(r_y);
+}
+
+// Check that a null-terminated array, a, has the same elements as b.
+template<typename T>
+void ExpectVectorContainsUnordered(const T* a, const vector<T>& b) {
+ // Compute the size of a.
+ int size = 0;
+ while (a[size]) {
+ ++size;
+ }
+ ASSERT_EQ(size, b.size());
+
+ // Sort a.
+ vector<T> a_sorted(size);
+ copy(a, a + size, a_sorted.begin());
+ sort(a_sorted.begin(), a_sorted.end());
+
+ // Sort b.
+ vector<T> b_sorted(b);
+ sort(b_sorted.begin(), b_sorted.end());
+
+ // Compare.
+ for (int i = 0; i < size; ++i) {
+ EXPECT_EQ(a_sorted[i], b_sorted[i]);
+ }
+}
+
+void ExpectProblemHasResidualBlocks(
+ const ProblemImpl &problem,
+ const ResidualBlockId *expected_residual_blocks) {
+ vector<ResidualBlockId> residual_blocks;
+ problem.GetResidualBlocks(&residual_blocks);
+ ExpectVectorContainsUnordered(expected_residual_blocks, residual_blocks);
+}
+
+TEST_P(DynamicProblem, GetXXXBlocksForYYYBlock) {
+ problem->AddParameterBlock(y, 4);
+ problem->AddParameterBlock(z, 5);
+ problem->AddParameterBlock(w, 3);
+
+ // Add all combinations of cost functions.
+ CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
+ CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5);
+ CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3);
+ CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3);
+ CostFunction* cost_y = new UnaryCostFunction (1, 4);
+ CostFunction* cost_z = new UnaryCostFunction (1, 5);
+ CostFunction* cost_w = new UnaryCostFunction (1, 3);
+
+ ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
+ {
+ ResidualBlockId expected_residuals[] = {r_yzw, 0};
+ ExpectProblemHasResidualBlocks(*problem, expected_residuals);
+ }
+ ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z);
+ {
+ ResidualBlockId expected_residuals[] = {r_yzw, r_yz, 0};
+ ExpectProblemHasResidualBlocks(*problem, expected_residuals);
+ }
+ ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w);
+ {
+ ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, 0};
+ ExpectProblemHasResidualBlocks(*problem, expected_residuals);
+ }
+ ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w);
+ {
+ ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, 0};
+ ExpectProblemHasResidualBlocks(*problem, expected_residuals);
+ }
+ ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y);
+ {
+ ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, r_y, 0};
+ ExpectProblemHasResidualBlocks(*problem, expected_residuals);
+ }
+ ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z);
+ {
+ ResidualBlock *expected_residuals[] = {
+ r_yzw, r_yz, r_yw, r_zw, r_y, r_z, 0
+ };
+ ExpectProblemHasResidualBlocks(*problem, expected_residuals);
+ }
+ ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w);
+ {
+ ResidualBlock *expected_residuals[] = {
+ r_yzw, r_yz, r_yw, r_zw, r_y, r_z, r_w, 0
+ };
+ ExpectProblemHasResidualBlocks(*problem, expected_residuals);
+ }
+
+ vector<double*> parameter_blocks;
+ vector<ResidualBlockId> residual_blocks;
+
+ // Check GetResidualBlocksForParameterBlock() for all parameter blocks.
+ struct GetResidualBlocksForParameterBlockTestCase {
+ double* parameter_block;
+ ResidualBlockId expected_residual_blocks[10];
+ };
+ GetResidualBlocksForParameterBlockTestCase get_residual_blocks_cases[] = {
+ { y, { r_yzw, r_yz, r_yw, r_y, NULL} },
+ { z, { r_yzw, r_yz, r_zw, r_z, NULL} },
+ { w, { r_yzw, r_yw, r_zw, r_w, NULL} },
+ { NULL }
+ };
+ for (int i = 0; get_residual_blocks_cases[i].parameter_block; ++i) {
+ problem->GetResidualBlocksForParameterBlock(
+ get_residual_blocks_cases[i].parameter_block,
+ &residual_blocks);
+ ExpectVectorContainsUnordered(
+ get_residual_blocks_cases[i].expected_residual_blocks,
+ residual_blocks);
+ }
+
+ // Check GetParameterBlocksForResidualBlock() for all residual blocks.
+ struct GetParameterBlocksForResidualBlockTestCase {
+ ResidualBlockId residual_block;
+ double* expected_parameter_blocks[10];
+ };
+ GetParameterBlocksForResidualBlockTestCase get_parameter_blocks_cases[] = {
+ { r_yzw, { y, z, w, NULL } },
+ { r_yz , { y, z, NULL } },
+ { r_yw , { y, w, NULL } },
+ { r_zw , { z, w, NULL } },
+ { r_y , { y, NULL } },
+ { r_z , { z, NULL } },
+ { r_w , { w, NULL } },
+ { NULL }
+ };
+ for (int i = 0; get_parameter_blocks_cases[i].residual_block; ++i) {
+ problem->GetParameterBlocksForResidualBlock(
+ get_parameter_blocks_cases[i].residual_block,
+ ¶meter_blocks);
+ ExpectVectorContainsUnordered(
+ get_parameter_blocks_cases[i].expected_parameter_blocks,
+ parameter_blocks);
+ }
+}
+
INSTANTIATE_TEST_CASE_P(OptionsInstantiation,
DynamicProblem,
::testing::Values(true, false));
@@ -862,7 +1080,9 @@
parameters_));
}
-
+ void TearDown() {
+ EXPECT_TRUE(problem_.program().IsValid());
+ }
void EvaluateAndCompare(const Problem::EvaluateOptions& options,
const int expected_num_rows,
diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc
index 82d76d3..1d0a157 100644
--- a/internal/ceres/program.cc
+++ b/internal/ceres/program.cc
@@ -32,6 +32,7 @@
#include <map>
#include <vector>
+#include "ceres/array_utils.h"
#include "ceres/casts.h"
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/cost_function.h"
@@ -44,6 +45,7 @@
#include "ceres/problem.h"
#include "ceres/residual_block.h"
#include "ceres/stl_util.h"
+#include "ceres/triplet_sparse_matrix.h"
namespace ceres {
namespace internal {
@@ -140,6 +142,289 @@
}
}
+bool Program::IsValid() const {
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ const ResidualBlock* residual_block = residual_blocks_[i];
+ if (residual_block->index() != i) {
+ LOG(WARNING) << "Residual block: " << i
+ << " has incorrect index: " << residual_block->index();
+ return false;
+ }
+ }
+
+ int state_offset = 0;
+ int delta_offset = 0;
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ if (parameter_block->index() != i ||
+ parameter_block->state_offset() != state_offset ||
+ parameter_block->delta_offset() != delta_offset) {
+ LOG(WARNING) << "Parameter block: " << i
+ << "has incorrect indexing information: "
+ << parameter_block->ToString();
+ return false;
+ }
+
+ state_offset += parameter_blocks_[i]->Size();
+ delta_offset += parameter_blocks_[i]->LocalSize();
+ }
+
+ return true;
+}
+
+bool Program::ParameterBlocksAreFinite(string* message) const {
+ CHECK_NOTNULL(message);
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ const double* array = parameter_block->user_state();
+ const int size = parameter_block->Size();
+ const int invalid_index = FindInvalidValue(size, array);
+ if (invalid_index != size) {
+ *message = StringPrintf(
+ "ParameterBlock: %p with size %d has at least one invalid value.\n"
+ "First invalid value is at index: %d.\n"
+ "Parameter block values: ",
+ array, size, invalid_index);
+ AppendArrayToString(size, array, message);
+ return false;
+ }
+ }
+ return true;
+}
+
+bool Program::IsBoundsConstrained() const {
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ if (parameter_block->IsConstant()) {
+ continue;
+ }
+ const int size = parameter_block->Size();
+ for (int j = 0; j < size; ++j) {
+ const double lower_bound = parameter_block->LowerBoundForParameter(j);
+ const double upper_bound = parameter_block->UpperBoundForParameter(j);
+ if (lower_bound > -std::numeric_limits<double>::max() ||
+ upper_bound < std::numeric_limits<double>::max()) {
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+bool Program::IsFeasible(string* message) const {
+ CHECK_NOTNULL(message);
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ const double* parameters = parameter_block->user_state();
+ const int size = parameter_block->Size();
+ if (parameter_block->IsConstant()) {
+ // Constant parameter blocks must start in the feasible region
+ // to ultimately produce a feasible solution, since Ceres cannot
+ // change them.
+ for (int j = 0; j < size; ++j) {
+ const double lower_bound = parameter_block->LowerBoundForParameter(j);
+ const double upper_bound = parameter_block->UpperBoundForParameter(j);
+ if (parameters[j] < lower_bound || parameters[j] > upper_bound) {
+ *message = StringPrintf(
+ "ParameterBlock: %p with size %d has at least one infeasible "
+ "value."
+ "\nFirst infeasible value is at index: %d."
+ "\nLower bound: %e, value: %e, upper bound: %e"
+ "\nParameter block values: ",
+ parameters, size, j, lower_bound, parameters[j], upper_bound);
+ AppendArrayToString(size, parameters, message);
+ return false;
+ }
+ }
+ } else {
+ // Variable parameter blocks must have non-empty feasible
+ // regions, otherwise there is no way to produce a feasible
+ // solution.
+ for (int j = 0; j < size; ++j) {
+ const double lower_bound = parameter_block->LowerBoundForParameter(j);
+ const double upper_bound = parameter_block->UpperBoundForParameter(j);
+ if (lower_bound >= upper_bound) {
+ *message = StringPrintf(
+ "ParameterBlock: %p with size %d has at least one infeasible "
+ "bound."
+ "\nFirst infeasible bound is at index: %d."
+ "\nLower bound: %e, upper bound: %e"
+ "\nParameter block values: ",
+ parameters, size, j, lower_bound, upper_bound);
+ AppendArrayToString(size, parameters, message);
+ return false;
+ }
+ }
+ }
+ }
+
+ return true;
+}
+
+Program* Program::CreateReducedProgram(vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* error) const {
+ CHECK_NOTNULL(removed_parameter_blocks);
+ CHECK_NOTNULL(fixed_cost);
+ CHECK_NOTNULL(error);
+
+ scoped_ptr<Program> reduced_program(new Program(*this));
+ if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks,
+ fixed_cost,
+ error)) {
+ return NULL;
+ }
+
+ reduced_program->SetParameterOffsetsAndIndex();
+ return reduced_program.release();
+}
+
+bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* error) {
+ CHECK_NOTNULL(removed_parameter_blocks);
+ CHECK_NOTNULL(fixed_cost);
+ CHECK_NOTNULL(error);
+
+ scoped_array<double> residual_block_evaluate_scratch;
+ residual_block_evaluate_scratch.reset(
+ new double[MaxScratchDoublesNeededForEvaluate()]);
+ *fixed_cost = 0.0;
+
+ // Mark all the parameters as unused. Abuse the index member of the
+ // parameter blocks for the marking.
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ parameter_blocks_[i]->set_index(-1);
+ }
+
+ // Filter out residual that have all-constant parameters, and mark
+ // all the parameter blocks that appear in residuals.
+ int num_active_residual_blocks = 0;
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ ResidualBlock* residual_block = residual_blocks_[i];
+ int num_parameter_blocks = residual_block->NumParameterBlocks();
+
+ // Determine if the residual block is fixed, and also mark varying
+ // parameters that appear in the residual block.
+ bool all_constant = true;
+ for (int k = 0; k < num_parameter_blocks; k++) {
+ ParameterBlock* parameter_block = residual_block->parameter_blocks()[k];
+ if (!parameter_block->IsConstant()) {
+ all_constant = false;
+ parameter_block->set_index(1);
+ }
+ }
+
+ if (!all_constant) {
+ residual_blocks_[num_active_residual_blocks++] = residual_block;
+ continue;
+ }
+
+ // The residual is constant and will be removed, so its cost is
+ // added to the variable fixed_cost.
+ double cost = 0.0;
+ if (!residual_block->Evaluate(true,
+ &cost,
+ NULL,
+ NULL,
+ residual_block_evaluate_scratch.get())) {
+ *error = StringPrintf("Evaluation of the residual %d failed during "
+ "removal of fixed residual blocks.", i);
+ return false;
+ }
+ *fixed_cost += cost;
+ }
+ residual_blocks_.resize(num_active_residual_blocks);
+
+ // Filter out unused or fixed parameter blocks.
+ int num_active_parameter_blocks = 0;
+ removed_parameter_blocks->clear();
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ ParameterBlock* parameter_block = parameter_blocks_[i];
+ if (parameter_block->index() == -1) {
+ removed_parameter_blocks->push_back(parameter_block->mutable_user_state());
+ } else {
+ parameter_blocks_[num_active_parameter_blocks++] = parameter_block;
+ }
+ }
+ parameter_blocks_.resize(num_active_parameter_blocks);
+
+ if (!(((NumResidualBlocks() == 0) &&
+ (NumParameterBlocks() == 0)) ||
+ ((NumResidualBlocks() != 0) &&
+ (NumParameterBlocks() != 0)))) {
+ *error = "Congratulations, you found a bug in Ceres. Please report it.";
+ return false;
+ }
+
+ return true;
+}
+
+bool Program::IsParameterBlockSetIndependent(const set<double*>& independent_set) const {
+ // Loop over each residual block and ensure that no two parameter
+ // blocks in the same residual block are part of
+ // parameter_block_ptrs as that would violate the assumption that it
+ // is an independent set in the Hessian matrix.
+ for (vector<ResidualBlock*>::const_iterator it = residual_blocks_.begin();
+ it != residual_blocks_.end();
+ ++it) {
+ ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks();
+ const int num_parameter_blocks = (*it)->NumParameterBlocks();
+ int count = 0;
+ for (int i = 0; i < num_parameter_blocks; ++i) {
+ count += independent_set.count(
+ parameter_blocks[i]->mutable_user_state());
+ }
+ if (count > 1) {
+ return false;
+ }
+ }
+ return true;
+}
+
+TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const {
+ // Matrix to store the block sparsity structure of the Jacobian.
+ TripletSparseMatrix* tsm =
+ new TripletSparseMatrix(NumParameterBlocks(),
+ NumResidualBlocks(),
+ 10 * NumResidualBlocks());
+ int num_nonzeros = 0;
+ int* rows = tsm->mutable_rows();
+ int* cols = tsm->mutable_cols();
+ double* values = tsm->mutable_values();
+
+ for (int c = 0; c < residual_blocks_.size(); ++c) {
+ const ResidualBlock* residual_block = residual_blocks_[c];
+ const int num_parameter_blocks = residual_block->NumParameterBlocks();
+ ParameterBlock* const* parameter_blocks =
+ residual_block->parameter_blocks();
+
+ for (int j = 0; j < num_parameter_blocks; ++j) {
+ if (parameter_blocks[j]->IsConstant()) {
+ continue;
+ }
+
+ // Re-size the matrix if needed.
+ if (num_nonzeros >= tsm->max_num_nonzeros()) {
+ tsm->set_num_nonzeros(num_nonzeros);
+ tsm->Reserve(2 * num_nonzeros);
+ rows = tsm->mutable_rows();
+ cols = tsm->mutable_cols();
+ values = tsm->mutable_values();
+ }
+
+ const int r = parameter_blocks[j]->index();
+ rows[num_nonzeros] = r;
+ cols[num_nonzeros] = c;
+ values[num_nonzeros] = 1.0;
+ ++num_nonzeros;
+ }
+ }
+
+ tsm->set_num_nonzeros(num_nonzeros);
+ return tsm;
+}
+
int Program::NumResidualBlocks() const {
return residual_blocks_.size();
}
diff --git a/internal/ceres/program.h b/internal/ceres/program.h
index 5002b7e..c7b22c4 100644
--- a/internal/ceres/program.h
+++ b/internal/ceres/program.h
@@ -31,6 +31,7 @@
#ifndef CERES_INTERNAL_PROGRAM_H_
#define CERES_INTERNAL_PROGRAM_H_
+#include <set>
#include <string>
#include <vector>
#include "ceres/internal/port.h"
@@ -41,6 +42,7 @@
class ParameterBlock;
class ProblemImpl;
class ResidualBlock;
+class TripletSparseMatrix;
// A nonlinear least squares optimization problem. This is different from the
// similarly-named "Problem" object, which offers a mutation interface for
@@ -99,6 +101,51 @@
// position of the parameter in the state and delta vector respectively.
void SetParameterOffsetsAndIndex();
+ // Check if the internal state of the program (the indexing and the
+ // offsets) are correct.
+ bool IsValid() const;
+
+ bool ParameterBlocksAreFinite(string* message) const;
+
+ // Returns true if the program has any non-constant parameter blocks
+ // which have non-trivial bounds constraints.
+ bool IsBoundsConstrained() const;
+
+ // Returns false, if the program has any constant parameter blocks
+ // which are not feasible, or any variable parameter blocks which
+ // have a lower bound greater than or equal to the upper bound.
+ bool IsFeasible(string* message) const;
+
+ // Loop over each residual block and ensure that no two parameter
+ // blocks in the same residual block are part of
+ // parameter_blocks as that would violate the assumption that it
+ // is an independent set in the Hessian matrix.
+ bool IsParameterBlockSetIndependent(const set<double*>& independent_set) const;
+
+ // Create a TripletSparseMatrix which contains the zero-one
+ // structure corresponding to the block sparsity of the transpose of
+ // the Jacobian matrix.
+ //
+ // Caller owns the result.
+ TripletSparseMatrix* CreateJacobianBlockSparsityTranspose() const;
+
+ // Create a copy of this program and removes constant parameter
+ // blocks and residual blocks with no varying parameter blocks while
+ // preserving their relative order.
+ //
+ // removed_parameter_blocks on exit will contain the list of
+ // parameter blocks that were removed.
+ //
+ // fixed_cost will be equal to the sum of the costs of the residual
+ // blocks that were removed.
+ //
+ // If there was a problem, then the function will return a NULL
+ // pointer and error will contain a human readable description of
+ // the problem.
+ Program* CreateReducedProgram(vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* error) const;
+
// See problem.h for what these do.
int NumParameterBlocks() const;
int NumParameters() const;
@@ -116,6 +163,21 @@
string ToString() const;
private:
+ // Remove constant parameter blocks and residual blocks with no
+ // varying parameter blocks while preserving their relative order.
+ //
+ // removed_parameter_blocks on exit will contain the list of
+ // parameter blocks that were removed.
+ //
+ // fixed_cost will be equal to the sum of the costs of the residual
+ // blocks that were removed.
+ //
+ // If there was a problem, then the function will return false and
+ // error will contain a human readable description of the problem.
+ bool RemoveFixedBlocks(vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* message);
+
// The Program does not own the ParameterBlock or ResidualBlock objects.
vector<ParameterBlock*> parameter_blocks_;
vector<ResidualBlock*> residual_blocks_;
diff --git a/internal/ceres/program_evaluator.h b/internal/ceres/program_evaluator.h
index 8aa2a39..672c233 100644
--- a/internal/ceres/program_evaluator.h
+++ b/internal/ceres/program_evaluator.h
@@ -79,6 +79,9 @@
#ifndef CERES_INTERNAL_PROGRAM_EVALUATOR_H_
#define CERES_INTERNAL_PROGRAM_EVALUATOR_H_
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifdef CERES_USE_OPENMP
#include <omp.h>
#endif
@@ -97,7 +100,13 @@
namespace ceres {
namespace internal {
-template<typename EvaluatePreparer, typename JacobianWriter>
+struct NullJacobianFinalizer {
+ void operator()(SparseMatrix* jacobian, int num_parameters) {}
+};
+
+template<typename EvaluatePreparer,
+ typename JacobianWriter,
+ typename JacobianFinalizer = NullJacobianFinalizer>
class ProgramEvaluator : public Evaluator {
public:
ProgramEvaluator(const Evaluator::Options &options, Program* program)
@@ -244,9 +253,10 @@
}
if (!abort) {
+ const int num_parameters = program_->NumEffectiveParameters();
+
// Sum the cost and gradient (if requested) from each thread.
(*cost) = 0.0;
- int num_parameters = program_->NumEffectiveParameters();
if (gradient != NULL) {
VectorRef(gradient, num_parameters).setZero();
}
@@ -257,6 +267,15 @@
VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters);
}
}
+
+ // Finalize the Jacobian if it is available.
+ // `num_parameters` is passed to the finalizer so that additional
+ // storage can be reserved for additional diagonal elements if
+ // necessary.
+ if (jacobian != NULL) {
+ JacobianFinalizer f;
+ f(jacobian, num_parameters);
+ }
}
return !abort;
}
diff --git a/internal/ceres/program_test.cc b/internal/ceres/program_test.cc
new file mode 100644
index 0000000..10bfa12
--- /dev/null
+++ b/internal/ceres/program_test.cc
@@ -0,0 +1,431 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/program.h"
+
+#include <limits>
+#include <cmath>
+#include <vector>
+#include "ceres/sized_cost_function.h"
+#include "ceres/problem_impl.h"
+#include "ceres/residual_block.h"
+#include "ceres/triplet_sparse_matrix.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+// A cost function that simply returns its argument.
+class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ residuals[0] = parameters[0][0];
+ if (jacobians != NULL && jacobians[0] != NULL) {
+ jacobians[0][0] = 1.0;
+ }
+ return true;
+ }
+};
+
+// Templated base class for the CostFunction signatures.
+template <int kNumResiduals, int N0, int N1, int N2>
+class MockCostFunctionBase : public
+SizedCostFunction<kNumResiduals, N0, N1, N2> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ for (int i = 0; i < kNumResiduals; ++i) {
+ residuals[i] = kNumResiduals + N0 + N1 + N2;
+ }
+ return true;
+ }
+};
+
+class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
+class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
+class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
+
+TEST(Program, RemoveFixedBlocksNothingConstant) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
+
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ scoped_ptr<Program> reduced_program(
+ CHECK_NOTNULL(problem
+ .program()
+ .CreateReducedProgram(&removed_parameter_blocks,
+ &fixed_cost,
+ &message)));
+
+ EXPECT_EQ(reduced_program->NumParameterBlocks(), 3);
+ EXPECT_EQ(reduced_program->NumResidualBlocks(), 3);
+ EXPECT_EQ(removed_parameter_blocks.size(), 0);
+ EXPECT_EQ(fixed_cost, 0.0);
+}
+
+TEST(Program, RemoveFixedBlocksAllParameterBlocksConstant) {
+ ProblemImpl problem;
+ double x = 1.0;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.SetParameterBlockConstant(&x);
+
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ scoped_ptr<Program> reduced_program(
+ CHECK_NOTNULL(problem
+ .program()
+ .CreateReducedProgram(&removed_parameter_blocks,
+ &fixed_cost,
+ &message)));
+ EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
+ EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
+ EXPECT_EQ(removed_parameter_blocks.size(), 1);
+ EXPECT_EQ(removed_parameter_blocks[0], &x);
+ EXPECT_EQ(fixed_cost, 9.0);
+}
+
+
+TEST(Program, RemoveFixedBlocksNoResidualBlocks) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ scoped_ptr<Program> reduced_program(
+ CHECK_NOTNULL(problem
+ .program()
+ .CreateReducedProgram(&removed_parameter_blocks,
+ &fixed_cost,
+ &message)));
+ EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
+ EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
+ EXPECT_EQ(removed_parameter_blocks.size(), 3);
+ EXPECT_EQ(fixed_cost, 0.0);
+}
+
+TEST(Program, RemoveFixedBlocksOneParameterBlockConstant) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.SetParameterBlockConstant(&x);
+
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ scoped_ptr<Program> reduced_program(
+ CHECK_NOTNULL(problem
+ .program()
+ .CreateReducedProgram(&removed_parameter_blocks,
+ &fixed_cost,
+ &message)));
+ EXPECT_EQ(reduced_program->NumParameterBlocks(), 1);
+ EXPECT_EQ(reduced_program->NumResidualBlocks(), 1);
+}
+
+TEST(Program, RemoveFixedBlocksNumEliminateBlocks) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.SetParameterBlockConstant(&x);
+
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ scoped_ptr<Program> reduced_program(
+ CHECK_NOTNULL(problem
+ .program()
+ .CreateReducedProgram(&removed_parameter_blocks,
+ &fixed_cost,
+ &message)));
+ EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
+ EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
+}
+
+TEST(Program, RemoveFixedBlocksFixedCost) {
+ ProblemImpl problem;
+ double x = 1.23;
+ double y = 4.56;
+ double z = 7.89;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+ problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.SetParameterBlockConstant(&x);
+
+ ResidualBlock *expected_removed_block = problem.program().residual_blocks()[0];
+ scoped_array<double> scratch(
+ new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
+ double expected_fixed_cost;
+ expected_removed_block->Evaluate(true,
+ &expected_fixed_cost,
+ NULL,
+ NULL,
+ scratch.get());
+
+
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ scoped_ptr<Program> reduced_program(
+ CHECK_NOTNULL(problem
+ .program()
+ .CreateReducedProgram(&removed_parameter_blocks,
+ &fixed_cost,
+ &message)));
+
+ EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
+ EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
+ EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
+}
+
+TEST(Program, CreateJacobianBlockSparsityTranspose) {
+ ProblemImpl problem;
+ double x[2];
+ double y[3];
+ double z;
+
+ problem.AddParameterBlock(x, 2);
+ problem.AddParameterBlock(y, 3);
+ problem.AddParameterBlock(&z, 1);
+
+ problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 0, 0>(), NULL, x);
+ problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2, 0>(), NULL, &z, x);
+ problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3, 0>(), NULL, &z, y);
+ problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3, 0>(), NULL, &z, y);
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1, 0>(), NULL, x, &z);
+ problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3, 0>(), NULL, &z, y);
+ problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1, 0>(), NULL, x, &z);
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 3, 0, 0>(), NULL, y);
+
+ TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
+ {
+ int* rows = expected_block_sparse_jacobian.mutable_rows();
+ int* cols = expected_block_sparse_jacobian.mutable_cols();
+ double* values = expected_block_sparse_jacobian.mutable_values();
+ rows[0] = 0;
+ cols[0] = 0;
+
+ rows[1] = 2;
+ cols[1] = 1;
+ rows[2] = 0;
+ cols[2] = 1;
+
+ rows[3] = 2;
+ cols[3] = 2;
+ rows[4] = 1;
+ cols[4] = 2;
+
+ rows[5] = 2;
+ cols[5] = 3;
+ rows[6] = 1;
+ cols[6] = 3;
+
+ rows[7] = 0;
+ cols[7] = 4;
+ rows[8] = 2;
+ cols[8] = 4;
+
+ rows[9] = 2;
+ cols[9] = 5;
+ rows[10] = 1;
+ cols[10] = 5;
+
+ rows[11] = 0;
+ cols[11] = 6;
+ rows[12] = 2;
+ cols[12] = 6;
+
+ rows[13] = 1;
+ cols[13] = 7;
+ fill(values, values + 14, 1.0);
+ expected_block_sparse_jacobian.set_num_nonzeros(14);
+ }
+
+ Program* program = problem.mutable_program();
+ program->SetParameterOffsetsAndIndex();
+
+ scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
+ program->CreateJacobianBlockSparsityTranspose());
+
+ Matrix expected_dense_jacobian;
+ expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
+
+ Matrix actual_dense_jacobian;
+ actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
+ EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
+}
+
+template <int kNumResiduals, int kNumParameterBlocks>
+class NumParameterBlocksCostFunction : public CostFunction {
+ public:
+ NumParameterBlocksCostFunction() {
+ set_num_residuals(kNumResiduals);
+ for (int i = 0; i < kNumParameterBlocks; ++i) {
+ mutable_parameter_block_sizes()->push_back(1);
+ }
+ }
+
+ virtual ~NumParameterBlocksCostFunction() {
+ }
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ return true;
+ }
+};
+
+TEST(Program, ReallocationInCreateJacobianBlockSparsityTranspose) {
+ // CreateJacobianBlockSparsityTranspose starts with a conservative
+ // estimate of the size of the sparsity pattern. This test ensures
+ // that when those estimates are violated, the reallocation/resizing
+ // logic works correctly.
+
+ ProblemImpl problem;
+ double x[20];
+
+ vector<double*> parameter_blocks;
+ for (int i = 0; i < 20; ++i) {
+ problem.AddParameterBlock(x + i, 1);
+ parameter_blocks.push_back(x + i);
+ }
+
+ problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
+ NULL,
+ parameter_blocks);
+
+ TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
+ {
+ int* rows = expected_block_sparse_jacobian.mutable_rows();
+ int* cols = expected_block_sparse_jacobian.mutable_cols();
+ for (int i = 0; i < 20; ++i) {
+ rows[i] = i;
+ cols[i] = 0;
+ }
+
+ double* values = expected_block_sparse_jacobian.mutable_values();
+ fill(values, values + 20, 1.0);
+ expected_block_sparse_jacobian.set_num_nonzeros(20);
+ }
+
+ Program* program = problem.mutable_program();
+ program->SetParameterOffsetsAndIndex();
+
+ scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
+ program->CreateJacobianBlockSparsityTranspose());
+
+ Matrix expected_dense_jacobian;
+ expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
+
+ Matrix actual_dense_jacobian;
+ actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
+ EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
+}
+
+TEST(Program, ProblemHasNanParameterBlocks) {
+ ProblemImpl problem;
+ double x[2];
+ x[0] = 1.0;
+ x[1] = std::numeric_limits<double>::quiet_NaN();
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
+ string error;
+ EXPECT_FALSE(problem.program().ParameterBlocksAreFinite(&error));
+ EXPECT_NE(error.find("has at least one invalid value"),
+ string::npos) << error;
+}
+
+TEST(Program, InfeasibleParameterBlock) {
+ ProblemImpl problem;
+ double x[] = {0.0, 0.0};
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
+ problem.SetParameterLowerBound(x, 0, 2.0);
+ problem.SetParameterUpperBound(x, 0, 1.0);
+ string error;
+ EXPECT_FALSE(problem.program().IsFeasible(&error));
+ EXPECT_NE(error.find("infeasible bound"), string::npos) << error;
+}
+
+TEST(Program, InfeasibleConstantParameterBlock) {
+ ProblemImpl problem;
+ double x[] = {0.0, 0.0};
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
+ problem.SetParameterLowerBound(x, 0, 1.0);
+ problem.SetParameterUpperBound(x, 0, 2.0);
+ problem.SetParameterBlockConstant(x);
+ string error;
+ EXPECT_FALSE(problem.program().IsFeasible(&error));
+ EXPECT_NE(error.find("infeasible value"), string::npos) << error;
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/reorder_program.cc b/internal/ceres/reorder_program.cc
new file mode 100644
index 0000000..162bfb8
--- /dev/null
+++ b/internal/ceres/reorder_program.cc
@@ -0,0 +1,434 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/reorder_program.h"
+
+#include <algorithm>
+#include <numeric>
+#include <vector>
+
+#include "ceres/cxsparse.h"
+#include "ceres/internal/port.h"
+#include "ceres/ordered_groups.h"
+#include "ceres/parameter_block.h"
+#include "ceres/parameter_block_ordering.h"
+#include "ceres/problem_impl.h"
+#include "ceres/program.h"
+#include "ceres/program.h"
+#include "ceres/residual_block.h"
+#include "ceres/solver.h"
+#include "ceres/suitesparse.h"
+#include "ceres/triplet_sparse_matrix.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+namespace {
+
+// Find the minimum index of any parameter block to the given residual.
+// Parameter blocks that have indices greater than num_eliminate_blocks are
+// considered to have an index equal to num_eliminate_blocks.
+static int MinParameterBlock(const ResidualBlock* residual_block,
+ int num_eliminate_blocks) {
+ int min_parameter_block_position = num_eliminate_blocks;
+ for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) {
+ ParameterBlock* parameter_block = residual_block->parameter_blocks()[i];
+ if (!parameter_block->IsConstant()) {
+ CHECK_NE(parameter_block->index(), -1)
+ << "Did you forget to call Program::SetParameterOffsetsAndIndex()? "
+ << "This is a Ceres bug; please contact the developers!";
+ min_parameter_block_position = std::min(parameter_block->index(),
+ min_parameter_block_position);
+ }
+ }
+ return min_parameter_block_position;
+}
+
+void OrderingForSparseNormalCholeskyUsingSuiteSparse(
+ const TripletSparseMatrix& tsm_block_jacobian_transpose,
+ const vector<ParameterBlock*>& parameter_blocks,
+ const ParameterBlockOrdering& parameter_block_ordering,
+ int* ordering) {
+#ifdef CERES_NO_SUITESPARSE
+ LOG(FATAL) << "Congratulations, you found a Ceres bug! "
+ << "Please report this error to the developers.";
+#else
+ SuiteSparse ss;
+ cholmod_sparse* block_jacobian_transpose =
+ ss.CreateSparseMatrix(
+ const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose));
+
+ // No CAMD or the user did not supply a useful ordering, then just
+ // use regular AMD.
+ if (parameter_block_ordering.NumGroups() <= 1 ||
+ !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) {
+ ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]);
+ } else {
+ vector<int> constraints;
+ for (int i = 0; i < parameter_blocks.size(); ++i) {
+ constraints.push_back(
+ parameter_block_ordering.GroupId(
+ parameter_blocks[i]->mutable_user_state()));
+ }
+ ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose,
+ &constraints[0],
+ ordering);
+ }
+
+ ss.Free(block_jacobian_transpose);
+#endif // CERES_NO_SUITESPARSE
+}
+
+void OrderingForSparseNormalCholeskyUsingCXSparse(
+ const TripletSparseMatrix& tsm_block_jacobian_transpose,
+ int* ordering) {
+#ifdef CERES_NO_CXSPARSE
+ LOG(FATAL) << "Congratulations, you found a Ceres bug! "
+ << "Please report this error to the developers.";
+#else // CERES_NO_CXSPARSE
+ // CXSparse works with J'J instead of J'. So compute the block
+ // sparsity for J'J and compute an approximate minimum degree
+ // ordering.
+ CXSparse cxsparse;
+ cs_di* block_jacobian_transpose;
+ block_jacobian_transpose =
+ cxsparse.CreateSparseMatrix(
+ const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose));
+ cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose);
+ cs_di* block_hessian =
+ cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian);
+ cxsparse.Free(block_jacobian);
+ cxsparse.Free(block_jacobian_transpose);
+
+ cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, ordering);
+ cxsparse.Free(block_hessian);
+#endif // CERES_NO_CXSPARSE
+}
+
+} // namespace
+
+bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map,
+ const ParameterBlockOrdering& ordering,
+ Program* program,
+ string* error) {
+ const int num_parameter_blocks = program->NumParameterBlocks();
+ if (ordering.NumElements() != num_parameter_blocks) {
+ *error = StringPrintf("User specified ordering does not have the same "
+ "number of parameters as the problem. The problem"
+ "has %d blocks while the ordering has %d blocks.",
+ num_parameter_blocks,
+ ordering.NumElements());
+ return false;
+ }
+
+ vector<ParameterBlock*>* parameter_blocks =
+ program->mutable_parameter_blocks();
+ parameter_blocks->clear();
+
+ const map<int, set<double*> >& groups =
+ ordering.group_to_elements();
+
+ for (map<int, set<double*> >::const_iterator group_it = groups.begin();
+ group_it != groups.end();
+ ++group_it) {
+ const set<double*>& group = group_it->second;
+ for (set<double*>::const_iterator parameter_block_ptr_it = group.begin();
+ parameter_block_ptr_it != group.end();
+ ++parameter_block_ptr_it) {
+ ProblemImpl::ParameterMap::const_iterator parameter_block_it =
+ parameter_map.find(*parameter_block_ptr_it);
+ if (parameter_block_it == parameter_map.end()) {
+ *error = StringPrintf("User specified ordering contains a pointer "
+ "to a double that is not a parameter block in "
+ "the problem. The invalid double is in group: %d",
+ group_it->first);
+ return false;
+ }
+ parameter_blocks->push_back(parameter_block_it->second);
+ }
+ }
+ return true;
+}
+
+bool LexicographicallyOrderResidualBlocks(const int num_eliminate_blocks,
+ Program* program,
+ string* error) {
+ CHECK_GE(num_eliminate_blocks, 1)
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
+
+ // Create a histogram of the number of residuals for each E block. There is an
+ // extra bucket at the end to catch all non-eliminated F blocks.
+ vector<int> residual_blocks_per_e_block(num_eliminate_blocks + 1);
+ vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks();
+ vector<int> min_position_per_residual(residual_blocks->size());
+ for (int i = 0; i < residual_blocks->size(); ++i) {
+ ResidualBlock* residual_block = (*residual_blocks)[i];
+ int position = MinParameterBlock(residual_block, num_eliminate_blocks);
+ min_position_per_residual[i] = position;
+ DCHECK_LE(position, num_eliminate_blocks);
+ residual_blocks_per_e_block[position]++;
+ }
+
+ // Run a cumulative sum on the histogram, to obtain offsets to the start of
+ // each histogram bucket (where each bucket is for the residuals for that
+ // E-block).
+ vector<int> offsets(num_eliminate_blocks + 1);
+ std::partial_sum(residual_blocks_per_e_block.begin(),
+ residual_blocks_per_e_block.end(),
+ offsets.begin());
+ CHECK_EQ(offsets.back(), residual_blocks->size())
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
+
+ CHECK(find(residual_blocks_per_e_block.begin(),
+ residual_blocks_per_e_block.end() - 1, 0) !=
+ residual_blocks_per_e_block.end())
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
+
+ // Fill in each bucket with the residual blocks for its corresponding E block.
+ // Each bucket is individually filled from the back of the bucket to the front
+ // of the bucket. The filling order among the buckets is dictated by the
+ // residual blocks. This loop uses the offsets as counters; subtracting one
+ // from each offset as a residual block is placed in the bucket. When the
+ // filling is finished, the offset pointerts should have shifted down one
+ // entry (this is verified below).
+ vector<ResidualBlock*> reordered_residual_blocks(
+ (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL));
+ for (int i = 0; i < residual_blocks->size(); ++i) {
+ int bucket = min_position_per_residual[i];
+
+ // Decrement the cursor, which should now point at the next empty position.
+ offsets[bucket]--;
+
+ // Sanity.
+ CHECK(reordered_residual_blocks[offsets[bucket]] == NULL)
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
+
+ reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i];
+ }
+
+ // Sanity check #1: The difference in bucket offsets should match the
+ // histogram sizes.
+ for (int i = 0; i < num_eliminate_blocks; ++i) {
+ CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i])
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
+ }
+ // Sanity check #2: No NULL's left behind.
+ for (int i = 0; i < reordered_residual_blocks.size(); ++i) {
+ CHECK(reordered_residual_blocks[i] != NULL)
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
+ }
+
+ // Now that the residuals are collected by E block, swap them in place.
+ swap(*program->mutable_residual_blocks(), reordered_residual_blocks);
+ return true;
+}
+
+void MaybeReorderSchurComplementColumnsUsingSuiteSparse(
+ const ParameterBlockOrdering& parameter_block_ordering,
+ Program* program) {
+ // Pre-order the columns corresponding to the schur complement if
+ // possible.
+#ifndef CERES_NO_SUITESPARSE
+ SuiteSparse ss;
+ if (!SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) {
+ return;
+ }
+
+ vector<int> constraints;
+ vector<ParameterBlock*>& parameter_blocks =
+ *(program->mutable_parameter_blocks());
+
+ for (int i = 0; i < parameter_blocks.size(); ++i) {
+ constraints.push_back(
+ parameter_block_ordering.GroupId(
+ parameter_blocks[i]->mutable_user_state()));
+ }
+
+ // Renumber the entries of constraints to be contiguous integers
+ // as camd requires that the group ids be in the range [0,
+ // parameter_blocks.size() - 1].
+ MapValuesToContiguousRange(constraints.size(), &constraints[0]);
+
+ // Set the offsets and index for CreateJacobianSparsityTranspose.
+ program->SetParameterOffsetsAndIndex();
+ // Compute a block sparse presentation of J'.
+ scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
+ program->CreateJacobianBlockSparsityTranspose());
+
+
+ cholmod_sparse* block_jacobian_transpose =
+ ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get());
+
+ vector<int> ordering(parameter_blocks.size(), 0);
+ ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose,
+ &constraints[0],
+ &ordering[0]);
+ ss.Free(block_jacobian_transpose);
+
+ const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
+ for (int i = 0; i < program->NumParameterBlocks(); ++i) {
+ parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
+ }
+#endif
+}
+
+bool ReorderProgramForSchurTypeLinearSolver(
+ const LinearSolverType linear_solver_type,
+ const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
+ const ProblemImpl::ParameterMap& parameter_map,
+ ParameterBlockOrdering* parameter_block_ordering,
+ Program* program,
+ string* error) {
+ if (parameter_block_ordering->NumGroups() == 1) {
+ // If the user supplied an parameter_block_ordering with just one
+ // group, it is equivalent to the user supplying NULL as an
+ // parameter_block_ordering. Ceres is completely free to choose the
+ // parameter block ordering as it sees fit. For Schur type solvers,
+ // this means that the user wishes for Ceres to identify the
+ // e_blocks, which we do by computing a maximal independent set.
+ vector<ParameterBlock*> schur_ordering;
+ const int num_eliminate_blocks =
+ ComputeStableSchurOrdering(*program, &schur_ordering);
+
+ CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks())
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
+
+ // Update the parameter_block_ordering object.
+ for (int i = 0; i < schur_ordering.size(); ++i) {
+ double* parameter_block = schur_ordering[i]->mutable_user_state();
+ const int group_id = (i < num_eliminate_blocks) ? 0 : 1;
+ parameter_block_ordering->AddElementToGroup(parameter_block, group_id);
+ }
+
+ // We could call ApplyOrdering but this is cheaper and
+ // simpler.
+ swap(*program->mutable_parameter_blocks(), schur_ordering);
+ } else {
+ // The user provided an ordering with more than one elimination
+ // group. Trust the user and apply the ordering.
+ if (!ApplyOrdering(parameter_map,
+ *parameter_block_ordering,
+ program,
+ error)) {
+ return false;
+ }
+ }
+
+ if (linear_solver_type == SPARSE_SCHUR &&
+ sparse_linear_algebra_library_type == SUITE_SPARSE) {
+ MaybeReorderSchurComplementColumnsUsingSuiteSparse(
+ *parameter_block_ordering,
+ program);
+ }
+
+ program->SetParameterOffsetsAndIndex();
+ // Schur type solvers also require that their residual blocks be
+ // lexicographically ordered.
+ const int num_eliminate_blocks =
+ parameter_block_ordering->group_to_elements().begin()->second.size();
+ if (!LexicographicallyOrderResidualBlocks(num_eliminate_blocks,
+ program,
+ error)) {
+ return false;
+ }
+
+ program->SetParameterOffsetsAndIndex();
+ return true;
+}
+
+bool ReorderProgramForSparseNormalCholesky(
+ const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
+ const ParameterBlockOrdering& parameter_block_ordering,
+ Program* program,
+ string* error) {
+
+ if (sparse_linear_algebra_library_type != SUITE_SPARSE &&
+ sparse_linear_algebra_library_type != CX_SPARSE &&
+ sparse_linear_algebra_library_type != EIGEN_SPARSE) {
+ *error = "Unknown sparse linear algebra library.";
+ return false;
+ }
+
+ // For Eigen, there is nothing to do. This is because Eigen in its
+ // current stable version does not expose a method for doing
+ // symbolic analysis on pre-ordered matrices, so a block
+ // pre-ordering is a bit pointless.
+ //
+ // The dev version as recently as July 20, 2014 has support for
+ // pre-ordering. Once this becomes more widespread, or we add
+ // support for detecting Eigen versions, we can add support for this
+ // along the lines of CXSparse.
+ if (sparse_linear_algebra_library_type == EIGEN_SPARSE) {
+ program->SetParameterOffsetsAndIndex();
+ return true;
+ }
+
+ // Set the offsets and index for CreateJacobianSparsityTranspose.
+ program->SetParameterOffsetsAndIndex();
+ // Compute a block sparse presentation of J'.
+ scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
+ program->CreateJacobianBlockSparsityTranspose());
+
+ vector<int> ordering(program->NumParameterBlocks(), 0);
+ vector<ParameterBlock*>& parameter_blocks =
+ *(program->mutable_parameter_blocks());
+
+ if (sparse_linear_algebra_library_type == SUITE_SPARSE) {
+ OrderingForSparseNormalCholeskyUsingSuiteSparse(
+ *tsm_block_jacobian_transpose,
+ parameter_blocks,
+ parameter_block_ordering,
+ &ordering[0]);
+ } else if (sparse_linear_algebra_library_type == CX_SPARSE){
+ OrderingForSparseNormalCholeskyUsingCXSparse(
+ *tsm_block_jacobian_transpose,
+ &ordering[0]);
+ }
+
+ // Apply ordering.
+ const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
+ for (int i = 0; i < program->NumParameterBlocks(); ++i) {
+ parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
+ }
+
+ program->SetParameterOffsetsAndIndex();
+ return true;
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/reorder_program.h b/internal/ceres/reorder_program.h
new file mode 100644
index 0000000..d3962f9
--- /dev/null
+++ b/internal/ceres/reorder_program.h
@@ -0,0 +1,101 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_INTERNAL_REORDER_PROGRAM_H_
+#define CERES_INTERNAL_REORDER_PROGRAM_H_
+
+#include <string>
+#include "ceres/internal/port.h"
+#include "ceres/parameter_block_ordering.h"
+#include "ceres/problem_impl.h"
+#include "ceres/types.h"
+
+namespace ceres {
+namespace internal {
+
+class Program;
+
+// Reorder the parameter blocks in program using the ordering
+bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map,
+ const ParameterBlockOrdering& ordering,
+ Program* program,
+ string* error);
+
+// Reorder the residuals for program, if necessary, so that the residuals
+// involving each E block occur together. This is a necessary condition for the
+// Schur eliminator, which works on these "row blocks" in the jacobian.
+bool LexicographicallyOrderResidualBlocks(int num_eliminate_blocks,
+ Program* program,
+ string* error);
+
+// Schur type solvers require that all parameter blocks eliminated
+// by the Schur eliminator occur before others and the residuals be
+// sorted in lexicographic order of their parameter blocks.
+//
+// If the parameter_block_ordering only contains one elimination
+// group then a maximal independent set is computed and used as the
+// first elimination group, otherwise the user's ordering is used.
+//
+// If the linear solver type is SPARSE_SCHUR and support for
+// constrained fill-reducing ordering is available in the sparse
+// linear algebra library (SuiteSparse version >= 4.2.0) then
+// columns of the schur complement matrix are ordered to reduce the
+// fill-in the Cholesky factorization.
+//
+// Upon return, ordering contains the parameter block ordering that
+// was used to order the program.
+bool ReorderProgramForSchurTypeLinearSolver(
+ LinearSolverType linear_solver_type,
+ SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
+ const ProblemImpl::ParameterMap& parameter_map,
+ ParameterBlockOrdering* parameter_block_ordering,
+ Program* program,
+ string* error);
+
+// Sparse cholesky factorization routines when doing the sparse
+// cholesky factorization of the Jacobian matrix, reorders its
+// columns to reduce the fill-in. Compute this permutation and
+// re-order the parameter blocks.
+//
+// When using SuiteSparse, if the parameter_block_ordering contains
+// more than one elimination group and support for constrained
+// fill-reducing ordering is available in the sparse linear algebra
+// library (SuiteSparse version >= 4.2.0) then the fill reducing
+// ordering will take it into account, otherwise it will be ignored.
+bool ReorderProgramForSparseNormalCholesky(
+ SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
+ const ParameterBlockOrdering& parameter_block_ordering,
+ Program* program,
+ string* error);
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_INTERNAL_REORDER_PROGRAM_
diff --git a/internal/ceres/reorder_program_test.cc b/internal/ceres/reorder_program_test.cc
new file mode 100644
index 0000000..2a0c4eb
--- /dev/null
+++ b/internal/ceres/reorder_program_test.cc
@@ -0,0 +1,170 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/reorder_program.h"
+
+#include "ceres/parameter_block.h"
+#include "ceres/problem_impl.h"
+#include "ceres/program.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/solver.h"
+
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+// Templated base class for the CostFunction signatures.
+template <int kNumResiduals, int N0, int N1, int N2>
+class MockCostFunctionBase : public
+SizedCostFunction<kNumResiduals, N0, N1, N2> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ // Do nothing. This is never called.
+ return true;
+ }
+};
+
+class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
+class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
+class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
+
+TEST(_, ReorderResidualBlockNormalFunction) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);
+
+ ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering;
+ linear_solver_ordering->AddElementToGroup(&x, 0);
+ linear_solver_ordering->AddElementToGroup(&y, 0);
+ linear_solver_ordering->AddElementToGroup(&z, 1);
+
+ Solver::Options options;
+ options.linear_solver_type = DENSE_SCHUR;
+ options.linear_solver_ordering.reset(linear_solver_ordering);
+
+ const vector<ResidualBlock*>& residual_blocks =
+ problem.program().residual_blocks();
+
+ vector<ResidualBlock*> expected_residual_blocks;
+
+ // This is a bit fragile, but it serves the purpose. We know the
+ // bucketing algorithm that the reordering function uses, so we
+ // expect the order for residual blocks for each e_block to be
+ // filled in reverse.
+ expected_residual_blocks.push_back(residual_blocks[4]);
+ expected_residual_blocks.push_back(residual_blocks[1]);
+ expected_residual_blocks.push_back(residual_blocks[0]);
+ expected_residual_blocks.push_back(residual_blocks[5]);
+ expected_residual_blocks.push_back(residual_blocks[2]);
+ expected_residual_blocks.push_back(residual_blocks[3]);
+
+ Program* program = problem.mutable_program();
+ program->SetParameterOffsetsAndIndex();
+
+ string message;
+ EXPECT_TRUE(LexicographicallyOrderResidualBlocks(
+ 2,
+ problem.mutable_program(),
+ &message));
+ EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size());
+ for (int i = 0; i < expected_residual_blocks.size(); ++i) {
+ EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]);
+ }
+}
+
+TEST(_, ApplyOrderingOrderingTooSmall) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+
+ ParameterBlockOrdering linear_solver_ordering;
+ linear_solver_ordering.AddElementToGroup(&x, 0);
+ linear_solver_ordering.AddElementToGroup(&y, 1);
+
+ Program program(problem.program());
+ string message;
+ EXPECT_FALSE(ApplyOrdering(problem.parameter_map(),
+ linear_solver_ordering,
+ &program,
+ &message));
+}
+
+TEST(_, ApplyOrderingNormal) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+
+ ParameterBlockOrdering linear_solver_ordering;
+ linear_solver_ordering.AddElementToGroup(&x, 0);
+ linear_solver_ordering.AddElementToGroup(&y, 2);
+ linear_solver_ordering.AddElementToGroup(&z, 1);
+
+ Program* program = problem.mutable_program();
+ string message;
+
+ EXPECT_TRUE(ApplyOrdering(problem.parameter_map(),
+ linear_solver_ordering,
+ program,
+ &message));
+ const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
+
+ EXPECT_EQ(parameter_blocks.size(), 3);
+ EXPECT_EQ(parameter_blocks[0]->user_state(), &x);
+ EXPECT_EQ(parameter_blocks[1]->user_state(), &z);
+ EXPECT_EQ(parameter_blocks[2]->user_state(), &y);
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/residual_block_test.cc b/internal/ceres/residual_block_test.cc
index 1e03e7d..b37f50f 100644
--- a/internal/ceres/residual_block_test.cc
+++ b/internal/ceres/residual_block_test.cc
@@ -43,9 +43,9 @@
class TernaryCostFunction: public CostFunction {
public:
TernaryCostFunction(int num_residuals,
- int16 parameter_block1_size,
- int16 parameter_block2_size,
- int16 parameter_block3_size) {
+ int32 parameter_block1_size,
+ int32 parameter_block2_size,
+ int32 parameter_block3_size) {
set_num_residuals(num_residuals);
mutable_parameter_block_sizes()->push_back(parameter_block1_size);
mutable_parameter_block_sizes()->push_back(parameter_block2_size);
diff --git a/internal/ceres/residual_block_utils.cc b/internal/ceres/residual_block_utils.cc
index 4d88a9f..d2564a7 100644
--- a/internal/ceres/residual_block_utils.cc
+++ b/internal/ceres/residual_block_utils.cc
@@ -61,24 +61,6 @@
}
}
-// Utility routine to print an array of doubles to a string. If the
-// array pointer is NULL, it is treated as an array of zeros.
-namespace {
-void AppendArrayToString(const int size, const double* x, string* result) {
- for (int i = 0; i < size; ++i) {
- if (x == NULL) {
- StringAppendF(result, "Not Computed ");
- } else {
- if (x[i] == kImpossibleValue) {
- StringAppendF(result, "Uninitialized ");
- } else {
- StringAppendF(result, "%12g ", x[i]);
- }
- }
- }
-}
-} // namespace
-
string EvaluationToString(const ResidualBlock& block,
double const* const* parameters,
double* cost,
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index 8de1bbd..fab0a7a 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -548,6 +548,41 @@
}
}
+// Takes a bunch of random axis/angle values near zero, converts them
+// to rotation matrices, and back again.
+TEST(Rotation, AngleAxisToRotationMatrixAndBackNearZero) {
+ srand(5);
+ for (int i = 0; i < kNumTrials; i++) {
+ double axis_angle[3];
+ // Make an axis by choosing three random numbers in [-1, 1) and
+ // normalizing.
+ double norm = 0;
+ for (int i = 0; i < 3; i++) {
+ axis_angle[i] = RandDouble() * 2 - 1;
+ norm += axis_angle[i] * axis_angle[i];
+ }
+ norm = sqrt(norm);
+
+ // Tiny theta.
+ double theta = 1e-16 * (kPi * 2 * RandDouble() - kPi);
+ for (int i = 0; i < 3; i++) {
+ axis_angle[i] = axis_angle[i] * theta / norm;
+ }
+
+ double matrix[9];
+ double round_trip[3];
+ AngleAxisToRotationMatrix(axis_angle, matrix);
+ ASSERT_THAT(matrix, IsOrthonormal());
+ RotationMatrixToAngleAxis(matrix, round_trip);
+
+ for (int i = 0; i < 3; ++i) {
+ EXPECT_NEAR(round_trip[i], axis_angle[i],
+ std::numeric_limits<double>::epsilon());
+ }
+ }
+}
+
+
// Transposes a 3x3 matrix.
static void Transpose3x3(double m[9]) {
std::swap(m[1], m[3]);
diff --git a/internal/ceres/runtime_numeric_diff_cost_function.cc b/internal/ceres/runtime_numeric_diff_cost_function.cc
deleted file mode 100644
index 7af275c..0000000
--- a/internal/ceres/runtime_numeric_diff_cost_function.cc
+++ /dev/null
@@ -1,217 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: keir@google.com (Keir Mierle)
-//
-// Based on the templated version in public/numeric_diff_cost_function.h.
-
-#include "ceres/runtime_numeric_diff_cost_function.h"
-
-#include <algorithm>
-#include <numeric>
-#include <vector>
-#include "Eigen/Dense"
-#include "ceres/cost_function.h"
-#include "ceres/internal/scoped_ptr.h"
-#include "glog/logging.h"
-
-namespace ceres {
-namespace internal {
-namespace {
-
-bool EvaluateJacobianForParameterBlock(const CostFunction* function,
- int parameter_block_size,
- int parameter_block,
- RuntimeNumericDiffMethod method,
- double relative_step_size,
- double const* residuals_at_eval_point,
- double** parameters,
- double** jacobians) {
- using Eigen::Map;
- using Eigen::Matrix;
- using Eigen::Dynamic;
- using Eigen::RowMajor;
-
- typedef Matrix<double, Dynamic, 1> ResidualVector;
- typedef Matrix<double, Dynamic, 1> ParameterVector;
- typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix;
-
- int num_residuals = function->num_residuals();
-
- Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block],
- num_residuals,
- parameter_block_size);
-
- // Mutate one element at a time and then restore.
- Map<ParameterVector> x_plus_delta(parameters[parameter_block],
- parameter_block_size);
- ParameterVector x(x_plus_delta);
- ParameterVector step_size = x.array().abs() * relative_step_size;
-
- // To handle cases where a paremeter is exactly zero, instead use the mean
- // step_size for the other dimensions.
- double fallback_step_size = step_size.sum() / step_size.rows();
- if (fallback_step_size == 0.0) {
- // If all the parameters are zero, there's no good answer. Use the given
- // relative step_size as absolute step_size and hope for the best.
- fallback_step_size = relative_step_size;
- }
-
- // For each parameter in the parameter block, use finite differences to
- // compute the derivative for that parameter.
- for (int j = 0; j < parameter_block_size; ++j) {
- if (step_size(j) == 0.0) {
- // The parameter is exactly zero, so compromise and use the mean step_size
- // from the other parameters. This can break in many cases, but it's hard
- // to pick a good number without problem specific knowledge.
- step_size(j) = fallback_step_size;
- }
- x_plus_delta(j) = x(j) + step_size(j);
-
- ResidualVector residuals(num_residuals);
- if (!function->Evaluate(parameters, &residuals[0], NULL)) {
- // Something went wrong; bail.
- return false;
- }
-
- // Compute this column of the jacobian in 3 steps:
- // 1. Store residuals for the forward part.
- // 2. Subtract residuals for the backward (or 0) part.
- // 3. Divide out the run.
- parameter_jacobian.col(j) = residuals;
-
- double one_over_h = 1 / step_size(j);
- if (method == CENTRAL) {
- // Compute the function on the other side of x(j).
- x_plus_delta(j) = x(j) - step_size(j);
-
- if (!function->Evaluate(parameters, &residuals[0], NULL)) {
- // Something went wrong; bail.
- return false;
- }
- parameter_jacobian.col(j) -= residuals;
- one_over_h /= 2;
- } else {
- // Forward difference only; reuse existing residuals evaluation.
- parameter_jacobian.col(j) -=
- Map<const ResidualVector>(residuals_at_eval_point, num_residuals);
- }
- x_plus_delta(j) = x(j); // Restore x_plus_delta.
-
- // Divide out the run to get slope.
- parameter_jacobian.col(j) *= one_over_h;
- }
- return true;
-}
-
-class RuntimeNumericDiffCostFunction : public CostFunction {
- public:
- RuntimeNumericDiffCostFunction(const CostFunction* function,
- RuntimeNumericDiffMethod method,
- double relative_step_size)
- : function_(function),
- method_(method),
- relative_step_size_(relative_step_size) {
- *mutable_parameter_block_sizes() = function->parameter_block_sizes();
- set_num_residuals(function->num_residuals());
- }
-
- virtual ~RuntimeNumericDiffCostFunction() { }
-
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- // Get the function value (residuals) at the the point to evaluate.
- bool success = function_->Evaluate(parameters, residuals, NULL);
- if (!success) {
- // Something went wrong; ignore the jacobian.
- return false;
- }
- if (!jacobians) {
- // Nothing to do; just forward.
- return true;
- }
-
- const vector<int16>& block_sizes = function_->parameter_block_sizes();
- CHECK(!block_sizes.empty());
-
- // Create local space for a copy of the parameters which will get mutated.
- int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0);
- vector<double> parameters_copy(parameters_size);
- vector<double*> parameters_references_copy(block_sizes.size());
- parameters_references_copy[0] = ¶meters_copy[0];
- for (int block = 1; block < block_sizes.size(); ++block) {
- parameters_references_copy[block] = parameters_references_copy[block - 1]
- + block_sizes[block - 1];
- }
-
- // Copy the parameters into the local temp space.
- for (int block = 0; block < block_sizes.size(); ++block) {
- memcpy(parameters_references_copy[block],
- parameters[block],
- block_sizes[block] * sizeof(*parameters[block]));
- }
-
- for (int block = 0; block < block_sizes.size(); ++block) {
- if (!jacobians[block]) {
- // No jacobian requested for this parameter / residual pair.
- continue;
- }
- if (!EvaluateJacobianForParameterBlock(function_,
- block_sizes[block],
- block,
- method_,
- relative_step_size_,
- residuals,
- ¶meters_references_copy[0],
- jacobians)) {
- return false;
- }
- }
- return true;
- }
-
- private:
- const CostFunction* function_;
- RuntimeNumericDiffMethod method_;
- double relative_step_size_;
-};
-
-} // namespace
-
-CostFunction* CreateRuntimeNumericDiffCostFunction(
- const CostFunction* cost_function,
- RuntimeNumericDiffMethod method,
- double relative_step_size) {
- return new RuntimeNumericDiffCostFunction(cost_function,
- method,
- relative_step_size);
-}
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/runtime_numeric_diff_cost_function.h b/internal/ceres/runtime_numeric_diff_cost_function.h
deleted file mode 100644
index 01b57f9..0000000
--- a/internal/ceres/runtime_numeric_diff_cost_function.h
+++ /dev/null
@@ -1,87 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: keir@google.com (Keir Mierle)
-//
-// Create CostFunctions as needed by the least squares framework with jacobians
-// computed via numeric differentiation.
-//
-// To get a numerically differentiated cost function, define a subclass of
-// CostFunction such that the Evaluate() function ignores the jacobian
-// parameter. The numeric differentiation wrapper will fill in the jacobian
-// parameter if nececssary by repeatedly calling the Evaluate() function with
-// small changes to the appropriate parameters, and computing the slope. This
-// implementation is not templated (hence the "Runtime" prefix), which is a bit
-// slower than but is more convenient than the templated version in
-// numeric_diff_cost_function.h
-//
-// The numerically differentiated version of a cost function for a cost function
-// can be constructed as follows:
-//
-// CostFunction* cost_function =
-// CreateRuntimeNumericDiffCostFunction(new MyCostFunction(...),
-// CENTRAL,
-// TAKE_OWNERSHIP);
-//
-// The central difference method is considerably more accurate; consider using
-// to start and only after that works, trying forward difference.
-//
-// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.
-
-#ifndef CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_
-#define CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_
-
-#include "ceres/cost_function.h"
-
-namespace ceres {
-namespace internal {
-
-enum RuntimeNumericDiffMethod {
- CENTRAL,
- FORWARD,
-};
-
-// Create a cost function that evaluates the derivative with finite differences.
-// The base cost_function's implementation of Evaluate() only needs to fill in
-// the "residuals" argument and not the "jacobians". Any data written to the
-// jacobians by the base cost_function is overwritten.
-//
-// Forward difference or central difference is selected with CENTRAL or FORWARD.
-// The relative eps, which determines the step size for forward and central
-// differencing, is set with relative eps. Caller owns the resulting cost
-// function, and the resulting cost function does not own the base cost
-// function.
-CostFunction *CreateRuntimeNumericDiffCostFunction(
- const CostFunction *cost_function,
- RuntimeNumericDiffMethod method,
- double relative_eps);
-
-} // namespace internal
-} // namespace ceres
-
-#endif // CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_
diff --git a/internal/ceres/runtime_numeric_diff_cost_function_test.cc b/internal/ceres/runtime_numeric_diff_cost_function_test.cc
deleted file mode 100644
index 71469ea..0000000
--- a/internal/ceres/runtime_numeric_diff_cost_function_test.cc
+++ /dev/null
@@ -1,222 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: keir@google.com (Keir Mierle)
-//
-// Based on the tests in numeric_diff_cost_function.cc.
-//
-// TODO(keir): See about code duplication.
-
-#include "ceres/runtime_numeric_diff_cost_function.h"
-
-#include <algorithm>
-#include <cmath>
-#include <string>
-#include <vector>
-#include "ceres/cost_function.h"
-#include "ceres/internal/macros.h"
-#include "ceres/internal/scoped_ptr.h"
-#include "ceres/stringprintf.h"
-#include "ceres/test_util.h"
-#include "glog/logging.h"
-#include "gtest/gtest.h"
-
-namespace ceres {
-namespace internal {
-
-const double kRelativeEps = 1e-6;
-
-// y1 = x1'x2 -> dy1/dx1 = x2, dy1/dx2 = x1
-// y2 = (x1'x2)^2 -> dy2/dx1 = 2 * x2 * (x1'x2), dy2/dx2 = 2 * x1 * (x1'x2)
-// y3 = x2'x2 -> dy3/dx1 = 0, dy3/dx2 = 2 * x2
-class TestCostFunction : public CostFunction {
- public:
- TestCostFunction() {
- set_num_residuals(3);
- mutable_parameter_block_sizes()->push_back(5); // x1.
- mutable_parameter_block_sizes()->push_back(5); // x2.
- }
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- (void) jacobians; // Ignored.
-
- residuals[0] = residuals[1] = residuals[2] = 0;
- for (int i = 0; i < 5; ++i) {
- residuals[0] += parameters[0][i] * parameters[1][i];
- residuals[2] += parameters[1][i] * parameters[1][i];
- }
- residuals[1] = residuals[0] * residuals[0];
- return true;
- }
-};
-
-TEST(NumericDiffCostFunction, EasyCase) {
- // Try both central and forward difference.
- TestCostFunction term;
- scoped_ptr<CostFunction> cfs[2];
- cfs[0].reset(
- CreateRuntimeNumericDiffCostFunction(&term, CENTRAL, kRelativeEps));
-
- cfs[1].reset(
- CreateRuntimeNumericDiffCostFunction(&term, FORWARD, kRelativeEps));
-
-
- for (int c = 0; c < 2; ++c) {
- CostFunction *cost_function = cfs[c].get();
-
- double x1[] = { 1.0, 2.0, 3.0, 4.0, 5.0 };
- double x2[] = { 9.0, 9.0, 5.0, 5.0, 1.0 };
- double *parameters[] = { &x1[0], &x2[0] };
-
- double dydx1[15]; // 3 x 5, row major.
- double dydx2[15]; // 3 x 5, row major.
- double *jacobians[2] = { &dydx1[0], &dydx2[0] };
-
- double residuals[3] = {-1e-100, -2e-100, -3e-100 };
-
- ASSERT_TRUE(cost_function->Evaluate(¶meters[0],
- &residuals[0],
- &jacobians[0]));
-
- EXPECT_EQ(residuals[0], 67);
- EXPECT_EQ(residuals[1], 4489);
- EXPECT_EQ(residuals[2], 213);
-
- for (int i = 0; i < 5; ++i) {
- LOG(INFO) << "c = " << c << " i = " << i;
- const double kEps = c == 0 ? /* central */ 3e-9 : /* forward */ 2e-5;
-
- ExpectClose(x2[i], dydx1[5 * 0 + i], kEps); // y1
- ExpectClose(x1[i], dydx2[5 * 0 + i], kEps);
- ExpectClose(2 * x2[i] * residuals[0], dydx1[5 * 1 + i], kEps); // y2
- ExpectClose(2 * x1[i] * residuals[0], dydx2[5 * 1 + i], kEps);
- ExpectClose(0.0, dydx1[5 * 2 + i], kEps); // y3
- ExpectClose(2 * x2[i], dydx2[5 * 2 + i], kEps);
- }
- }
-}
-
-// y1 = sin(x1'x2)
-// y2 = exp(-x1'x2 / 10)
-//
-// dy1/dx1 = x2 * cos(x1'x2), dy1/dx2 = x1 * cos(x1'x2)
-// dy2/dx1 = -x2 * exp(-x1'x2 / 10) / 10, dy2/dx2 = -x2 * exp(-x1'x2 / 10) / 10
-class TranscendentalTestCostFunction : public CostFunction {
- public:
- TranscendentalTestCostFunction() {
- set_num_residuals(2);
- mutable_parameter_block_sizes()->push_back(5); // x1.
- mutable_parameter_block_sizes()->push_back(5); // x2.
- }
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- (void) jacobians; // Ignored.
-
- double x1x2 = 0;
- for (int i = 0; i < 5; ++i) {
- x1x2 += parameters[0][i] * parameters[1][i];
- }
- residuals[0] = sin(x1x2);
- residuals[1] = exp(-x1x2 / 10);
- return true;
- }
-};
-
-TEST(NumericDiffCostFunction, TransendentalOperationsInCostFunction) {
- // Try both central and forward difference.
- TranscendentalTestCostFunction term;
- scoped_ptr<CostFunction> cfs[2];
- cfs[0].reset(
- CreateRuntimeNumericDiffCostFunction(&term, CENTRAL, kRelativeEps));
-
- cfs[1].reset(
- CreateRuntimeNumericDiffCostFunction(&term, FORWARD, kRelativeEps));
-
- for (int c = 0; c < 2; ++c) {
- CostFunction *cost_function = cfs[c].get();
-
- struct {
- double x1[5];
- double x2[5];
- } kTests[] = {
- { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // No zeros.
- { 9.0, 9.0, 5.0, 5.0, 1.0 },
- },
- { { 0.0, 2.0, 3.0, 0.0, 5.0 }, // Some zeros x1.
- { 9.0, 9.0, 5.0, 5.0, 1.0 },
- },
- { { 1.0, 2.0, 3.0, 1.0, 5.0 }, // Some zeros x2.
- { 0.0, 9.0, 0.0, 5.0, 0.0 },
- },
- { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros x1.
- { 9.0, 9.0, 5.0, 5.0, 1.0 },
- },
- { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // All zeros x2.
- { 0.0, 0.0, 0.0, 0.0, 0.0 },
- },
- { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros.
- { 0.0, 0.0, 0.0, 0.0, 0.0 },
- },
- };
- for (int k = 0; k < CERES_ARRAYSIZE(kTests); ++k) {
- double *x1 = &(kTests[k].x1[0]);
- double *x2 = &(kTests[k].x2[0]);
- double *parameters[] = { x1, x2 };
-
- double dydx1[10];
- double dydx2[10];
- double *jacobians[2] = { &dydx1[0], &dydx2[0] };
-
- double residuals[2];
-
- ASSERT_TRUE(cost_function->Evaluate(¶meters[0],
- &residuals[0],
- &jacobians[0]));
- LOG(INFO) << "Ran evaluate for test k=" << k << " c=" << c;
-
- double x1x2 = 0;
- for (int i = 0; i < 5; ++i) {
- x1x2 += x1[i] * x2[i];
- }
-
- for (int i = 0; i < 5; ++i) {
- const double kEps = (c == 0 ? /* central */ 3e-9 : /* forward */ 2e-5);
-
- ExpectClose( x2[i] * cos(x1x2), dydx1[5 * 0 + i], kEps); // NOLINT
- ExpectClose( x1[i] * cos(x1x2), dydx2[5 * 0 + i], kEps); // NOLINT
- ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], kEps);
- ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], kEps);
- }
- }
- }
-}
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/schur_complement_solver.cc b/internal/ceres/schur_complement_solver.cc
index b192aa1..d2aa168 100644
--- a/internal/ceres/schur_complement_solver.cc
+++ b/internal/ceres/schur_complement_solver.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2014 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -28,12 +28,13 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+#include "ceres/internal/port.h"
+
#include <algorithm>
#include <ctime>
#include <set>
#include <vector>
-#include "Eigen/Dense"
#include "ceres/block_random_access_dense_matrix.h"
#include "ceres/block_random_access_matrix.h"
#include "ceres/block_random_access_sparse_matrix.h"
@@ -42,7 +43,6 @@
#include "ceres/cxsparse.h"
#include "ceres/detect_structure.h"
#include "ceres/internal/eigen.h"
-#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/lapack.h"
#include "ceres/linear_solver.h"
@@ -51,6 +51,8 @@
#include "ceres/triplet_sparse_matrix.h"
#include "ceres/types.h"
#include "ceres/wall_time.h"
+#include "Eigen/Dense"
+#include "Eigen/SparseCore"
namespace ceres {
namespace internal {
@@ -75,24 +77,19 @@
fill(x, x + A->num_cols(), 0.0);
event_logger.AddEvent("Setup");
- LinearSolver::Summary summary;
- summary.num_iterations = 1;
- summary.termination_type = FAILURE;
eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get());
event_logger.AddEvent("Eliminate");
double* reduced_solution = x + A->num_cols() - lhs_->num_cols();
- const bool status = SolveReducedLinearSystem(reduced_solution);
+ const LinearSolver::Summary summary =
+ SolveReducedLinearSystem(reduced_solution);
event_logger.AddEvent("ReducedSolve");
- if (!status) {
- return summary;
+ if (summary.termination_type == LINEAR_SOLVER_SUCCESS) {
+ eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x);
+ event_logger.AddEvent("BackSubstitute");
}
- eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x);
- summary.termination_type = TOLERANCE;
-
- event_logger.AddEvent("BackSubstitute");
return summary;
}
@@ -117,7 +114,13 @@
// Solve the system Sx = r, assuming that the matrix S is stored in a
// BlockRandomAccessDenseMatrix. The linear system is solved using
// Eigen's Cholesky factorization.
-bool DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) {
+LinearSolver::Summary
+DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) {
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
+
const BlockRandomAccessDenseMatrix* m =
down_cast<const BlockRandomAccessDenseMatrix*>(lhs());
const int num_rows = m->num_rows();
@@ -125,29 +128,36 @@
// The case where there are no f blocks, and the system is block
// diagonal.
if (num_rows == 0) {
- return true;
+ return summary;
}
+ summary.num_iterations = 1;
+
if (options().dense_linear_algebra_library_type == EIGEN) {
- // TODO(sameeragarwal): Add proper error handling; this completely ignores
- // the quality of the solution to the solve.
- VectorRef(solution, num_rows) =
+ Eigen::LLT<Matrix, Eigen::Upper> llt =
ConstMatrixRef(m->values(), num_rows, num_rows)
.selfadjointView<Eigen::Upper>()
- .llt()
- .solve(ConstVectorRef(rhs(), num_rows));
- return true;
+ .llt();
+ if (llt.info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message =
+ "Eigen failure. Unable to perform dense Cholesky factorization.";
+ return summary;
+ }
+
+ VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows));
+ } else {
+ VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows);
+ summary.termination_type =
+ LAPACK::SolveInPlaceUsingCholesky(num_rows,
+ m->values(),
+ solution,
+ &summary.message);
}
- VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows);
- const int info = LAPACK::SolveInPlaceUsingCholesky(num_rows,
- m->values(),
- solution);
- return (info == 0);
+ return summary;
}
-#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE)
-
SparseSchurComplementSolver::SparseSchurComplementSolver(
const LinearSolver::Options& options)
: SchurComplementSolver(options),
@@ -156,19 +166,15 @@
}
SparseSchurComplementSolver::~SparseSchurComplementSolver() {
-#ifndef CERES_NO_SUITESPARSE
if (factor_ != NULL) {
ss_.Free(factor_);
factor_ = NULL;
}
-#endif // CERES_NO_SUITESPARSE
-#ifndef CERES_NO_CXSPARSE
if (cxsparse_factor_ != NULL) {
cxsparse_.Free(cxsparse_factor_);
cxsparse_factor_ = NULL;
}
-#endif // CERES_NO_CXSPARSE
}
// Determine the non-zero blocks in the Schur Complement matrix, and
@@ -242,40 +248,57 @@
set_rhs(new double[lhs()->num_rows()]);
}
-bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) {
+LinearSolver::Summary
+SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) {
switch (options().sparse_linear_algebra_library_type) {
case SUITE_SPARSE:
return SolveReducedLinearSystemUsingSuiteSparse(solution);
case CX_SPARSE:
return SolveReducedLinearSystemUsingCXSparse(solution);
+ case EIGEN_SPARSE:
+ return SolveReducedLinearSystemUsingEigen(solution);
default:
LOG(FATAL) << "Unknown sparse linear algebra library : "
<< options().sparse_linear_algebra_library_type;
}
- LOG(FATAL) << "Unknown sparse linear algebra library : "
- << options().sparse_linear_algebra_library_type;
- return false;
+ return LinearSolver::Summary();
}
-#ifndef CERES_NO_SUITESPARSE
// Solve the system Sx = r, assuming that the matrix S is stored in a
// BlockRandomAccessSparseMatrix. The linear system is solved using
// CHOLMOD's sparse cholesky factorization routines.
-bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse(
+LinearSolver::Summary
+SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse(
double* solution) {
+#ifdef CERES_NO_SUITESPARSE
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message = "Ceres was not built with SuiteSparse support. "
+ "Therefore, SPARSE_SCHUR cannot be used with SUITE_SPARSE";
+ return summary;
+
+#else
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
+
TripletSparseMatrix* tsm =
const_cast<TripletSparseMatrix*>(
down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix());
-
const int num_rows = tsm->num_rows();
// The case where there are no f blocks, and the system is block
// diagonal.
if (num_rows == 0) {
- return true;
+ return summary;
}
+ summary.num_iterations = 1;
cholmod_sparse* cholmod_lhs = NULL;
if (options().use_postordering) {
// If we are going to do a full symbolic analysis of the schur
@@ -288,7 +311,10 @@
cholmod_lhs->stype = 1;
if (factor_ == NULL) {
- factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, blocks_, blocks_);
+ factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs,
+ blocks_,
+ blocks_,
+ &summary.message);
}
} else {
// If we are going to use the natural ordering (i.e. rely on the
@@ -301,53 +327,83 @@
cholmod_lhs->stype = -1;
if (factor_ == NULL) {
- factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(cholmod_lhs);
+ factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(cholmod_lhs,
+ &summary.message);
}
}
+ if (factor_ == NULL) {
+ ss_.Free(cholmod_lhs);
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ // No need to set message as it has already been set by the
+ // symbolic analysis routines above.
+ return summary;
+ }
+
+ summary.termination_type =
+ ss_.Cholesky(cholmod_lhs, factor_, &summary.message);
+
+ ss_.Free(cholmod_lhs);
+
+ if (summary.termination_type != LINEAR_SOLVER_SUCCESS) {
+ // No need to set message as it has already been set by the
+ // numeric factorization routine above.
+ return summary;
+ }
+
cholmod_dense* cholmod_rhs =
ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows);
- cholmod_dense* cholmod_solution =
- ss_.SolveCholesky(cholmod_lhs, factor_, cholmod_rhs);
-
- ss_.Free(cholmod_lhs);
+ cholmod_dense* cholmod_solution = ss_.Solve(factor_,
+ cholmod_rhs,
+ &summary.message);
ss_.Free(cholmod_rhs);
if (cholmod_solution == NULL) {
- LOG(WARNING) << "CHOLMOD solve failed.";
- return false;
+ summary.message =
+ "SuiteSparse failure. Unable to perform triangular solve.";
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ return summary;
}
VectorRef(solution, num_rows)
= VectorRef(static_cast<double*>(cholmod_solution->x), num_rows);
ss_.Free(cholmod_solution);
- return true;
-}
-#else
-bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse(
- double* solution) {
- LOG(FATAL) << "No SuiteSparse support in Ceres.";
- return false;
-}
+ return summary;
#endif // CERES_NO_SUITESPARSE
+}
-#ifndef CERES_NO_CXSPARSE
// Solve the system Sx = r, assuming that the matrix S is stored in a
// BlockRandomAccessSparseMatrix. The linear system is solved using
// CXSparse's sparse cholesky factorization routines.
-bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse(
+LinearSolver::Summary
+SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse(
double* solution) {
+#ifdef CERES_NO_CXSPARSE
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message = "Ceres was not built with CXSparse support. "
+ "Therefore, SPARSE_SCHUR cannot be used with CX_SPARSE";
+ return summary;
+
+#else
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
+
// Extract the TripletSparseMatrix that is used for actually storing S.
TripletSparseMatrix* tsm =
const_cast<TripletSparseMatrix*>(
down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix());
-
const int num_rows = tsm->num_rows();
// The case where there are no f blocks, and the system is block
// diagonal.
if (num_rows == 0) {
- return true;
+ return summary;
}
cs_di* lhs = CHECK_NOTNULL(cxsparse_.CreateSparseMatrix(tsm));
@@ -355,24 +411,108 @@
// Compute symbolic factorization if not available.
if (cxsparse_factor_ == NULL) {
- cxsparse_factor_ =
- CHECK_NOTNULL(cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_));
+ cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_);
}
- // Solve the linear system.
- bool ok = cxsparse_.SolveCholesky(lhs, cxsparse_factor_, solution);
+ if (cxsparse_factor_ == NULL) {
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "CXSparse failure. Unable to find symbolic factorization.";
+ } else if (!cxsparse_.SolveCholesky(lhs, cxsparse_factor_, solution)) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = "CXSparse::SolveCholesky failed.";
+ }
cxsparse_.Free(lhs);
- return ok;
-}
-#else
-bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse(
- double* solution) {
- LOG(FATAL) << "No CXSparse support in Ceres.";
- return false;
-}
+ return summary;
#endif // CERES_NO_CXPARSE
+}
-#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE)
+// Solve the system Sx = r, assuming that the matrix S is stored in a
+// BlockRandomAccessSparseMatrix. The linear system is solved using
+// Eigen's sparse cholesky factorization routines.
+LinearSolver::Summary
+SparseSchurComplementSolver::SolveReducedLinearSystemUsingEigen(
+ double* solution) {
+#ifndef CERES_USE_EIGEN_SPARSE
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "SPARSE_SCHUR cannot be used with EIGEN_SPARSE. "
+ "Ceres was not built with support for "
+ "Eigen's SimplicialLDLT decomposition. "
+ "This requires enabling building with -DEIGENSPARSE=ON.";
+ return summary;
+
+#else
+ EventLogger event_logger("SchurComplementSolver::EigenSolve");
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
+
+ // Extract the TripletSparseMatrix that is used for actually storing S.
+ TripletSparseMatrix* tsm =
+ const_cast<TripletSparseMatrix*>(
+ down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix());
+ const int num_rows = tsm->num_rows();
+
+ // The case where there are no f blocks, and the system is block
+ // diagonal.
+ if (num_rows == 0) {
+ return summary;
+ }
+
+ // This is an upper triangular matrix.
+ CompressedRowSparseMatrix crsm(*tsm);
+ // Map this to a column major, lower triangular matrix.
+ Eigen::MappedSparseMatrix<double, Eigen::ColMajor> eigen_lhs(
+ crsm.num_rows(),
+ crsm.num_rows(),
+ crsm.num_nonzeros(),
+ crsm.mutable_rows(),
+ crsm.mutable_cols(),
+ crsm.mutable_values());
+ event_logger.AddEvent("ToCompressedRowSparseMatrix");
+
+ // Compute symbolic factorization if one does not exist.
+ if (simplicial_ldlt_.get() == NULL) {
+ simplicial_ldlt_.reset(new SimplicialLDLT);
+ // This ordering is quite bad. The scalar ordering produced by the
+ // AMD algorithm is quite bad and can be an order of magnitude
+ // worse than the one computed using the block version of the
+ // algorithm.
+ simplicial_ldlt_->analyzePattern(eigen_lhs);
+ event_logger.AddEvent("Analysis");
+ if (simplicial_ldlt_->info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "Eigen failure. Unable to find symbolic factorization.";
+ return summary;
+ }
+ }
+
+ simplicial_ldlt_->factorize(eigen_lhs);
+ event_logger.AddEvent("Factorize");
+ if (simplicial_ldlt_->info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = "Eigen failure. Unable to find numeric factoriztion.";
+ return summary;
+ }
+
+ VectorRef(solution, num_rows) =
+ simplicial_ldlt_->solve(ConstVectorRef(rhs(), num_rows));
+ event_logger.AddEvent("Solve");
+ if (simplicial_ldlt_->info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = "Eigen failure. Unable to do triangular solve.";
+ }
+
+ return summary;
+#endif // CERES_USE_EIGEN_SPARSE
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/schur_complement_solver.h b/internal/ceres/schur_complement_solver.h
index b5a1c74..723b149 100644
--- a/internal/ceres/schur_complement_solver.h
+++ b/internal/ceres/schur_complement_solver.h
@@ -35,6 +35,8 @@
#include <utility>
#include <vector>
+#include "ceres/internal/port.h"
+
#include "ceres/block_random_access_matrix.h"
#include "ceres/block_sparse_matrix.h"
#include "ceres/block_structure.h"
@@ -45,6 +47,10 @@
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"
+#ifdef CERES_USE_EIGEN_SPARSE
+#include "Eigen/SparseCholesky"
+#endif
+
namespace ceres {
namespace internal {
@@ -126,7 +132,8 @@
private:
virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0;
- virtual bool SolveReducedLinearSystem(double* solution) = 0;
+ virtual LinearSolver::Summary SolveReducedLinearSystem(
+ double* solution) = 0;
LinearSolver::Options options_;
@@ -146,12 +153,12 @@
private:
virtual void InitStorage(const CompressedRowBlockStructure* bs);
- virtual bool SolveReducedLinearSystem(double* solution);
+ virtual LinearSolver::Summary SolveReducedLinearSystem(
+ double* solution);
CERES_DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver);
};
-#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE)
// Sparse Cholesky factorization based solver.
class SparseSchurComplementSolver : public SchurComplementSolver {
public:
@@ -160,9 +167,14 @@
private:
virtual void InitStorage(const CompressedRowBlockStructure* bs);
- virtual bool SolveReducedLinearSystem(double* solution);
- bool SolveReducedLinearSystemUsingSuiteSparse(double* solution);
- bool SolveReducedLinearSystemUsingCXSparse(double* solution);
+ virtual LinearSolver::Summary SolveReducedLinearSystem(
+ double* solution);
+ LinearSolver::Summary SolveReducedLinearSystemUsingSuiteSparse(
+ double* solution);
+ LinearSolver::Summary SolveReducedLinearSystemUsingCXSparse(
+ double* solution);
+ LinearSolver::Summary SolveReducedLinearSystemUsingEigen(
+ double* solution);
// Size of the blocks in the Schur complement.
vector<int> blocks_;
@@ -175,10 +187,15 @@
CXSparse cxsparse_;
// Cached factorization
cs_dis* cxsparse_factor_;
+
+#ifdef CERES_USE_EIGEN_SPARSE
+ typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > SimplicialLDLT;
+ scoped_ptr<SimplicialLDLT> simplicial_ldlt_;
+#endif
+
CERES_DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver);
};
-#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE)
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/schur_complement_solver_test.cc b/internal/ceres/schur_complement_solver_test.cc
index d91c162..8e71b2e 100644
--- a/internal/ceres/schur_complement_solver_test.cc
+++ b/internal/ceres/schur_complement_solver_test.cc
@@ -187,17 +187,31 @@
#ifndef CERES_NO_CXSPARSE
TEST_F(SchurComplementSolverTest,
- SparseSchurWithSuiteSparseSmallProblem) {
+ SparseSchurWithCXSparseSmallProblem) {
ComputeAndCompareSolutions(2, false, SPARSE_SCHUR, EIGEN, CX_SPARSE, true);
ComputeAndCompareSolutions(2, true, SPARSE_SCHUR, EIGEN, CX_SPARSE, true);
}
TEST_F(SchurComplementSolverTest,
- SparseSchurWithSuiteSparseLargeProblem) {
+ SparseSchurWithCXSparseLargeProblem) {
ComputeAndCompareSolutions(3, false, SPARSE_SCHUR, EIGEN, CX_SPARSE, true);
ComputeAndCompareSolutions(3, true, SPARSE_SCHUR, EIGEN, CX_SPARSE, true);
}
#endif // CERES_NO_CXSPARSE
+#ifdef CERES_USE_EIGEN_SPARSE
+TEST_F(SchurComplementSolverTest,
+ SparseSchurWithEigenSparseSmallProblem) {
+ ComputeAndCompareSolutions(2, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, true);
+ ComputeAndCompareSolutions(2, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, true);
+}
+
+TEST_F(SchurComplementSolverTest,
+ SparseSchurWithEigenSparseLargeProblem) {
+ ComputeAndCompareSolutions(3, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, true);
+ ComputeAndCompareSolutions(3, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, true);
+}
+#endif // CERES_USE_EIGEN_SPARSE
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/schur_eliminator.cc b/internal/ceres/schur_eliminator.cc
index 31f8354..4d9b175 100644
--- a/internal/ceres/schur_eliminator.cc
+++ b/internal/ceres/schur_eliminator.cc
@@ -37,7 +37,7 @@
// THIS FILE IS AUTOGENERATED. DO NOT EDIT.
//=========================================
//
-// This file is generated using generate_eliminator_specializations.py.
+// This file is generated using generate_eliminator_specialization.py.
// Editing it manually is not recommended.
#include "ceres/linear_solver.h"
@@ -102,9 +102,24 @@
}
if ((options.row_block_size == 2) &&
(options.e_block_size == 4) &&
+ (options.f_block_size == 8)) {
+ return new SchurEliminator<2, 4, 8>(options);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 4) &&
+ (options.f_block_size == 9)) {
+ return new SchurEliminator<2, 4, 9>(options);
+ }
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == 4) &&
(options.f_block_size == Eigen::Dynamic)) {
return new SchurEliminator<2, 4, Eigen::Dynamic>(options);
}
+ if ((options.row_block_size == 2) &&
+ (options.e_block_size == Eigen::Dynamic) &&
+ (options.f_block_size == Eigen::Dynamic)) {
+ return new SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>(options);
+ }
if ((options.row_block_size == 4) &&
(options.e_block_size == 4) &&
(options.f_block_size == 2)) {
diff --git a/internal/ceres/schur_eliminator_impl.h b/internal/ceres/schur_eliminator_impl.h
index c09b7fb..305d94e 100644
--- a/internal/ceres/schur_eliminator_impl.h
+++ b/internal/ceres/schur_eliminator_impl.h
@@ -45,6 +45,9 @@
#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifdef CERES_USE_OPENMP
#include <omp.h>
#endif
diff --git a/internal/ceres/schur_jacobi_preconditioner.cc b/internal/ceres/schur_jacobi_preconditioner.cc
index 338df71..6dc9e89 100644
--- a/internal/ceres/schur_jacobi_preconditioner.cc
+++ b/internal/ceres/schur_jacobi_preconditioner.cc
@@ -33,10 +33,9 @@
#include <utility>
#include <vector>
#include "Eigen/Dense"
-#include "ceres/block_random_access_sparse_matrix.h"
+#include "ceres/block_random_access_diagonal_matrix.h"
#include "ceres/block_sparse_matrix.h"
#include "ceres/collections_port.h"
-#include "ceres/detect_structure.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/linear_solver.h"
#include "ceres/schur_eliminator.h"
@@ -57,16 +56,11 @@
<< "SCHUR_JACOBI preconditioner.";
block_size_.resize(num_blocks);
- set<pair<int, int> > block_pairs;
-
- int num_block_diagonal_entries = 0;
for (int i = 0; i < num_blocks; ++i) {
block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size;
- block_pairs.insert(make_pair(i, i));
- num_block_diagonal_entries += block_size_[i] * block_size_[i];
}
- m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs));
+ m_.reset(new BlockRandomAccessDiagonalMatrix(block_size_));
InitEliminator(bs);
}
@@ -77,17 +71,13 @@
void SchurJacobiPreconditioner::InitEliminator(
const CompressedRowBlockStructure& bs) {
LinearSolver::Options eliminator_options;
-
eliminator_options.elimination_groups = options_.elimination_groups;
eliminator_options.num_threads = options_.num_threads;
-
- DetectStructure(bs, options_.elimination_groups[0],
- &eliminator_options.row_block_size,
- &eliminator_options.e_block_size,
- &eliminator_options.f_block_size);
-
+ eliminator_options.e_block_size = options_.e_block_size;
+ eliminator_options.f_block_size = options_.f_block_size;
+ eliminator_options.row_block_size = options_.row_block_size;
eliminator_.reset(SchurEliminatorBase::Create(eliminator_options));
- eliminator_->Init(options_.elimination_groups[0], &bs);
+ eliminator_->Init(eliminator_options.elimination_groups[0], &bs);
}
// Update the values of the preconditioner matrix and factorize it.
@@ -118,7 +108,7 @@
CHECK_NOTNULL(y);
const double* lhs_values =
- down_cast<BlockRandomAccessSparseMatrix*>(m_.get())->matrix()->values();
+ down_cast<BlockRandomAccessDiagonalMatrix*>(m_.get())->matrix()->values();
// This loop can be easily multi-threaded with OpenMP if need be.
for (int i = 0; i < block_size_.size(); ++i) {
diff --git a/internal/ceres/schur_jacobi_preconditioner.h b/internal/ceres/schur_jacobi_preconditioner.h
index f6e7b0d..aecb015 100644
--- a/internal/ceres/schur_jacobi_preconditioner.h
+++ b/internal/ceres/schur_jacobi_preconditioner.h
@@ -49,7 +49,7 @@
namespace ceres {
namespace internal {
-class BlockRandomAccessSparseMatrix;
+class BlockRandomAccessDiagonalMatrix;
class BlockSparseMatrix;
struct CompressedRowBlockStructure;
class SchurEliminatorBase;
@@ -100,7 +100,7 @@
scoped_ptr<SchurEliminatorBase> eliminator_;
// Preconditioner matrix.
- scoped_ptr<BlockRandomAccessSparseMatrix> m_;
+ scoped_ptr<BlockRandomAccessDiagonalMatrix> m_;
CERES_DISALLOW_COPY_AND_ASSIGN(SchurJacobiPreconditioner);
};
diff --git a/internal/ceres/schur_ordering.cc b/internal/ceres/schur_ordering.cc
deleted file mode 100644
index 1cdff4e..0000000
--- a/internal/ceres/schur_ordering.cc
+++ /dev/null
@@ -1,101 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: sameeragarwal@google.com (Sameer Agarwal)
-
-#include "ceres/schur_ordering.h"
-
-#include "ceres/graph.h"
-#include "ceres/graph_algorithms.h"
-#include "ceres/internal/scoped_ptr.h"
-#include "ceres/map_util.h"
-#include "ceres/parameter_block.h"
-#include "ceres/program.h"
-#include "ceres/residual_block.h"
-#include "glog/logging.h"
-
-namespace ceres {
-namespace internal {
-
-int ComputeSchurOrdering(const Program& program,
- vector<ParameterBlock*>* ordering) {
- CHECK_NOTNULL(ordering)->clear();
-
- scoped_ptr<Graph< ParameterBlock*> > graph(
- CHECK_NOTNULL(CreateHessianGraph(program)));
- int independent_set_size = IndependentSetOrdering(*graph, ordering);
- const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
-
- // Add the excluded blocks to back of the ordering vector.
- for (int i = 0; i < parameter_blocks.size(); ++i) {
- ParameterBlock* parameter_block = parameter_blocks[i];
- if (parameter_block->IsConstant()) {
- ordering->push_back(parameter_block);
- }
- }
-
- return independent_set_size;
-}
-
-Graph<ParameterBlock*>*
-CreateHessianGraph(const Program& program) {
- Graph<ParameterBlock*>* graph = new Graph<ParameterBlock*>;
- const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
- for (int i = 0; i < parameter_blocks.size(); ++i) {
- ParameterBlock* parameter_block = parameter_blocks[i];
- if (!parameter_block->IsConstant()) {
- graph->AddVertex(parameter_block);
- }
- }
-
- const vector<ResidualBlock*>& residual_blocks = program.residual_blocks();
- for (int i = 0; i < residual_blocks.size(); ++i) {
- const ResidualBlock* residual_block = residual_blocks[i];
- const int num_parameter_blocks = residual_block->NumParameterBlocks();
- ParameterBlock* const* parameter_blocks =
- residual_block->parameter_blocks();
- for (int j = 0; j < num_parameter_blocks; ++j) {
- if (parameter_blocks[j]->IsConstant()) {
- continue;
- }
-
- for (int k = j + 1; k < num_parameter_blocks; ++k) {
- if (parameter_blocks[k]->IsConstant()) {
- continue;
- }
-
- graph->AddEdge(parameter_blocks[j], parameter_blocks[k]);
- }
- }
- }
-
- return graph;
-}
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/schur_ordering.h b/internal/ceres/schur_ordering.h
deleted file mode 100644
index 1f9a4ff..0000000
--- a/internal/ceres/schur_ordering.h
+++ /dev/null
@@ -1,74 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: sameeragarwal@google.com (Sameer Agarwal)
-//
-// Compute a parameter block ordering for use with the Schur
-// complement based algorithms.
-
-#ifndef CERES_INTERNAL_SCHUR_ORDERING_H_
-#define CERES_INTERNAL_SCHUR_ORDERING_H_
-
-#include <vector>
-#include "ceres/graph.h"
-#include "ceres/types.h"
-
-namespace ceres {
-namespace internal {
-
-class Program;
-class ParameterBlock;
-
-// Uses an approximate independent set ordering to order the parameter
-// blocks of a problem so that it is suitable for use with Schur
-// complement based solvers. The output variable ordering contains an
-// ordering of the parameter blocks and the return value is size of
-// the independent set or the number of e_blocks (see
-// schur_complement_solver.h for an explanation). Constant parameters
-// are added to the end.
-//
-// The ordering vector has the structure
-//
-// ordering = [independent set,
-// complement of the independent set,
-// fixed blocks]
-int ComputeSchurOrdering(const Program& program,
- vector<ParameterBlock* >* ordering);
-
-
-// Builds a graph on the parameter blocks of a Problem, whose
-// structure reflects the sparsity structure of the Hessian. Each
-// vertex corresponds to a parameter block in the Problem except for
-// parameter blocks that are marked constant. An edge connects two
-// parameter blocks, if they co-occur in a residual block.
-Graph<ParameterBlock*>* CreateHessianGraph(const Program& program);
-
-} // namespace internal
-} // namespace ceres
-
-#endif // CERES_INTERNAL_SCHUR_ORDERING_H_
diff --git a/internal/ceres/schur_ordering_test.cc b/internal/ceres/schur_ordering_test.cc
deleted file mode 100644
index bd74ebb..0000000
--- a/internal/ceres/schur_ordering_test.cc
+++ /dev/null
@@ -1,177 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-// http://code.google.com/p/ceres-solver/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: sameeragarwal@google.com (Sameer Agarwal)
-
-#include "ceres/schur_ordering.h"
-
-#include <cstddef>
-#include <vector>
-#include "gtest/gtest.h"
-#include "ceres/collections_port.h"
-#include "ceres/graph.h"
-#include "ceres/problem_impl.h"
-#include "ceres/program.h"
-#include "ceres/stl_util.h"
-#include "ceres/cost_function.h"
-#include "ceres/internal/scoped_ptr.h"
-#include "ceres/sized_cost_function.h"
-
-namespace ceres {
-namespace internal {
-
-typedef Graph<ParameterBlock*> HessianGraph;
-typedef HashSet<ParameterBlock*> VertexSet;
-
-template <int M, int N1 = 0, int N2 = 0, int N3 = 0>
-class DummyCostFunction: public SizedCostFunction<M, N1, N2, N3> {
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- return true;
- }
-};
-
-class SchurOrderingTest : public ::testing::Test {
- protected :
- virtual void SetUp() {
- // The explicit calls to AddParameterBlock are necessary because
- // the below tests depend on the specific numbering of the
- // parameter blocks.
- problem_.AddParameterBlock(x_, 3);
- problem_.AddParameterBlock(y_, 4);
- problem_.AddParameterBlock(z_, 5);
- problem_.AddParameterBlock(w_, 6);
-
- problem_.AddResidualBlock(new DummyCostFunction<2, 3>, NULL, x_);
- problem_.AddResidualBlock(new DummyCostFunction<6, 5, 4>, NULL, z_, y_);
- problem_.AddResidualBlock(new DummyCostFunction<3, 3, 5>, NULL, x_, z_);
- problem_.AddResidualBlock(new DummyCostFunction<7, 5, 3>, NULL, z_, x_);
- problem_.AddResidualBlock(new DummyCostFunction<1, 5, 3, 6>, NULL,
- z_, x_, w_);
- }
-
- ProblemImpl problem_;
- double x_[3], y_[4], z_[5], w_[6];
-};
-
-TEST_F(SchurOrderingTest, NoFixed) {
- const Program& program = problem_.program();
- const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
- scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
-
- const VertexSet& vertices = graph->vertices();
- EXPECT_EQ(vertices.size(), 4);
-
- for (int i = 0; i < 4; ++i) {
- EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end());
- }
-
- {
- const VertexSet& neighbors = graph->Neighbors(parameter_blocks[0]);
- EXPECT_EQ(neighbors.size(), 2);
- EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
- EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
- }
-
- {
- const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]);
- EXPECT_EQ(neighbors.size(), 1);
- EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
- }
-
- {
- const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]);
- EXPECT_EQ(neighbors.size(), 3);
- EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end());
- EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end());
- EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
- }
-
- {
- const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]);
- EXPECT_EQ(neighbors.size(), 2);
- EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end());
- EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
- }
-}
-
-TEST_F(SchurOrderingTest, AllFixed) {
- problem_.SetParameterBlockConstant(x_);
- problem_.SetParameterBlockConstant(y_);
- problem_.SetParameterBlockConstant(z_);
- problem_.SetParameterBlockConstant(w_);
-
- const Program& program = problem_.program();
- scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
- EXPECT_EQ(graph->vertices().size(), 0);
-}
-
-TEST_F(SchurOrderingTest, OneFixed) {
- problem_.SetParameterBlockConstant(x_);
-
- const Program& program = problem_.program();
- const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
- scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
-
- const VertexSet& vertices = graph->vertices();
-
- EXPECT_EQ(vertices.size(), 3);
- EXPECT_TRUE(vertices.find(parameter_blocks[0]) == vertices.end());
-
- for (int i = 1; i < 3; ++i) {
- EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end());
- }
-
- {
- const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]);
- EXPECT_EQ(neighbors.size(), 1);
- EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
- }
-
- {
- const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]);
- EXPECT_EQ(neighbors.size(), 2);
- EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end());
- EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
- }
-
- {
- const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]);
- EXPECT_EQ(neighbors.size(), 1);
- EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
- }
-
- // The constant parameter block is at the end.
- vector<ParameterBlock*> ordering;
- ComputeSchurOrdering(program, &ordering);
- EXPECT_EQ(ordering.back(), parameter_blocks[0]);
-}
-
-} // namespace internal
-} // namespace ceres
diff --git a/internal/ceres/single_linkage_clustering.cc b/internal/ceres/single_linkage_clustering.cc
new file mode 100644
index 0000000..0a8b20c
--- /dev/null
+++ b/internal/ceres/single_linkage_clustering.cc
@@ -0,0 +1,110 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_NO_SUITESPARSE
+
+#include "ceres/single_linkage_clustering.h"
+
+#include "ceres/graph.h"
+#include "ceres/collections_port.h"
+#include "ceres/graph_algorithms.h"
+
+namespace ceres {
+namespace internal {
+
+int ComputeSingleLinkageClustering(
+ const SingleLinkageClusteringOptions& options,
+ const Graph<int>& graph,
+ HashMap<int, int>* membership) {
+ CHECK_NOTNULL(membership)->clear();
+
+ // Initially each vertex is in its own cluster.
+ const HashSet<int>& vertices = graph.vertices();
+ for (HashSet<int>::const_iterator it = vertices.begin();
+ it != vertices.end();
+ ++it) {
+ (*membership)[*it] = *it;
+ }
+
+ for (HashSet<int>::const_iterator it1 = vertices.begin();
+ it1 != vertices.end();
+ ++it1) {
+ const int vertex1 = *it1;
+ const HashSet<int>& neighbors = graph.Neighbors(vertex1);
+ for (HashSet<int>::const_iterator it2 = neighbors.begin();
+ it2 != neighbors.end();
+ ++it2) {
+ const int vertex2 = *it2;
+
+ // Since the graph is undirected, only pay attention to one side
+ // of the edge and ignore weak edges.
+ if ((vertex1 > vertex2) ||
+ (graph.EdgeWeight(vertex1, vertex2) < options.min_similarity)) {
+ continue;
+ }
+
+ // Use a union-find algorithm to keep track of the clusters.
+ const int c1 = FindConnectedComponent(vertex1, membership);
+ const int c2 = FindConnectedComponent(vertex2, membership);
+
+ if (c1 == c2) {
+ continue;
+ }
+
+ if (c1 < c2) {
+ (*membership)[c2] = c1;
+ } else {
+ (*membership)[c1] = c2;
+ }
+ }
+ }
+
+ // Make sure that every vertex is connected directly to the vertex
+ // identifying the cluster.
+ int num_clusters = 0;
+ for (HashMap<int, int>::iterator it = membership->begin();
+ it != membership->end();
+ ++it) {
+ it->second = FindConnectedComponent(it->first, membership);
+ if (it->first == it->second) {
+ ++num_clusters;
+ }
+ }
+
+ return num_clusters;
+}
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_NO_SUITESPARSE
diff --git a/internal/ceres/single_linkage_clustering.h b/internal/ceres/single_linkage_clustering.h
new file mode 100644
index 0000000..e6fdeab
--- /dev/null
+++ b/internal/ceres/single_linkage_clustering.h
@@ -0,0 +1,74 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_INTERNAL_SINGLE_LINKAGE_CLUSTERING_H_
+#define CERES_INTERNAL_SINGLE_LINKAGE_CLUSTERING_H_
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_NO_SUITESPARSE
+
+#include "ceres/collections_port.h"
+#include "ceres/graph.h"
+
+namespace ceres {
+namespace internal {
+
+struct SingleLinkageClusteringOptions {
+ SingleLinkageClusteringOptions()
+ : min_similarity(0.99) {
+ }
+
+ // Graph edges with edge weight less than min_similarity are ignored
+ // during the clustering process.
+ double min_similarity;
+};
+
+// Compute a partitioning of the vertices of the graph using the
+// single linkage clustering algorithm. Edges with weight less than
+// SingleLinkageClusteringOptions::min_similarity will be ignored.
+//
+// membership upon return will contain a mapping from the vertices of
+// the graph to an integer indicating the identity of the cluster that
+// it belongs to.
+//
+// The return value of this function is the number of clusters
+// identified by the algorithm.
+int ComputeSingleLinkageClustering(
+ const SingleLinkageClusteringOptions& options,
+ const Graph<int>& graph,
+ HashMap<int, int>* membership);
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_NO_SUITESPARSE
+#endif // CERES_INTERNAL_SINGLE_LINKAGE_CLUSTERING_H_
diff --git a/internal/ceres/single_linkage_clustering_test.cc b/internal/ceres/single_linkage_clustering_test.cc
new file mode 100644
index 0000000..1cbc5be
--- /dev/null
+++ b/internal/ceres/single_linkage_clustering_test.cc
@@ -0,0 +1,132 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Sameer Agarwal (sameeragarwal@google.com)
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_NO_SUITESPARSE
+
+#include "ceres/single_linkage_clustering.h"
+
+#include "ceres/collections_port.h"
+#include "ceres/graph.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+TEST(SingleLinkageClustering, GraphHasTwoComponents) {
+ Graph<int> graph;
+ const int kNumVertices = 6;
+ for (int i = 0; i < kNumVertices; ++i) {
+ graph.AddVertex(i);
+ }
+ // Graph structure:
+ //
+ // 0-1-2-3 4-5
+ graph.AddEdge(0, 1, 1.0);
+ graph.AddEdge(1, 2, 1.0);
+ graph.AddEdge(2, 3, 1.0);
+ graph.AddEdge(4, 5, 1.0);
+
+ SingleLinkageClusteringOptions options;
+ HashMap<int, int> membership;
+ ComputeSingleLinkageClustering(options, graph, &membership);
+ EXPECT_EQ(membership.size(), kNumVertices);
+
+ EXPECT_EQ(membership[1], membership[0]);
+ EXPECT_EQ(membership[2], membership[0]);
+ EXPECT_EQ(membership[3], membership[0]);
+ EXPECT_NE(membership[4], membership[0]);
+ EXPECT_NE(membership[5], membership[0]);
+ EXPECT_EQ(membership[4], membership[5]);
+}
+
+TEST(SingleLinkageClustering, ComponentWithWeakLink) {
+ Graph<int> graph;
+ const int kNumVertices = 6;
+ for (int i = 0; i < kNumVertices; ++i) {
+ graph.AddVertex(i);
+ }
+ // Graph structure:
+ //
+ // 0-1-2-3 4-5
+ graph.AddEdge(0, 1, 1.0);
+ graph.AddEdge(1, 2, 1.0);
+ graph.AddEdge(2, 3, 1.0);
+
+ // This component should break up into two.
+ graph.AddEdge(4, 5, 0.5);
+
+ SingleLinkageClusteringOptions options;
+ HashMap<int, int> membership;
+ ComputeSingleLinkageClustering(options, graph, &membership);
+ EXPECT_EQ(membership.size(), kNumVertices);
+
+ EXPECT_EQ(membership[1], membership[0]);
+ EXPECT_EQ(membership[2], membership[0]);
+ EXPECT_EQ(membership[3], membership[0]);
+ EXPECT_NE(membership[4], membership[0]);
+ EXPECT_NE(membership[5], membership[0]);
+ EXPECT_NE(membership[4], membership[5]);
+}
+
+TEST(SingleLinkageClustering, ComponentWithWeakLinkAndStrongLink) {
+ Graph<int> graph;
+ const int kNumVertices = 6;
+ for (int i = 0; i < kNumVertices; ++i) {
+ graph.AddVertex(i);
+ }
+ // Graph structure:
+ //
+ // 0-1-2-3 4-5
+ graph.AddEdge(0, 1, 1.0);
+ graph.AddEdge(1, 2, 1.0);
+ graph.AddEdge(2, 3, 0.5); // Weak link
+ graph.AddEdge(0, 3, 1.0);
+
+ // This component should break up into two.
+ graph.AddEdge(4, 5, 1.0);
+
+ SingleLinkageClusteringOptions options;
+ HashMap<int, int> membership;
+ ComputeSingleLinkageClustering(options, graph, &membership);
+ EXPECT_EQ(membership.size(), kNumVertices);
+
+ EXPECT_EQ(membership[1], membership[0]);
+ EXPECT_EQ(membership[2], membership[0]);
+ EXPECT_EQ(membership[3], membership[0]);
+ EXPECT_EQ(membership[4], membership[5]);
+}
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_NO_SUITESPARSE
diff --git a/internal/ceres/small_blas.h b/internal/ceres/small_blas.h
index e14e664..5639664 100644
--- a/internal/ceres/small_blas.h
+++ b/internal/ceres/small_blas.h
@@ -35,36 +35,13 @@
#ifndef CERES_INTERNAL_SMALL_BLAS_H_
#define CERES_INTERNAL_SMALL_BLAS_H_
+#include "ceres/internal/port.h"
#include "ceres/internal/eigen.h"
#include "glog/logging.h"
namespace ceres {
namespace internal {
-// Remove the ".noalias()" annotation from the matrix matrix
-// mutliplies to produce a correct build with the Android NDK,
-// including versions 6, 7, 8, and 8b, when built with STLPort and the
-// non-standalone toolchain (i.e. ndk-build). This appears to be a
-// compiler bug; if the workaround is not in place, the line
-//
-// block.noalias() -= A * B;
-//
-// gets compiled to
-//
-// block.noalias() += A * B;
-//
-// which breaks schur elimination. Introducing a temporary by removing the
-// .noalias() annotation causes the issue to disappear. Tracking this
-// issue down was tricky, since the test suite doesn't run when built with
-// the non-standalone toolchain.
-//
-// TODO(keir): Make a reproduction case for this and send it upstream.
-#ifdef CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG
-#define CERES_MAYBE_NOALIAS
-#else
-#define CERES_MAYBE_NOALIAS .noalias()
-#endif
-
// The following three macros are used to share code and reduce
// template junk across the various GEMM variants.
#define CERES_GEMM_BEGIN(name) \
@@ -167,11 +144,11 @@
block(Cref, start_row_c, start_col_c, num_row_a, num_col_b);
if (kOperation > 0) {
- block CERES_MAYBE_NOALIAS += Aref * Bref;
+ block.noalias() += Aref * Bref;
} else if (kOperation < 0) {
- block CERES_MAYBE_NOALIAS -= Aref * Bref;
+ block.noalias() -= Aref * Bref;
} else {
- block CERES_MAYBE_NOALIAS = Aref * Bref;
+ block.noalias() = Aref * Bref;
}
}
@@ -227,11 +204,11 @@
start_row_c, start_col_c,
num_col_a, num_col_b);
if (kOperation > 0) {
- block CERES_MAYBE_NOALIAS += Aref.transpose() * Bref;
+ block.noalias() += Aref.transpose() * Bref;
} else if (kOperation < 0) {
- block CERES_MAYBE_NOALIAS -= Aref.transpose() * Bref;
+ block.noalias() -= Aref.transpose() * Bref;
} else {
- block CERES_MAYBE_NOALIAS = Aref.transpose() * Bref;
+ block.noalias() = Aref.transpose() * Bref;
}
}
@@ -393,8 +370,6 @@
#endif // CERES_NO_CUSTOM_BLAS
}
-
-#undef CERES_MAYBE_NOALIAS
#undef CERES_GEMM_BEGIN
#undef CERES_GEMM_EIGEN_HEADER
#undef CERES_GEMM_NAIVE_HEADER
diff --git a/internal/ceres/solver.cc b/internal/ceres/solver.cc
index 3b67746..3a57084 100644
--- a/internal/ceres/solver.cc
+++ b/internal/ceres/solver.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2014 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -29,19 +29,257 @@
// Author: keir@google.com (Keir Mierle)
// sameeragarwal@google.com (Sameer Agarwal)
+#include "ceres/internal/port.h"
#include "ceres/solver.h"
+#include <sstream> // NOLINT
#include <vector>
+
#include "ceres/problem.h"
#include "ceres/problem_impl.h"
#include "ceres/program.h"
#include "ceres/solver_impl.h"
#include "ceres/stringprintf.h"
+#include "ceres/types.h"
+#include "ceres/version.h"
#include "ceres/wall_time.h"
namespace ceres {
namespace {
+#define OPTION_OP(x, y, OP) \
+ if (!(options.x OP y)) { \
+ std::stringstream ss; \
+ ss << "Invalid configuration. "; \
+ ss << string("Solver::Options::" #x " = ") << options.x << ". "; \
+ ss << "Violated constraint: "; \
+ ss << string("Solver::Options::" #x " " #OP " "#y); \
+ *error = ss.str(); \
+ return false; \
+ }
+
+#define OPTION_OP_OPTION(x, y, OP) \
+ if (!(options.x OP options.y)) { \
+ std::stringstream ss; \
+ ss << "Invalid configuration. "; \
+ ss << string("Solver::Options::" #x " = ") << options.x << ". "; \
+ ss << string("Solver::Options::" #y " = ") << options.y << ". "; \
+ ss << "Violated constraint: "; \
+ ss << string("Solver::Options::" #x ); \
+ ss << string(#OP " Solver::Options::" #y "."); \
+ *error = ss.str(); \
+ return false; \
+ }
+
+#define OPTION_GE(x, y) OPTION_OP(x, y, >=);
+#define OPTION_GT(x, y) OPTION_OP(x, y, >);
+#define OPTION_LE(x, y) OPTION_OP(x, y, <=);
+#define OPTION_LT(x, y) OPTION_OP(x, y, <);
+#define OPTION_LE_OPTION(x, y) OPTION_OP_OPTION(x ,y, <=)
+#define OPTION_LT_OPTION(x, y) OPTION_OP_OPTION(x ,y, <)
+
+bool CommonOptionsAreValid(const Solver::Options& options, string* error) {
+ OPTION_GE(max_num_iterations, 0);
+ OPTION_GE(max_solver_time_in_seconds, 0.0);
+ OPTION_GE(function_tolerance, 0.0);
+ OPTION_GE(gradient_tolerance, 0.0);
+ OPTION_GE(parameter_tolerance, 0.0);
+ OPTION_GT(num_threads, 0);
+ OPTION_GT(num_linear_solver_threads, 0);
+ if (options.check_gradients) {
+ OPTION_GT(gradient_check_relative_precision, 0.0);
+ OPTION_GT(numeric_derivative_relative_step_size, 0.0);
+ }
+ return true;
+}
+
+bool TrustRegionOptionsAreValid(const Solver::Options& options, string* error) {
+ OPTION_GT(initial_trust_region_radius, 0.0);
+ OPTION_GT(min_trust_region_radius, 0.0);
+ OPTION_GT(max_trust_region_radius, 0.0);
+ OPTION_LE_OPTION(min_trust_region_radius, max_trust_region_radius);
+ OPTION_LE_OPTION(min_trust_region_radius, initial_trust_region_radius);
+ OPTION_LE_OPTION(initial_trust_region_radius, max_trust_region_radius);
+ OPTION_GE(min_relative_decrease, 0.0);
+ OPTION_GE(min_lm_diagonal, 0.0);
+ OPTION_GE(max_lm_diagonal, 0.0);
+ OPTION_LE_OPTION(min_lm_diagonal, max_lm_diagonal);
+ OPTION_GE(max_num_consecutive_invalid_steps, 0);
+ OPTION_GT(eta, 0.0);
+ OPTION_GE(min_linear_solver_iterations, 1);
+ OPTION_GE(max_linear_solver_iterations, 1);
+ OPTION_LE_OPTION(min_linear_solver_iterations, max_linear_solver_iterations);
+
+ if (options.use_inner_iterations) {
+ OPTION_GE(inner_iteration_tolerance, 0.0);
+ }
+
+ if (options.use_nonmonotonic_steps) {
+ OPTION_GT(max_consecutive_nonmonotonic_steps, 0);
+ }
+
+ if (options.preconditioner_type == CLUSTER_JACOBI &&
+ options.sparse_linear_algebra_library_type != SUITE_SPARSE) {
+ *error = "CLUSTER_JACOBI requires "
+ "Solver::Options::sparse_linear_algebra_library_type to be "
+ "SUITE_SPARSE";
+ return false;
+ }
+
+ if (options.preconditioner_type == CLUSTER_TRIDIAGONAL &&
+ options.sparse_linear_algebra_library_type != SUITE_SPARSE) {
+ *error = "CLUSTER_TRIDIAGONAL requires "
+ "Solver::Options::sparse_linear_algebra_library_type to be "
+ "SUITE_SPARSE";
+ return false;
+ }
+
+#ifdef CERES_NO_LAPACK
+ if (options.dense_linear_algebra_library_type == LAPACK) {
+ if (options.linear_solver_type == DENSE_NORMAL_CHOLESKY) {
+ *error = "Can't use DENSE_NORMAL_CHOLESKY with LAPACK because "
+ "LAPACK was not enabled when Ceres was built.";
+ return false;
+ }
+
+ if (options.linear_solver_type == DENSE_QR) {
+ *error = "Can't use DENSE_QR with LAPACK because "
+ "LAPACK was not enabled when Ceres was built.";
+ return false;
+ }
+
+ if (options.linear_solver_type == DENSE_SCHUR) {
+ *error = "Can't use DENSE_SCHUR with LAPACK because "
+ "LAPACK was not enabled when Ceres was built.";
+ return false;
+ }
+ }
+#endif
+
+#ifdef CERES_NO_SUITESPARSE
+ if (options.sparse_linear_algebra_library_type == SUITE_SPARSE) {
+ if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
+ *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because "
+ "SuiteSparse was not enabled when Ceres was built.";
+ return false;
+ }
+
+ if (options.linear_solver_type == SPARSE_SCHUR) {
+ *error = "Can't use SPARSE_SCHUR with SUITESPARSE because "
+ "SuiteSparse was not enabled when Ceres was built.";
+ return false;
+ }
+
+ if (options.preconditioner_type == CLUSTER_JACOBI) {
+ *error = "CLUSTER_JACOBI preconditioner not supported. "
+ "SuiteSparse was not enabled when Ceres was built.";
+ return false;
+ }
+
+ if (options.preconditioner_type == CLUSTER_TRIDIAGONAL) {
+ *error = "CLUSTER_TRIDIAGONAL preconditioner not supported. "
+ "SuiteSparse was not enabled when Ceres was built.";
+ return false;
+ }
+ }
+#endif
+
+#ifdef CERES_NO_CXSPARSE
+ if (options.sparse_linear_algebra_library_type == CX_SPARSE) {
+ if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
+ *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because "
+ "CXSparse was not enabled when Ceres was built.";
+ return false;
+ }
+
+ if (options.linear_solver_type == SPARSE_SCHUR) {
+ *error = "Can't use SPARSE_SCHUR with CX_SPARSE because "
+ "CXSparse was not enabled when Ceres was built.";
+ return false;
+ }
+ }
+#endif
+
+ if (options.trust_region_strategy_type == DOGLEG) {
+ if (options.linear_solver_type == ITERATIVE_SCHUR ||
+ options.linear_solver_type == CGNR) {
+ *error = "DOGLEG only supports exact factorization based linear "
+ "solvers. If you want to use an iterative solver please "
+ "use LEVENBERG_MARQUARDT as the trust_region_strategy_type";
+ return false;
+ }
+ }
+
+ if (options.trust_region_minimizer_iterations_to_dump.size() > 0 &&
+ options.trust_region_problem_dump_format_type != CONSOLE &&
+ options.trust_region_problem_dump_directory.empty()) {
+ *error = "Solver::Options::trust_region_problem_dump_directory is empty.";
+ return false;
+ }
+
+ if (options.dynamic_sparsity &&
+ options.linear_solver_type != SPARSE_NORMAL_CHOLESKY) {
+ *error = "Dynamic sparsity is only supported with SPARSE_NORMAL_CHOLESKY.";
+ return false;
+ }
+
+ return true;
+}
+
+bool LineSearchOptionsAreValid(const Solver::Options& options, string* error) {
+ OPTION_GT(max_lbfgs_rank, 0);
+ OPTION_GT(min_line_search_step_size, 0.0);
+ OPTION_GT(max_line_search_step_contraction, 0.0);
+ OPTION_LT(max_line_search_step_contraction, 1.0);
+ OPTION_LT_OPTION(max_line_search_step_contraction,
+ min_line_search_step_contraction);
+ OPTION_LE(min_line_search_step_contraction, 1.0);
+ OPTION_GT(max_num_line_search_step_size_iterations, 0);
+ OPTION_GT(line_search_sufficient_function_decrease, 0.0);
+ OPTION_LT_OPTION(line_search_sufficient_function_decrease,
+ line_search_sufficient_curvature_decrease);
+ OPTION_LT(line_search_sufficient_curvature_decrease, 1.0);
+ OPTION_GT(max_line_search_step_expansion, 1.0);
+
+ if ((options.line_search_direction_type == ceres::BFGS ||
+ options.line_search_direction_type == ceres::LBFGS) &&
+ options.line_search_type != ceres::WOLFE) {
+
+ *error =
+ string("Invalid configuration: Solver::Options::line_search_type = ")
+ + string(LineSearchTypeToString(options.line_search_type))
+ + string(". When using (L)BFGS, "
+ "Solver::Options::line_search_type must be set to WOLFE.");
+ return false;
+ }
+
+ // Warn user if they have requested BISECTION interpolation, but constraints
+ // on max/min step size change during line search prevent bisection scaling
+ // from occurring. Warn only, as this is likely a user mistake, but one which
+ // does not prevent us from continuing.
+ LOG_IF(WARNING,
+ (options.line_search_interpolation_type == ceres::BISECTION &&
+ (options.max_line_search_step_contraction > 0.5 ||
+ options.min_line_search_step_contraction < 0.5)))
+ << "Line search interpolation type is BISECTION, but specified "
+ << "max_line_search_step_contraction: "
+ << options.max_line_search_step_contraction << ", and "
+ << "min_line_search_step_contraction: "
+ << options.min_line_search_step_contraction
+ << ", prevent bisection (0.5) scaling, continuing with solve regardless.";
+
+ return true;
+}
+
+#undef OPTION_OP
+#undef OPTION_OP_OPTION
+#undef OPTION_GT
+#undef OPTION_GE
+#undef OPTION_LE
+#undef OPTION_LT
+#undef OPTION_LE_OPTION
+#undef OPTION_LT_OPTION
+
void StringifyOrdering(const vector<int>& ordering, string* report) {
if (ordering.size() == 0) {
internal::StringAppendF(report, "AUTOMATIC");
@@ -54,11 +292,19 @@
internal::StringAppendF(report, "%d", ordering.back());
}
-} // namespace
+} // namespace
-Solver::Options::~Options() {
- delete linear_solver_ordering;
- delete inner_iteration_ordering;
+bool Solver::Options::IsValid(string* error) const {
+ if (!CommonOptionsAreValid(*this, error)) {
+ return false;
+ }
+
+ if (minimizer_type == TRUST_REGION) {
+ return TrustRegionOptionsAreValid(*this, error);
+ }
+
+ CHECK_EQ(minimizer_type, LINE_SEARCH);
+ return LineSearchOptionsAreValid(*this, error);
}
Solver::~Solver() {}
@@ -67,8 +313,16 @@
Problem* problem,
Solver::Summary* summary) {
double start_time_seconds = internal::WallTimeInSeconds();
- internal::ProblemImpl* problem_impl =
- CHECK_NOTNULL(problem)->problem_impl_.get();
+ CHECK_NOTNULL(problem);
+ CHECK_NOTNULL(summary);
+
+ *summary = Summary();
+ if (!options.IsValid(&summary->message)) {
+ LOG(ERROR) << "Terminating: " << summary->message;
+ return;
+ }
+
+ internal::ProblemImpl* problem_impl = problem->problem_impl_.get();
internal::SolverImpl::Solve(options, problem_impl, summary);
summary->total_time_in_seconds =
internal::WallTimeInSeconds() - start_time_seconds;
@@ -85,7 +339,8 @@
// Invalid values for most fields, to ensure that we are not
// accidentally reporting default values.
: minimizer_type(TRUST_REGION),
- termination_type(DID_NOT_RUN),
+ termination_type(FAILURE),
+ message("ceres::Solve was not called."),
initial_cost(-1.0),
final_cost(-1.0),
fixed_cost(-1.0),
@@ -119,75 +374,51 @@
inner_iterations_given(false),
inner_iterations_used(false),
preconditioner_type(IDENTITY),
+ visibility_clustering_type(CANONICAL_VIEWS),
trust_region_strategy_type(LEVENBERG_MARQUARDT),
dense_linear_algebra_library_type(EIGEN),
sparse_linear_algebra_library_type(SUITE_SPARSE),
line_search_direction_type(LBFGS),
- line_search_type(ARMIJO) {
+ line_search_type(ARMIJO),
+ line_search_interpolation_type(BISECTION),
+ nonlinear_conjugate_gradient_type(FLETCHER_REEVES),
+ max_lbfgs_rank(-1) {
}
-string Solver::Summary::BriefReport() const {
- string report = "Ceres Solver Report: ";
- if (termination_type == DID_NOT_RUN) {
- CHECK(!error.empty())
- << "Solver terminated with DID_NOT_RUN but the solver did not "
- << "return a reason. This is a Ceres error. Please report this "
- << "to the Ceres team";
- return report + "Termination: DID_NOT_RUN, because " + error;
- }
-
- internal::StringAppendF(&report, "Iterations: %d",
- num_successful_steps + num_unsuccessful_steps);
- internal::StringAppendF(&report, ", Initial cost: %e", initial_cost);
-
- // If the solver failed or was aborted, then the final_cost has no
- // meaning.
- if (termination_type != NUMERICAL_FAILURE &&
- termination_type != USER_ABORT) {
- internal::StringAppendF(&report, ", Final cost: %e", final_cost);
- }
-
- internal::StringAppendF(&report, ", Termination: %s.",
- SolverTerminationTypeToString(termination_type));
- return report;
-};
-
using internal::StringAppendF;
using internal::StringPrintf;
+string Solver::Summary::BriefReport() const {
+ return StringPrintf("Ceres Solver Report: "
+ "Iterations: %d, "
+ "Initial cost: %e, "
+ "Final cost: %e, "
+ "Termination: %s",
+ num_successful_steps + num_unsuccessful_steps,
+ initial_cost,
+ final_cost,
+ TerminationTypeToString(termination_type));
+};
+
string Solver::Summary::FullReport() const {
string report =
"\n"
- "Ceres Solver Report\n"
- "-------------------\n";
+ "Ceres Solver v" CERES_VERSION_STRING " Solve Report\n"
+ "----------------------------------\n";
- if (termination_type == DID_NOT_RUN) {
- StringAppendF(&report, " Original\n");
- StringAppendF(&report, "Parameter blocks % 10d\n", num_parameter_blocks);
- StringAppendF(&report, "Parameters % 10d\n", num_parameters);
- if (num_effective_parameters != num_parameters) {
- StringAppendF(&report, "Effective parameters% 10d\n", num_parameters);
- }
-
- StringAppendF(&report, "Residual blocks % 10d\n",
- num_residual_blocks);
- StringAppendF(&report, "Residuals % 10d\n\n",
- num_residuals);
- } else {
- StringAppendF(&report, "%45s %21s\n", "Original", "Reduced");
- StringAppendF(&report, "Parameter blocks % 25d% 25d\n",
- num_parameter_blocks, num_parameter_blocks_reduced);
- StringAppendF(&report, "Parameters % 25d% 25d\n",
- num_parameters, num_parameters_reduced);
- if (num_effective_parameters_reduced != num_parameters_reduced) {
- StringAppendF(&report, "Effective parameters% 25d% 25d\n",
- num_effective_parameters, num_effective_parameters_reduced);
- }
- StringAppendF(&report, "Residual blocks % 25d% 25d\n",
- num_residual_blocks, num_residual_blocks_reduced);
- StringAppendF(&report, "Residual % 25d% 25d\n",
- num_residuals, num_residuals_reduced);
+ StringAppendF(&report, "%45s %21s\n", "Original", "Reduced");
+ StringAppendF(&report, "Parameter blocks % 25d% 25d\n",
+ num_parameter_blocks, num_parameter_blocks_reduced);
+ StringAppendF(&report, "Parameters % 25d% 25d\n",
+ num_parameters, num_parameters_reduced);
+ if (num_effective_parameters_reduced != num_parameters_reduced) {
+ StringAppendF(&report, "Effective parameters% 25d% 25d\n",
+ num_effective_parameters, num_effective_parameters_reduced);
}
+ StringAppendF(&report, "Residual blocks % 25d% 25d\n",
+ num_residual_blocks, num_residual_blocks_reduced);
+ StringAppendF(&report, "Residual % 25d% 25d\n",
+ num_residuals, num_residuals_reduced);
if (minimizer_type == TRUST_REGION) {
// TRUST_SEARCH HEADER
@@ -237,6 +468,14 @@
PreconditionerTypeToString(preconditioner_type));
}
+ if (preconditioner_type == CLUSTER_JACOBI ||
+ preconditioner_type == CLUSTER_TRIDIAGONAL) {
+ StringAppendF(&report, "Visibility clustering%24s%25s\n",
+ VisibilityClusteringTypeToString(
+ visibility_clustering_type),
+ VisibilityClusteringTypeToString(
+ visibility_clustering_type));
+ }
StringAppendF(&report, "Threads % 25d% 25d\n",
num_threads_given, num_threads_used);
StringAppendF(&report, "Linear solver threads % 23d% 25d\n",
@@ -305,21 +544,10 @@
num_threads_given, num_threads_used);
}
- if (termination_type == DID_NOT_RUN) {
- CHECK(!error.empty())
- << "Solver terminated with DID_NOT_RUN but the solver did not "
- << "return a reason. This is a Ceres error. Please report this "
- << "to the Ceres team";
- StringAppendF(&report, "Termination: %20s\n",
- "DID_NOT_RUN");
- StringAppendF(&report, "Reason: %s\n", error.c_str());
- return report;
- }
-
StringAppendF(&report, "\nCost:\n");
StringAppendF(&report, "Initial % 30e\n", initial_cost);
- if (termination_type != NUMERICAL_FAILURE &&
- termination_type != USER_ABORT) {
+ if (termination_type != FAILURE &&
+ termination_type != USER_FAILURE) {
StringAppendF(&report, "Final % 30e\n", final_cost);
StringAppendF(&report, "Change % 30e\n",
initial_cost - final_cost);
@@ -370,9 +598,15 @@
StringAppendF(&report, "Total %25.3f\n\n",
total_time_in_seconds);
- StringAppendF(&report, "Termination: %25s\n",
- SolverTerminationTypeToString(termination_type));
+ StringAppendF(&report, "Termination: %25s (%s)\n",
+ TerminationTypeToString(termination_type), message.c_str());
return report;
};
+bool Solver::Summary::IsSolutionUsable() const {
+ return (termination_type == CONVERGENCE ||
+ termination_type == NO_CONVERGENCE ||
+ termination_type == USER_SUCCESS);
+}
+
} // namespace ceres
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 83faa05..a1cf4ca 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2014 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -34,6 +34,8 @@
#include <iostream> // NOLINT
#include <numeric>
#include <string>
+#include "ceres/array_utils.h"
+#include "ceres/callbacks.h"
#include "ceres/coordinate_descent_minimizer.h"
#include "ceres/cxsparse.h"
#include "ceres/evaluator.h"
@@ -47,168 +49,20 @@
#include "ceres/ordered_groups.h"
#include "ceres/parameter_block.h"
#include "ceres/parameter_block_ordering.h"
+#include "ceres/preconditioner.h"
#include "ceres/problem.h"
#include "ceres/problem_impl.h"
#include "ceres/program.h"
+#include "ceres/reorder_program.h"
#include "ceres/residual_block.h"
#include "ceres/stringprintf.h"
#include "ceres/suitesparse.h"
+#include "ceres/summary_utils.h"
#include "ceres/trust_region_minimizer.h"
#include "ceres/wall_time.h"
namespace ceres {
namespace internal {
-namespace {
-
-// Callback for updating the user's parameter blocks. Updates are only
-// done if the step is successful.
-class StateUpdatingCallback : public IterationCallback {
- public:
- StateUpdatingCallback(Program* program, double* parameters)
- : program_(program), parameters_(parameters) {}
-
- CallbackReturnType operator()(const IterationSummary& summary) {
- if (summary.step_is_successful) {
- program_->StateVectorToParameterBlocks(parameters_);
- program_->CopyParameterBlockStateToUserState();
- }
- return SOLVER_CONTINUE;
- }
-
- private:
- Program* program_;
- double* parameters_;
-};
-
-void SetSummaryFinalCost(Solver::Summary* summary) {
- summary->final_cost = summary->initial_cost;
- // We need the loop here, instead of just looking at the last
- // iteration because the minimizer maybe making non-monotonic steps.
- for (int i = 0; i < summary->iterations.size(); ++i) {
- const IterationSummary& iteration_summary = summary->iterations[i];
- summary->final_cost = min(iteration_summary.cost, summary->final_cost);
- }
-}
-
-// Callback for logging the state of the minimizer to STDERR or STDOUT
-// depending on the user's preferences and logging level.
-class TrustRegionLoggingCallback : public IterationCallback {
- public:
- explicit TrustRegionLoggingCallback(bool log_to_stdout)
- : log_to_stdout_(log_to_stdout) {}
-
- ~TrustRegionLoggingCallback() {}
-
- CallbackReturnType operator()(const IterationSummary& summary) {
- const char* kReportRowFormat =
- "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e "
- "rho:% 3.2e mu:% 3.2e li:% 3d it:% 3.2e tt:% 3.2e";
- string output = StringPrintf(kReportRowFormat,
- summary.iteration,
- summary.cost,
- summary.cost_change,
- summary.gradient_max_norm,
- summary.step_norm,
- summary.relative_decrease,
- summary.trust_region_radius,
- summary.linear_solver_iterations,
- summary.iteration_time_in_seconds,
- summary.cumulative_time_in_seconds);
- if (log_to_stdout_) {
- cout << output << endl;
- } else {
- VLOG(1) << output;
- }
- return SOLVER_CONTINUE;
- }
-
- private:
- const bool log_to_stdout_;
-};
-
-// Callback for logging the state of the minimizer to STDERR or STDOUT
-// depending on the user's preferences and logging level.
-class LineSearchLoggingCallback : public IterationCallback {
- public:
- explicit LineSearchLoggingCallback(bool log_to_stdout)
- : log_to_stdout_(log_to_stdout) {}
-
- ~LineSearchLoggingCallback() {}
-
- CallbackReturnType operator()(const IterationSummary& summary) {
- const char* kReportRowFormat =
- "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e "
- "s:% 3.2e e:% 3d it:% 3.2e tt:% 3.2e";
- string output = StringPrintf(kReportRowFormat,
- summary.iteration,
- summary.cost,
- summary.cost_change,
- summary.gradient_max_norm,
- summary.step_norm,
- summary.step_size,
- summary.line_search_function_evaluations,
- summary.iteration_time_in_seconds,
- summary.cumulative_time_in_seconds);
- if (log_to_stdout_) {
- cout << output << endl;
- } else {
- VLOG(1) << output;
- }
- return SOLVER_CONTINUE;
- }
-
- private:
- const bool log_to_stdout_;
-};
-
-
-// Basic callback to record the execution of the solver to a file for
-// offline analysis.
-class FileLoggingCallback : public IterationCallback {
- public:
- explicit FileLoggingCallback(const string& filename)
- : fptr_(NULL) {
- fptr_ = fopen(filename.c_str(), "w");
- CHECK_NOTNULL(fptr_);
- }
-
- virtual ~FileLoggingCallback() {
- if (fptr_ != NULL) {
- fclose(fptr_);
- }
- }
-
- virtual CallbackReturnType operator()(const IterationSummary& summary) {
- fprintf(fptr_,
- "%4d %e %e\n",
- summary.iteration,
- summary.cost,
- summary.cumulative_time_in_seconds);
- return SOLVER_CONTINUE;
- }
- private:
- FILE* fptr_;
-};
-
-// Iterate over each of the groups in order of their priority and fill
-// summary with their sizes.
-void SummarizeOrdering(ParameterBlockOrdering* ordering,
- vector<int>* summary) {
- CHECK_NOTNULL(summary)->clear();
- if (ordering == NULL) {
- return;
- }
-
- const map<int, set<double*> >& group_to_elements =
- ordering->group_to_elements();
- for (map<int, set<double*> >::const_iterator it = group_to_elements.begin();
- it != group_to_elements.end();
- ++it) {
- summary->push_back(it->second.size());
- }
-}
-
-} // namespace
void SolverImpl::TrustRegionMinimize(
const Solver::Options& options,
@@ -216,27 +70,26 @@
CoordinateDescentMinimizer* inner_iteration_minimizer,
Evaluator* evaluator,
LinearSolver* linear_solver,
- double* parameters,
Solver::Summary* summary) {
Minimizer::Options minimizer_options(options);
+ minimizer_options.is_constrained = program->IsBoundsConstrained();
- // TODO(sameeragarwal): Add support for logging the configuration
- // and more detailed stats.
- scoped_ptr<IterationCallback> file_logging_callback;
- if (!options.solver_log.empty()) {
- file_logging_callback.reset(new FileLoggingCallback(options.solver_log));
- minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(),
- file_logging_callback.get());
- }
+ // The optimizer works on contiguous parameter vectors; allocate
+ // some.
+ Vector parameters(program->NumParameters());
- TrustRegionLoggingCallback logging_callback(
- options.minimizer_progress_to_stdout);
+ // Collect the discontiguous parameters into a contiguous state
+ // vector.
+ program->ParameterBlocksToStateVector(parameters.data());
+
+ LoggingCallback logging_callback(TRUST_REGION,
+ options.minimizer_progress_to_stdout);
if (options.logging_type != SILENT) {
minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(),
&logging_callback);
}
- StateUpdatingCallback updating_callback(program, parameters);
+ StateUpdatingCallback updating_callback(program, parameters.data());
if (options.update_state_every_iteration) {
// This must get pushed to the front of the callbacks so that it is run
// before any of the user callbacks.
@@ -266,37 +119,42 @@
TrustRegionMinimizer minimizer;
double minimizer_start_time = WallTimeInSeconds();
- minimizer.Minimize(minimizer_options, parameters, summary);
+ minimizer.Minimize(minimizer_options, parameters.data(), summary);
+
+ // If the user aborted mid-optimization or the optimization
+ // terminated because of a numerical failure, then do not update
+ // user state.
+ if (summary->termination_type != USER_FAILURE &&
+ summary->termination_type != FAILURE) {
+ program->StateVectorToParameterBlocks(parameters.data());
+ program->CopyParameterBlockStateToUserState();
+ }
+
summary->minimizer_time_in_seconds =
WallTimeInSeconds() - minimizer_start_time;
}
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
void SolverImpl::LineSearchMinimize(
const Solver::Options& options,
Program* program,
Evaluator* evaluator,
- double* parameters,
Solver::Summary* summary) {
Minimizer::Options minimizer_options(options);
- // TODO(sameeragarwal): Add support for logging the configuration
- // and more detailed stats.
- scoped_ptr<IterationCallback> file_logging_callback;
- if (!options.solver_log.empty()) {
- file_logging_callback.reset(new FileLoggingCallback(options.solver_log));
- minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(),
- file_logging_callback.get());
- }
+ // The optimizer works on contiguous parameter vectors; allocate some.
+ Vector parameters(program->NumParameters());
- LineSearchLoggingCallback logging_callback(
- options.minimizer_progress_to_stdout);
+ // Collect the discontiguous parameters into a contiguous state vector.
+ program->ParameterBlocksToStateVector(parameters.data());
+
+ LoggingCallback logging_callback(LINE_SEARCH,
+ options.minimizer_progress_to_stdout);
if (options.logging_type != SILENT) {
minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(),
&logging_callback);
}
- StateUpdatingCallback updating_callback(program, parameters);
+ StateUpdatingCallback updating_callback(program, parameters.data());
if (options.update_state_every_iteration) {
// This must get pushed to the front of the callbacks so that it is run
// before any of the user callbacks.
@@ -308,11 +166,20 @@
LineSearchMinimizer minimizer;
double minimizer_start_time = WallTimeInSeconds();
- minimizer.Minimize(minimizer_options, parameters, summary);
+ minimizer.Minimize(minimizer_options, parameters.data(), summary);
+
+ // If the user aborted mid-optimization or the optimization
+ // terminated because of a numerical failure, then do not update
+ // user state.
+ if (summary->termination_type != USER_FAILURE &&
+ summary->termination_type != FAILURE) {
+ program->StateVectorToParameterBlocks(parameters.data());
+ program->CopyParameterBlockStateToUserState();
+ }
+
summary->minimizer_time_in_seconds =
WallTimeInSeconds() - minimizer_start_time;
}
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
void SolverImpl::Solve(const Solver::Options& options,
ProblemImpl* problem_impl,
@@ -326,15 +193,10 @@
<< " residual blocks, "
<< problem_impl->NumResiduals()
<< " residuals.";
-
if (options.minimizer_type == TRUST_REGION) {
TrustRegionSolve(options, problem_impl, summary);
} else {
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
LineSearchSolve(options, problem_impl, summary);
-#else
- LOG(FATAL) << "Ceres Solver was compiled with -DLINE_SEARCH_MINIMIZER=OFF";
-#endif
}
}
@@ -347,39 +209,15 @@
Program* original_program = original_problem_impl->mutable_program();
ProblemImpl* problem_impl = original_problem_impl;
- // Reset the summary object to its default values.
- *CHECK_NOTNULL(summary) = Solver::Summary();
-
summary->minimizer_type = TRUST_REGION;
- summary->num_parameter_blocks = problem_impl->NumParameterBlocks();
- summary->num_parameters = problem_impl->NumParameters();
- summary->num_effective_parameters =
- original_program->NumEffectiveParameters();
- summary->num_residual_blocks = problem_impl->NumResidualBlocks();
- summary->num_residuals = problem_impl->NumResiduals();
- // Empty programs are usually a user error.
- if (summary->num_parameter_blocks == 0) {
- summary->error = "Problem contains no parameter blocks.";
- LOG(ERROR) << summary->error;
- return;
- }
-
- if (summary->num_residual_blocks == 0) {
- summary->error = "Problem contains no residual blocks.";
- LOG(ERROR) << summary->error;
- return;
- }
-
- SummarizeOrdering(original_options.linear_solver_ordering,
- &(summary->linear_solver_ordering_given));
-
- SummarizeOrdering(original_options.inner_iteration_ordering,
- &(summary->inner_iteration_ordering_given));
+ SummarizeGivenProgram(*original_program, summary);
+ OrderingToGroupSizes(original_options.linear_solver_ordering.get(),
+ &(summary->linear_solver_ordering_given));
+ OrderingToGroupSizes(original_options.inner_iteration_ordering.get(),
+ &(summary->inner_iteration_ordering_given));
Solver::Options options(original_options);
- options.linear_solver_ordering = NULL;
- options.inner_iteration_ordering = NULL;
#ifndef CERES_USE_OPENMP
if (options.num_threads > 1) {
@@ -404,9 +242,19 @@
if (options.trust_region_minimizer_iterations_to_dump.size() > 0 &&
options.trust_region_problem_dump_format_type != CONSOLE &&
options.trust_region_problem_dump_directory.empty()) {
- summary->error =
+ summary->message =
"Solver::Options::trust_region_problem_dump_directory is empty.";
- LOG(ERROR) << summary->error;
+ LOG(ERROR) << summary->message;
+ return;
+ }
+
+ if (!original_program->ParameterBlocksAreFinite(&summary->message)) {
+ LOG(ERROR) << "Terminating: " << summary->message;
+ return;
+ }
+
+ if (!original_program->IsFeasible(&summary->message)) {
+ LOG(ERROR) << "Terminating: " << summary->message;
return;
}
@@ -433,17 +281,14 @@
problem_impl = gradient_checking_problem_impl.get();
}
- if (original_options.linear_solver_ordering != NULL) {
- if (!IsOrderingValid(original_options, problem_impl, &summary->error)) {
- LOG(ERROR) << summary->error;
+ if (options.linear_solver_ordering.get() != NULL) {
+ if (!IsOrderingValid(options, problem_impl, &summary->message)) {
+ LOG(ERROR) << summary->message;
return;
}
event_logger.AddEvent("CheckOrdering");
- options.linear_solver_ordering =
- new ParameterBlockOrdering(*original_options.linear_solver_ordering);
- event_logger.AddEvent("CopyOrdering");
} else {
- options.linear_solver_ordering = new ParameterBlockOrdering;
+ options.linear_solver_ordering.reset(new ParameterBlockOrdering);
const ProblemImpl::ParameterMap& parameter_map =
problem_impl->parameter_map();
for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin();
@@ -459,41 +304,35 @@
scoped_ptr<Program> reduced_program(CreateReducedProgram(&options,
problem_impl,
&summary->fixed_cost,
- &summary->error));
+ &summary->message));
event_logger.AddEvent("CreateReducedProgram");
if (reduced_program == NULL) {
return;
}
- SummarizeOrdering(options.linear_solver_ordering,
- &(summary->linear_solver_ordering_used));
-
- summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks();
- summary->num_parameters_reduced = reduced_program->NumParameters();
- summary->num_effective_parameters_reduced =
- reduced_program->NumEffectiveParameters();
- summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks();
- summary->num_residuals_reduced = reduced_program->NumResiduals();
+ OrderingToGroupSizes(options.linear_solver_ordering.get(),
+ &(summary->linear_solver_ordering_used));
+ SummarizeReducedProgram(*reduced_program, summary);
if (summary->num_parameter_blocks_reduced == 0) {
summary->preprocessor_time_in_seconds =
WallTimeInSeconds() - solver_start_time;
double post_process_start_time = WallTimeInSeconds();
- LOG(INFO) << "Terminating: FUNCTION_TOLERANCE reached. "
- << "No non-constant parameter blocks found.";
+
+ summary->message =
+ "Function tolerance reached. "
+ "No non-constant parameter blocks found.";
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, options.logging_type != SILENT) << summary->message;
summary->initial_cost = summary->fixed_cost;
summary->final_cost = summary->fixed_cost;
- // FUNCTION_TOLERANCE is the right convergence here, as we know
- // that the objective function is constant and cannot be changed
- // any further.
- summary->termination_type = FUNCTION_TOLERANCE;
-
// Ensure the program state is set to the user parameters on the way out.
original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+ original_program->SetParameterOffsetsAndIndex();
summary->postprocessor_time_in_seconds =
WallTimeInSeconds() - post_process_start_time;
@@ -501,7 +340,7 @@
}
scoped_ptr<LinearSolver>
- linear_solver(CreateLinearSolver(&options, &summary->error));
+ linear_solver(CreateLinearSolver(&options, &summary->message));
event_logger.AddEvent("CreateLinearSolver");
if (linear_solver == NULL) {
return;
@@ -511,6 +350,7 @@
summary->linear_solver_type_used = options.linear_solver_type;
summary->preconditioner_type = options.preconditioner_type;
+ summary->visibility_clustering_type = options.visibility_clustering_type;
summary->num_linear_solver_threads_given =
original_options.num_linear_solver_threads;
@@ -527,7 +367,7 @@
scoped_ptr<Evaluator> evaluator(CreateEvaluator(options,
problem_impl->parameter_map(),
reduced_program.get(),
- &summary->error));
+ &summary->message));
event_logger.AddEvent("CreateEvaluator");
@@ -542,26 +382,18 @@
<< "Disabling inner iterations.";
} else {
inner_iteration_minimizer.reset(
- CreateInnerIterationMinimizer(original_options,
+ CreateInnerIterationMinimizer(options,
*reduced_program,
problem_impl->parameter_map(),
summary));
if (inner_iteration_minimizer == NULL) {
- LOG(ERROR) << summary->error;
+ LOG(ERROR) << summary->message;
return;
}
}
}
event_logger.AddEvent("CreateInnerIterationMinimizer");
- // The optimizer works on contiguous parameter vectors; allocate some.
- Vector parameters(reduced_program->NumParameters());
-
- // Collect the discontiguous parameters into a contiguous state vector.
- reduced_program->ParameterBlocksToStateVector(parameters.data());
-
- Vector original_parameters = parameters;
-
double minimizer_start_time = WallTimeInSeconds();
summary->preprocessor_time_in_seconds =
minimizer_start_time - solver_start_time;
@@ -572,30 +404,17 @@
inner_iteration_minimizer.get(),
evaluator.get(),
linear_solver.get(),
- parameters.data(),
summary);
event_logger.AddEvent("Minimize");
- SetSummaryFinalCost(summary);
-
- // If the user aborted mid-optimization or the optimization
- // terminated because of a numerical failure, then return without
- // updating user state.
- if (summary->termination_type == USER_ABORT ||
- summary->termination_type == NUMERICAL_FAILURE) {
- return;
- }
-
double post_process_start_time = WallTimeInSeconds();
- // Push the contiguous optimized parameters back to the user's
- // parameters.
- reduced_program->StateVectorToParameterBlocks(parameters.data());
- reduced_program->CopyParameterBlockStateToUserState();
+ SetSummaryFinalCost(summary);
// Ensure the program state is set to the user parameters on the way
// out.
original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+ original_program->SetParameterOffsetsAndIndex();
const map<string, double>& linear_solver_time_statistics =
linear_solver->TimeStatistics();
@@ -618,8 +437,6 @@
event_logger.AddEvent("PostProcess");
}
-
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
void SolverImpl::LineSearchSolve(const Solver::Options& original_options,
ProblemImpl* original_problem_impl,
Solver::Summary* summary) {
@@ -628,9 +445,7 @@
Program* original_program = original_problem_impl->mutable_program();
ProblemImpl* problem_impl = original_problem_impl;
- // Reset the summary object to its default values.
- *CHECK_NOTNULL(summary) = Solver::Summary();
-
+ SummarizeGivenProgram(*original_program, summary);
summary->minimizer_type = LINE_SEARCH;
summary->line_search_direction_type =
original_options.line_search_direction_type;
@@ -641,104 +456,9 @@
summary->nonlinear_conjugate_gradient_type =
original_options.nonlinear_conjugate_gradient_type;
- summary->num_parameter_blocks = original_program->NumParameterBlocks();
- summary->num_parameters = original_program->NumParameters();
- summary->num_residual_blocks = original_program->NumResidualBlocks();
- summary->num_residuals = original_program->NumResiduals();
- summary->num_effective_parameters =
- original_program->NumEffectiveParameters();
-
- // Validate values for configuration parameters supplied by user.
- if ((original_options.line_search_direction_type == ceres::BFGS ||
- original_options.line_search_direction_type == ceres::LBFGS) &&
- original_options.line_search_type != ceres::WOLFE) {
- summary->error =
- string("Invalid configuration: require line_search_type == "
- "ceres::WOLFE when using (L)BFGS to ensure that underlying "
- "assumptions are guaranteed to be satisfied.");
- LOG(ERROR) << summary->error;
- return;
- }
- if (original_options.max_lbfgs_rank <= 0) {
- summary->error =
- string("Invalid configuration: require max_lbfgs_rank > 0");
- LOG(ERROR) << summary->error;
- return;
- }
- if (original_options.min_line_search_step_size <= 0.0) {
- summary->error = "Invalid configuration: min_line_search_step_size <= 0.0.";
- LOG(ERROR) << summary->error;
- return;
- }
- if (original_options.line_search_sufficient_function_decrease <= 0.0) {
- summary->error =
- string("Invalid configuration: require ") +
- string("line_search_sufficient_function_decrease <= 0.0.");
- LOG(ERROR) << summary->error;
- return;
- }
- if (original_options.max_line_search_step_contraction <= 0.0 ||
- original_options.max_line_search_step_contraction >= 1.0) {
- summary->error = string("Invalid configuration: require ") +
- string("0.0 < max_line_search_step_contraction < 1.0.");
- LOG(ERROR) << summary->error;
- return;
- }
- if (original_options.min_line_search_step_contraction <=
- original_options.max_line_search_step_contraction ||
- original_options.min_line_search_step_contraction > 1.0) {
- summary->error = string("Invalid configuration: require ") +
- string("max_line_search_step_contraction < ") +
- string("min_line_search_step_contraction <= 1.0.");
- LOG(ERROR) << summary->error;
- return;
- }
- // Warn user if they have requested BISECTION interpolation, but constraints
- // on max/min step size change during line search prevent bisection scaling
- // from occurring. Warn only, as this is likely a user mistake, but one which
- // does not prevent us from continuing.
- LOG_IF(WARNING,
- (original_options.line_search_interpolation_type == ceres::BISECTION &&
- (original_options.max_line_search_step_contraction > 0.5 ||
- original_options.min_line_search_step_contraction < 0.5)))
- << "Line search interpolation type is BISECTION, but specified "
- << "max_line_search_step_contraction: "
- << original_options.max_line_search_step_contraction << ", and "
- << "min_line_search_step_contraction: "
- << original_options.min_line_search_step_contraction
- << ", prevent bisection (0.5) scaling, continuing with solve regardless.";
- if (original_options.max_num_line_search_step_size_iterations <= 0) {
- summary->error = string("Invalid configuration: require ") +
- string("max_num_line_search_step_size_iterations > 0.");
- LOG(ERROR) << summary->error;
- return;
- }
- if (original_options.line_search_sufficient_curvature_decrease <=
- original_options.line_search_sufficient_function_decrease ||
- original_options.line_search_sufficient_curvature_decrease > 1.0) {
- summary->error = string("Invalid configuration: require ") +
- string("line_search_sufficient_function_decrease < ") +
- string("line_search_sufficient_curvature_decrease < 1.0.");
- LOG(ERROR) << summary->error;
- return;
- }
- if (original_options.max_line_search_step_expansion <= 1.0) {
- summary->error = string("Invalid configuration: require ") +
- string("max_line_search_step_expansion > 1.0.");
- LOG(ERROR) << summary->error;
- return;
- }
-
- // Empty programs are usually a user error.
- if (summary->num_parameter_blocks == 0) {
- summary->error = "Problem contains no parameter blocks.";
- LOG(ERROR) << summary->error;
- return;
- }
-
- if (summary->num_residual_blocks == 0) {
- summary->error = "Problem contains no residual blocks.";
- LOG(ERROR) << summary->error;
+ if (original_program->IsBoundsConstrained()) {
+ summary->message = "LINE_SEARCH Minimizer does not support bounds.";
+ LOG(ERROR) << "Terminating: " << summary->message;
return;
}
@@ -750,8 +470,6 @@
// line search.
options.linear_solver_type = CGNR;
- options.linear_solver_ordering = NULL;
- options.inner_iteration_ordering = NULL;
#ifndef CERES_USE_OPENMP
if (options.num_threads > 1) {
@@ -766,15 +484,18 @@
summary->num_threads_given = original_options.num_threads;
summary->num_threads_used = options.num_threads;
- if (original_options.linear_solver_ordering != NULL) {
- if (!IsOrderingValid(original_options, problem_impl, &summary->error)) {
- LOG(ERROR) << summary->error;
+ if (!original_program->ParameterBlocksAreFinite(&summary->message)) {
+ LOG(ERROR) << "Terminating: " << summary->message;
+ return;
+ }
+
+ if (options.linear_solver_ordering.get() != NULL) {
+ if (!IsOrderingValid(options, problem_impl, &summary->message)) {
+ LOG(ERROR) << summary->message;
return;
}
- options.linear_solver_ordering =
- new ParameterBlockOrdering(*original_options.linear_solver_ordering);
} else {
- options.linear_solver_ordering = new ParameterBlockOrdering;
+ options.linear_solver_ordering.reset(new ParameterBlockOrdering);
const ProblemImpl::ParameterMap& parameter_map =
problem_impl->parameter_map();
for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin();
@@ -784,6 +505,7 @@
}
}
+
original_program->SetParameterBlockStatePtrsToUserStatePtrs();
// If the user requests gradient checking, construct a new
@@ -809,36 +531,31 @@
scoped_ptr<Program> reduced_program(CreateReducedProgram(&options,
problem_impl,
&summary->fixed_cost,
- &summary->error));
+ &summary->message));
if (reduced_program == NULL) {
return;
}
- summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks();
- summary->num_parameters_reduced = reduced_program->NumParameters();
- summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks();
- summary->num_effective_parameters_reduced =
- reduced_program->NumEffectiveParameters();
- summary->num_residuals_reduced = reduced_program->NumResiduals();
-
+ SummarizeReducedProgram(*reduced_program, summary);
if (summary->num_parameter_blocks_reduced == 0) {
summary->preprocessor_time_in_seconds =
WallTimeInSeconds() - solver_start_time;
- LOG(INFO) << "Terminating: FUNCTION_TOLERANCE reached. "
- << "No non-constant parameter blocks found.";
-
- // FUNCTION_TOLERANCE is the right convergence here, as we know
- // that the objective function is constant and cannot be changed
- // any further.
- summary->termination_type = FUNCTION_TOLERANCE;
+ summary->message =
+ "Function tolerance reached. "
+ "No non-constant parameter blocks found.";
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, options.logging_type != SILENT) << summary->message;
+ summary->initial_cost = summary->fixed_cost;
+ summary->final_cost = summary->fixed_cost;
const double post_process_start_time = WallTimeInSeconds();
-
SetSummaryFinalCost(summary);
// Ensure the program state is set to the user parameters on the way out.
original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+ original_program->SetParameterOffsetsAndIndex();
+
summary->postprocessor_time_in_seconds =
WallTimeInSeconds() - post_process_start_time;
return;
@@ -847,48 +564,25 @@
scoped_ptr<Evaluator> evaluator(CreateEvaluator(options,
problem_impl->parameter_map(),
reduced_program.get(),
- &summary->error));
+ &summary->message));
if (evaluator == NULL) {
return;
}
- // The optimizer works on contiguous parameter vectors; allocate some.
- Vector parameters(reduced_program->NumParameters());
-
- // Collect the discontiguous parameters into a contiguous state vector.
- reduced_program->ParameterBlocksToStateVector(parameters.data());
-
- Vector original_parameters = parameters;
-
const double minimizer_start_time = WallTimeInSeconds();
summary->preprocessor_time_in_seconds =
minimizer_start_time - solver_start_time;
// Run the optimization.
- LineSearchMinimize(options,
- reduced_program.get(),
- evaluator.get(),
- parameters.data(),
- summary);
-
- // If the user aborted mid-optimization or the optimization
- // terminated because of a numerical failure, then return without
- // updating user state.
- if (summary->termination_type == USER_ABORT ||
- summary->termination_type == NUMERICAL_FAILURE) {
- return;
- }
+ LineSearchMinimize(options, reduced_program.get(), evaluator.get(), summary);
const double post_process_start_time = WallTimeInSeconds();
- // Push the contiguous optimized parameters back to the user's parameters.
- reduced_program->StateVectorToParameterBlocks(parameters.data());
- reduced_program->CopyParameterBlockStateToUserState();
-
SetSummaryFinalCost(summary);
// Ensure the program state is set to the user parameters on the way out.
original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+ original_program->SetParameterOffsetsAndIndex();
const map<string, double>& evaluator_time_statistics =
evaluator->TimeStatistics();
@@ -902,7 +596,6 @@
summary->postprocessor_time_in_seconds =
WallTimeInSeconds() - post_process_start_time;
}
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
bool SolverImpl::IsOrderingValid(const Solver::Options& options,
const ProblemImpl* problem_impl,
@@ -966,133 +659,48 @@
return true;
}
-
-// Strips varying parameters and residuals, maintaining order, and updating
-// num_eliminate_blocks.
-bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program,
- ParameterBlockOrdering* ordering,
- double* fixed_cost,
- string* error) {
- vector<ParameterBlock*>* parameter_blocks =
- program->mutable_parameter_blocks();
-
- scoped_array<double> residual_block_evaluate_scratch;
- if (fixed_cost != NULL) {
- residual_block_evaluate_scratch.reset(
- new double[program->MaxScratchDoublesNeededForEvaluate()]);
- *fixed_cost = 0.0;
- }
-
- // Mark all the parameters as unused. Abuse the index member of the parameter
- // blocks for the marking.
- for (int i = 0; i < parameter_blocks->size(); ++i) {
- (*parameter_blocks)[i]->set_index(-1);
- }
-
- // Filter out residual that have all-constant parameters, and mark all the
- // parameter blocks that appear in residuals.
- {
- vector<ResidualBlock*>* residual_blocks =
- program->mutable_residual_blocks();
- int j = 0;
- for (int i = 0; i < residual_blocks->size(); ++i) {
- ResidualBlock* residual_block = (*residual_blocks)[i];
- int num_parameter_blocks = residual_block->NumParameterBlocks();
-
- // Determine if the residual block is fixed, and also mark varying
- // parameters that appear in the residual block.
- bool all_constant = true;
- for (int k = 0; k < num_parameter_blocks; k++) {
- ParameterBlock* parameter_block = residual_block->parameter_blocks()[k];
- if (!parameter_block->IsConstant()) {
- all_constant = false;
- parameter_block->set_index(1);
- }
- }
-
- if (!all_constant) {
- (*residual_blocks)[j++] = (*residual_blocks)[i];
- } else if (fixed_cost != NULL) {
- // The residual is constant and will be removed, so its cost is
- // added to the variable fixed_cost.
- double cost = 0.0;
- if (!residual_block->Evaluate(true,
- &cost,
- NULL,
- NULL,
- residual_block_evaluate_scratch.get())) {
- *error = StringPrintf("Evaluation of the residual %d failed during "
- "removal of fixed residual blocks.", i);
- return false;
- }
- *fixed_cost += cost;
- }
- }
- residual_blocks->resize(j);
- }
-
- // Filter out unused or fixed parameter blocks, and update
- // the ordering.
- {
- vector<ParameterBlock*>* parameter_blocks =
- program->mutable_parameter_blocks();
- int j = 0;
- for (int i = 0; i < parameter_blocks->size(); ++i) {
- ParameterBlock* parameter_block = (*parameter_blocks)[i];
- if (parameter_block->index() == 1) {
- (*parameter_blocks)[j++] = parameter_block;
- } else {
- ordering->Remove(parameter_block->mutable_user_state());
- }
- }
- parameter_blocks->resize(j);
- }
-
- if (!(((program->NumResidualBlocks() == 0) &&
- (program->NumParameterBlocks() == 0)) ||
- ((program->NumResidualBlocks() != 0) &&
- (program->NumParameterBlocks() != 0)))) {
- *error = "Congratulations, you found a bug in Ceres. Please report it.";
- return false;
- }
-
- return true;
-}
-
Program* SolverImpl::CreateReducedProgram(Solver::Options* options,
ProblemImpl* problem_impl,
double* fixed_cost,
string* error) {
- CHECK_NOTNULL(options->linear_solver_ordering);
+ CHECK_NOTNULL(options->linear_solver_ordering.get());
Program* original_program = problem_impl->mutable_program();
- scoped_ptr<Program> transformed_program(new Program(*original_program));
- ParameterBlockOrdering* linear_solver_ordering =
- options->linear_solver_ordering;
- const int min_group_id =
- linear_solver_ordering->group_to_elements().begin()->first;
-
- if (!RemoveFixedBlocksFromProgram(transformed_program.get(),
- linear_solver_ordering,
- fixed_cost,
- error)) {
+ vector<double*> removed_parameter_blocks;
+ scoped_ptr<Program> reduced_program(
+ original_program->CreateReducedProgram(&removed_parameter_blocks,
+ fixed_cost,
+ error));
+ if (reduced_program.get() == NULL) {
return NULL;
}
VLOG(2) << "Reduced problem: "
- << transformed_program->NumParameterBlocks()
+ << reduced_program->NumParameterBlocks()
<< " parameter blocks, "
- << transformed_program->NumParameters()
+ << reduced_program->NumParameters()
<< " parameters, "
- << transformed_program->NumResidualBlocks()
+ << reduced_program->NumResidualBlocks()
<< " residual blocks, "
- << transformed_program->NumResiduals()
+ << reduced_program->NumResiduals()
<< " residuals.";
- if (transformed_program->NumParameterBlocks() == 0) {
+ if (reduced_program->NumParameterBlocks() == 0) {
LOG(WARNING) << "No varying parameter blocks to optimize; "
<< "bailing early.";
- return transformed_program.release();
+ return reduced_program.release();
+ }
+
+ ParameterBlockOrdering* linear_solver_ordering =
+ options->linear_solver_ordering.get();
+ const int min_group_id =
+ linear_solver_ordering->MinNonZeroGroup();
+ linear_solver_ordering->Remove(removed_parameter_blocks);
+
+ ParameterBlockOrdering* inner_iteration_ordering =
+ options->inner_iteration_ordering.get();
+ if (inner_iteration_ordering != NULL) {
+ inner_iteration_ordering->Remove(removed_parameter_blocks);
}
if (IsSchurType(options->linear_solver_type) &&
@@ -1108,7 +716,15 @@
// as they assume there is at least one e_block. Thus, we
// automatically switch to the closest solver to the one indicated
// by the user.
- AlternateLinearSolverForSchurTypeLinearSolver(options);
+ if (options->linear_solver_type == ITERATIVE_SCHUR) {
+ options->preconditioner_type =
+ Preconditioner::PreconditionerForZeroEBlocks(
+ options->preconditioner_type);
+ }
+
+ options->linear_solver_type =
+ LinearSolver::LinearSolverForZeroEBlocks(
+ options->linear_solver_type);
}
if (IsSchurType(options->linear_solver_type)) {
@@ -1117,33 +733,34 @@
options->sparse_linear_algebra_library_type,
problem_impl->parameter_map(),
linear_solver_ordering,
- transformed_program.get(),
+ reduced_program.get(),
error)) {
return NULL;
}
- return transformed_program.release();
+ return reduced_program.release();
}
- if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
+ if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY &&
+ !options->dynamic_sparsity) {
if (!ReorderProgramForSparseNormalCholesky(
options->sparse_linear_algebra_library_type,
- linear_solver_ordering,
- transformed_program.get(),
+ *linear_solver_ordering,
+ reduced_program.get(),
error)) {
return NULL;
}
- return transformed_program.release();
+ return reduced_program.release();
}
- transformed_program->SetParameterOffsetsAndIndex();
- return transformed_program.release();
+ reduced_program->SetParameterOffsetsAndIndex();
+ return reduced_program.release();
}
LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options,
string* error) {
CHECK_NOTNULL(options);
- CHECK_NOTNULL(options->linear_solver_ordering);
+ CHECK_NOTNULL(options->linear_solver_ordering.get());
CHECK_NOTNULL(error);
if (options->trust_region_strategy_type == DOGLEG) {
@@ -1209,14 +826,6 @@
}
#endif
-#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
- if (options->linear_solver_type == SPARSE_SCHUR) {
- *error = "Can't use SPARSE_SCHUR because neither SuiteSparse nor"
- "CXSparse was enabled when Ceres was compiled.";
- return NULL;
- }
-#endif
-
if (options->max_linear_solver_iterations <= 0) {
*error = "Solver::Options::max_linear_solver_iterations is not positive.";
return NULL;
@@ -1239,11 +848,14 @@
options->max_linear_solver_iterations;
linear_solver_options.type = options->linear_solver_type;
linear_solver_options.preconditioner_type = options->preconditioner_type;
+ linear_solver_options.visibility_clustering_type =
+ options->visibility_clustering_type;
linear_solver_options.sparse_linear_algebra_library_type =
options->sparse_linear_algebra_library_type;
linear_solver_options.dense_linear_algebra_library_type =
options->dense_linear_algebra_library_type;
linear_solver_options.use_postordering = options->use_postordering;
+ linear_solver_options.dynamic_sparsity = options->dynamic_sparsity;
// Ignore user's postordering preferences and force it to be true if
// cholmod_camd is not available. This ensures that the linear
@@ -1259,13 +871,8 @@
linear_solver_options.num_threads = options->num_linear_solver_threads;
options->num_linear_solver_threads = linear_solver_options.num_threads;
- const map<int, set<double*> >& groups =
- options->linear_solver_ordering->group_to_elements();
- for (map<int, set<double*> >::const_iterator it = groups.begin();
- it != groups.end();
- ++it) {
- linear_solver_options.elimination_groups.push_back(it->second.size());
- }
+ OrderingToGroupSizes(options->linear_solver_ordering.get(),
+ &linear_solver_options.elimination_groups);
// Schur type solvers, expect at least two elimination groups. If
// there is only one elimination group, then CreateReducedProgram
// guarantees that this group only contains e_blocks. Thus we add a
@@ -1278,109 +885,6 @@
return LinearSolver::Create(linear_solver_options);
}
-
-// Find the minimum index of any parameter block to the given residual.
-// Parameter blocks that have indices greater than num_eliminate_blocks are
-// considered to have an index equal to num_eliminate_blocks.
-static int MinParameterBlock(const ResidualBlock* residual_block,
- int num_eliminate_blocks) {
- int min_parameter_block_position = num_eliminate_blocks;
- for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) {
- ParameterBlock* parameter_block = residual_block->parameter_blocks()[i];
- if (!parameter_block->IsConstant()) {
- CHECK_NE(parameter_block->index(), -1)
- << "Did you forget to call Program::SetParameterOffsetsAndIndex()? "
- << "This is a Ceres bug; please contact the developers!";
- min_parameter_block_position = std::min(parameter_block->index(),
- min_parameter_block_position);
- }
- }
- return min_parameter_block_position;
-}
-
-// Reorder the residuals for program, if necessary, so that the residuals
-// involving each E block occur together. This is a necessary condition for the
-// Schur eliminator, which works on these "row blocks" in the jacobian.
-bool SolverImpl::LexicographicallyOrderResidualBlocks(
- const int num_eliminate_blocks,
- Program* program,
- string* error) {
- CHECK_GE(num_eliminate_blocks, 1)
- << "Congratulations, you found a Ceres bug! Please report this error "
- << "to the developers.";
-
- // Create a histogram of the number of residuals for each E block. There is an
- // extra bucket at the end to catch all non-eliminated F blocks.
- vector<int> residual_blocks_per_e_block(num_eliminate_blocks + 1);
- vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks();
- vector<int> min_position_per_residual(residual_blocks->size());
- for (int i = 0; i < residual_blocks->size(); ++i) {
- ResidualBlock* residual_block = (*residual_blocks)[i];
- int position = MinParameterBlock(residual_block, num_eliminate_blocks);
- min_position_per_residual[i] = position;
- DCHECK_LE(position, num_eliminate_blocks);
- residual_blocks_per_e_block[position]++;
- }
-
- // Run a cumulative sum on the histogram, to obtain offsets to the start of
- // each histogram bucket (where each bucket is for the residuals for that
- // E-block).
- vector<int> offsets(num_eliminate_blocks + 1);
- std::partial_sum(residual_blocks_per_e_block.begin(),
- residual_blocks_per_e_block.end(),
- offsets.begin());
- CHECK_EQ(offsets.back(), residual_blocks->size())
- << "Congratulations, you found a Ceres bug! Please report this error "
- << "to the developers.";
-
- CHECK(find(residual_blocks_per_e_block.begin(),
- residual_blocks_per_e_block.end() - 1, 0) !=
- residual_blocks_per_e_block.end())
- << "Congratulations, you found a Ceres bug! Please report this error "
- << "to the developers.";
-
- // Fill in each bucket with the residual blocks for its corresponding E block.
- // Each bucket is individually filled from the back of the bucket to the front
- // of the bucket. The filling order among the buckets is dictated by the
- // residual blocks. This loop uses the offsets as counters; subtracting one
- // from each offset as a residual block is placed in the bucket. When the
- // filling is finished, the offset pointerts should have shifted down one
- // entry (this is verified below).
- vector<ResidualBlock*> reordered_residual_blocks(
- (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL));
- for (int i = 0; i < residual_blocks->size(); ++i) {
- int bucket = min_position_per_residual[i];
-
- // Decrement the cursor, which should now point at the next empty position.
- offsets[bucket]--;
-
- // Sanity.
- CHECK(reordered_residual_blocks[offsets[bucket]] == NULL)
- << "Congratulations, you found a Ceres bug! Please report this error "
- << "to the developers.";
-
- reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i];
- }
-
- // Sanity check #1: The difference in bucket offsets should match the
- // histogram sizes.
- for (int i = 0; i < num_eliminate_blocks; ++i) {
- CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i])
- << "Congratulations, you found a Ceres bug! Please report this error "
- << "to the developers.";
- }
- // Sanity check #2: No NULL's left behind.
- for (int i = 0; i < reordered_residual_blocks.size(); ++i) {
- CHECK(reordered_residual_blocks[i] != NULL)
- << "Congratulations, you found a Ceres bug! Please report this error "
- << "to the developers.";
- }
-
- // Now that the residuals are collected by E block, swap them in place.
- swap(*program->mutable_residual_blocks(), reordered_residual_blocks);
- return true;
-}
-
Evaluator* SolverImpl::CreateEvaluator(
const Solver::Options& options,
const ProblemImpl::ParameterMap& parameter_map,
@@ -1396,6 +900,7 @@
->second.size())
: 0;
evaluator_options.num_threads = options.num_threads;
+ evaluator_options.dynamic_sparsity = options.dynamic_sparsity;
return Evaluator::Create(evaluator_options, program, error);
}
@@ -1411,374 +916,32 @@
scoped_ptr<ParameterBlockOrdering> inner_iteration_ordering;
ParameterBlockOrdering* ordering_ptr = NULL;
- if (options.inner_iteration_ordering == NULL) {
- // Find a recursive decomposition of the Hessian matrix as a set
- // of independent sets of decreasing size and invert it. This
- // seems to work better in practice, i.e., Cameras before
- // points.
- inner_iteration_ordering.reset(new ParameterBlockOrdering);
- ComputeRecursiveIndependentSetOrdering(program,
- inner_iteration_ordering.get());
- inner_iteration_ordering->Reverse();
+ if (options.inner_iteration_ordering.get() == NULL) {
+ inner_iteration_ordering.reset(
+ CoordinateDescentMinimizer::CreateOrdering(program));
ordering_ptr = inner_iteration_ordering.get();
} else {
- const map<int, set<double*> >& group_to_elements =
- options.inner_iteration_ordering->group_to_elements();
-
- // Iterate over each group and verify that it is an independent
- // set.
- map<int, set<double*> >::const_iterator it = group_to_elements.begin();
- for ( ; it != group_to_elements.end(); ++it) {
- if (!IsParameterBlockSetIndependent(it->second,
- program.residual_blocks())) {
- summary->error =
- StringPrintf("The user-provided "
- "parameter_blocks_for_inner_iterations does not "
- "form an independent set. Group Id: %d", it->first);
- return NULL;
- }
+ ordering_ptr = options.inner_iteration_ordering.get();
+ if (!CoordinateDescentMinimizer::IsOrderingValid(program,
+ *ordering_ptr,
+ &summary->message)) {
+ return NULL;
}
- ordering_ptr = options.inner_iteration_ordering;
}
if (!inner_iteration_minimizer->Init(program,
parameter_map,
*ordering_ptr,
- &summary->error)) {
+ &summary->message)) {
return NULL;
}
summary->inner_iterations_used = true;
summary->inner_iteration_time_in_seconds = 0.0;
- SummarizeOrdering(ordering_ptr, &(summary->inner_iteration_ordering_used));
+ OrderingToGroupSizes(ordering_ptr,
+ &(summary->inner_iteration_ordering_used));
return inner_iteration_minimizer.release();
}
-void SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(
- Solver::Options* options) {
- if (!IsSchurType(options->linear_solver_type)) {
- return;
- }
-
- string msg = "No e_blocks remaining. Switching from ";
- if (options->linear_solver_type == SPARSE_SCHUR) {
- options->linear_solver_type = SPARSE_NORMAL_CHOLESKY;
- msg += "SPARSE_SCHUR to SPARSE_NORMAL_CHOLESKY.";
- } else if (options->linear_solver_type == DENSE_SCHUR) {
- // TODO(sameeragarwal): This is probably not a great choice.
- // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can
- // take a BlockSparseMatrix as input.
- options->linear_solver_type = DENSE_QR;
- msg += "DENSE_SCHUR to DENSE_QR.";
- } else if (options->linear_solver_type == ITERATIVE_SCHUR) {
- options->linear_solver_type = CGNR;
- if (options->preconditioner_type != IDENTITY) {
- msg += StringPrintf("ITERATIVE_SCHUR with %s preconditioner "
- "to CGNR with JACOBI preconditioner.",
- PreconditionerTypeToString(
- options->preconditioner_type));
- // CGNR currently only supports the JACOBI preconditioner.
- options->preconditioner_type = JACOBI;
- } else {
- msg += "ITERATIVE_SCHUR with IDENTITY preconditioner"
- "to CGNR with IDENTITY preconditioner.";
- }
- }
- LOG(WARNING) << msg;
-}
-
-bool SolverImpl::ApplyUserOrdering(
- const ProblemImpl::ParameterMap& parameter_map,
- const ParameterBlockOrdering* parameter_block_ordering,
- Program* program,
- string* error) {
- const int num_parameter_blocks = program->NumParameterBlocks();
- if (parameter_block_ordering->NumElements() != num_parameter_blocks) {
- *error = StringPrintf("User specified ordering does not have the same "
- "number of parameters as the problem. The problem"
- "has %d blocks while the ordering has %d blocks.",
- num_parameter_blocks,
- parameter_block_ordering->NumElements());
- return false;
- }
-
- vector<ParameterBlock*>* parameter_blocks =
- program->mutable_parameter_blocks();
- parameter_blocks->clear();
-
- const map<int, set<double*> >& groups =
- parameter_block_ordering->group_to_elements();
-
- for (map<int, set<double*> >::const_iterator group_it = groups.begin();
- group_it != groups.end();
- ++group_it) {
- const set<double*>& group = group_it->second;
- for (set<double*>::const_iterator parameter_block_ptr_it = group.begin();
- parameter_block_ptr_it != group.end();
- ++parameter_block_ptr_it) {
- ProblemImpl::ParameterMap::const_iterator parameter_block_it =
- parameter_map.find(*parameter_block_ptr_it);
- if (parameter_block_it == parameter_map.end()) {
- *error = StringPrintf("User specified ordering contains a pointer "
- "to a double that is not a parameter block in "
- "the problem. The invalid double is in group: %d",
- group_it->first);
- return false;
- }
- parameter_blocks->push_back(parameter_block_it->second);
- }
- }
- return true;
-}
-
-
-TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose(
- const Program* program) {
-
- // Matrix to store the block sparsity structure of the Jacobian.
- TripletSparseMatrix* tsm =
- new TripletSparseMatrix(program->NumParameterBlocks(),
- program->NumResidualBlocks(),
- 10 * program->NumResidualBlocks());
- int num_nonzeros = 0;
- int* rows = tsm->mutable_rows();
- int* cols = tsm->mutable_cols();
- double* values = tsm->mutable_values();
-
- const vector<ResidualBlock*>& residual_blocks = program->residual_blocks();
- for (int c = 0; c < residual_blocks.size(); ++c) {
- const ResidualBlock* residual_block = residual_blocks[c];
- const int num_parameter_blocks = residual_block->NumParameterBlocks();
- ParameterBlock* const* parameter_blocks =
- residual_block->parameter_blocks();
-
- for (int j = 0; j < num_parameter_blocks; ++j) {
- if (parameter_blocks[j]->IsConstant()) {
- continue;
- }
-
- // Re-size the matrix if needed.
- if (num_nonzeros >= tsm->max_num_nonzeros()) {
- tsm->set_num_nonzeros(num_nonzeros);
- tsm->Reserve(2 * num_nonzeros);
- rows = tsm->mutable_rows();
- cols = tsm->mutable_cols();
- values = tsm->mutable_values();
- }
- CHECK_LT(num_nonzeros, tsm->max_num_nonzeros());
-
- const int r = parameter_blocks[j]->index();
- rows[num_nonzeros] = r;
- cols[num_nonzeros] = c;
- values[num_nonzeros] = 1.0;
- ++num_nonzeros;
- }
- }
-
- tsm->set_num_nonzeros(num_nonzeros);
- return tsm;
-}
-
-bool SolverImpl::ReorderProgramForSchurTypeLinearSolver(
- const LinearSolverType linear_solver_type,
- const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
- const ProblemImpl::ParameterMap& parameter_map,
- ParameterBlockOrdering* parameter_block_ordering,
- Program* program,
- string* error) {
- if (parameter_block_ordering->NumGroups() == 1) {
- // If the user supplied an parameter_block_ordering with just one
- // group, it is equivalent to the user supplying NULL as an
- // parameter_block_ordering. Ceres is completely free to choose the
- // parameter block ordering as it sees fit. For Schur type solvers,
- // this means that the user wishes for Ceres to identify the
- // e_blocks, which we do by computing a maximal independent set.
- vector<ParameterBlock*> schur_ordering;
- const int num_eliminate_blocks =
- ComputeStableSchurOrdering(*program, &schur_ordering);
-
- CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks())
- << "Congratulations, you found a Ceres bug! Please report this error "
- << "to the developers.";
-
- // Update the parameter_block_ordering object.
- for (int i = 0; i < schur_ordering.size(); ++i) {
- double* parameter_block = schur_ordering[i]->mutable_user_state();
- const int group_id = (i < num_eliminate_blocks) ? 0 : 1;
- parameter_block_ordering->AddElementToGroup(parameter_block, group_id);
- }
-
- // We could call ApplyUserOrdering but this is cheaper and
- // simpler.
- swap(*program->mutable_parameter_blocks(), schur_ordering);
- } else {
- // The user provided an ordering with more than one elimination
- // group. Trust the user and apply the ordering.
- if (!ApplyUserOrdering(parameter_map,
- parameter_block_ordering,
- program,
- error)) {
- return false;
- }
- }
-
- // Pre-order the columns corresponding to the schur complement if
- // possible.
-#if !defined(CERES_NO_SUITESPARSE) && !defined(CERES_NO_CAMD)
- if (linear_solver_type == SPARSE_SCHUR &&
- sparse_linear_algebra_library_type == SUITE_SPARSE) {
- vector<int> constraints;
- vector<ParameterBlock*>& parameter_blocks =
- *(program->mutable_parameter_blocks());
-
- for (int i = 0; i < parameter_blocks.size(); ++i) {
- constraints.push_back(
- parameter_block_ordering->GroupId(
- parameter_blocks[i]->mutable_user_state()));
- }
-
- // Renumber the entries of constraints to be contiguous integers
- // as camd requires that the group ids be in the range [0,
- // parameter_blocks.size() - 1].
- SolverImpl::CompactifyArray(&constraints);
-
- // Set the offsets and index for CreateJacobianSparsityTranspose.
- program->SetParameterOffsetsAndIndex();
- // Compute a block sparse presentation of J'.
- scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
-
- SuiteSparse ss;
- cholmod_sparse* block_jacobian_transpose =
- ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get());
-
- vector<int> ordering(parameter_blocks.size(), 0);
- ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose,
- &constraints[0],
- &ordering[0]);
- ss.Free(block_jacobian_transpose);
-
- const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
- for (int i = 0; i < program->NumParameterBlocks(); ++i) {
- parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
- }
- }
-#endif
-
- program->SetParameterOffsetsAndIndex();
- // Schur type solvers also require that their residual blocks be
- // lexicographically ordered.
- const int num_eliminate_blocks =
- parameter_block_ordering->group_to_elements().begin()->second.size();
- return LexicographicallyOrderResidualBlocks(num_eliminate_blocks,
- program,
- error);
-}
-
-bool SolverImpl::ReorderProgramForSparseNormalCholesky(
- const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
- const ParameterBlockOrdering* parameter_block_ordering,
- Program* program,
- string* error) {
- // Set the offsets and index for CreateJacobianSparsityTranspose.
- program->SetParameterOffsetsAndIndex();
- // Compute a block sparse presentation of J'.
- scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
-
- vector<int> ordering(program->NumParameterBlocks(), 0);
- vector<ParameterBlock*>& parameter_blocks =
- *(program->mutable_parameter_blocks());
-
- if (sparse_linear_algebra_library_type == SUITE_SPARSE) {
-#ifdef CERES_NO_SUITESPARSE
- *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITE_SPARSE because "
- "SuiteSparse was not enabled when Ceres was built.";
- return false;
-#else
- SuiteSparse ss;
- cholmod_sparse* block_jacobian_transpose =
- ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get());
-
-# ifdef CERES_NO_CAMD
- // No cholmod_camd, so ignore user's parameter_block_ordering and
- // use plain old AMD.
- ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]);
-# else
- if (parameter_block_ordering->NumGroups() > 1) {
- // If the user specified more than one elimination groups use them
- // to constrain the ordering.
- vector<int> constraints;
- for (int i = 0; i < parameter_blocks.size(); ++i) {
- constraints.push_back(
- parameter_block_ordering->GroupId(
- parameter_blocks[i]->mutable_user_state()));
- }
- ss.ConstrainedApproximateMinimumDegreeOrdering(
- block_jacobian_transpose,
- &constraints[0],
- &ordering[0]);
- } else {
- ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose,
- &ordering[0]);
- }
-# endif // CERES_NO_CAMD
-
- ss.Free(block_jacobian_transpose);
-#endif // CERES_NO_SUITESPARSE
-
- } else if (sparse_linear_algebra_library_type == CX_SPARSE) {
-#ifndef CERES_NO_CXSPARSE
-
- // CXSparse works with J'J instead of J'. So compute the block
- // sparsity for J'J and compute an approximate minimum degree
- // ordering.
- CXSparse cxsparse;
- cs_di* block_jacobian_transpose;
- block_jacobian_transpose =
- cxsparse.CreateSparseMatrix(tsm_block_jacobian_transpose.get());
- cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose);
- cs_di* block_hessian =
- cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian);
- cxsparse.Free(block_jacobian);
- cxsparse.Free(block_jacobian_transpose);
-
- cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, &ordering[0]);
- cxsparse.Free(block_hessian);
-#else // CERES_NO_CXSPARSE
- *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because "
- "CXSparse was not enabled when Ceres was built.";
- return false;
-#endif // CERES_NO_CXSPARSE
- } else {
- *error = "Unknown sparse linear algebra library.";
- return false;
- }
-
- // Apply ordering.
- const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
- for (int i = 0; i < program->NumParameterBlocks(); ++i) {
- parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
- }
-
- program->SetParameterOffsetsAndIndex();
- return true;
-}
-
-void SolverImpl::CompactifyArray(vector<int>* array_ptr) {
- vector<int>& array = *array_ptr;
- const set<int> unique_group_ids(array.begin(), array.end());
- map<int, int> group_id_map;
- for (set<int>::const_iterator it = unique_group_ids.begin();
- it != unique_group_ids.end();
- ++it) {
- InsertOrDie(&group_id_map, *it, group_id_map.size());
- }
-
- for (int i = 0; i < array.size(); ++i) {
- array[i] = group_id_map[array[i]];
- }
-}
-
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h
index 2b7ca3e..c42c32a 100644
--- a/internal/ceres/solver_impl.h
+++ b/internal/ceres/solver_impl.h
@@ -67,10 +67,8 @@
CoordinateDescentMinimizer* inner_iteration_minimizer,
Evaluator* evaluator,
LinearSolver* linear_solver,
- double* parameters,
Solver::Summary* summary);
-#ifndef CERES_NO_LINE_SEARCH_MINIMIZER
static void LineSearchSolve(const Solver::Options& options,
ProblemImpl* problem_impl,
Solver::Summary* summary);
@@ -79,9 +77,7 @@
static void LineSearchMinimize(const Solver::Options &options,
Program* program,
Evaluator* evaluator,
- double* parameters,
Solver::Summary* summary);
-#endif // CERES_NO_LINE_SEARCH_MINIMIZER
// Create the transformed Program, which has all the fixed blocks
// and residuals eliminated, and in the case of automatic schur
@@ -93,7 +89,7 @@
static Program* CreateReducedProgram(Solver::Options* options,
ProblemImpl* problem_impl,
double* fixed_cost,
- string* error);
+ string* message);
// Create the appropriate linear solver, taking into account any
// config changes decided by CreateTransformedProgram(). The
@@ -101,38 +97,18 @@
// selected; consider the case that the remaining elimininated
// blocks is zero after removing fixed blocks.
static LinearSolver* CreateLinearSolver(Solver::Options* options,
- string* error);
-
- // Reorder the residuals for program, if necessary, so that the
- // residuals involving e block (i.e., the first num_eliminate_block
- // parameter blocks) occur together. This is a necessary condition
- // for the Schur eliminator.
- static bool LexicographicallyOrderResidualBlocks(
- const int num_eliminate_blocks,
- Program* program,
- string* error);
+ string* message);
// Create the appropriate evaluator for the transformed program.
static Evaluator* CreateEvaluator(
const Solver::Options& options,
const ProblemImpl::ParameterMap& parameter_map,
Program* program,
- string* error);
-
- // Remove the fixed or unused parameter blocks and residuals
- // depending only on fixed parameters from the problem. Also updates
- // num_eliminate_blocks, since removed parameters changes the point
- // at which the eliminated blocks is valid. If fixed_cost is not
- // NULL, the residual blocks that are removed are evaluated and the
- // sum of their cost is returned in fixed_cost.
- static bool RemoveFixedBlocksFromProgram(Program* program,
- ParameterBlockOrdering* ordering,
- double* fixed_cost,
- string* error);
+ string* message);
static bool IsOrderingValid(const Solver::Options& options,
const ProblemImpl* problem_impl,
- string* error);
+ string* message);
static bool IsParameterBlockSetIndependent(
const set<double*>& parameter_block_ptrs,
@@ -143,78 +119,6 @@
const Program& program,
const ProblemImpl::ParameterMap& parameter_map,
Solver::Summary* summary);
-
- // If the linear solver is of Schur type, then replace it with the
- // closest equivalent linear solver. This is done when the user
- // requested a Schur type solver but the problem structure makes it
- // impossible to use one.
- //
- // If the linear solver is not of Schur type, the function is a
- // no-op.
- static void AlternateLinearSolverForSchurTypeLinearSolver(
- Solver::Options* options);
-
- // Create a TripletSparseMatrix which contains the zero-one
- // structure corresponding to the block sparsity of the transpose of
- // the Jacobian matrix.
- //
- // Caller owns the result.
- static TripletSparseMatrix* CreateJacobianBlockSparsityTranspose(
- const Program* program);
-
- // Reorder the parameter blocks in program using the ordering
- static bool ApplyUserOrdering(
- const ProblemImpl::ParameterMap& parameter_map,
- const ParameterBlockOrdering* parameter_block_ordering,
- Program* program,
- string* error);
-
- // Sparse cholesky factorization routines when doing the sparse
- // cholesky factorization of the Jacobian matrix, reorders its
- // columns to reduce the fill-in. Compute this permutation and
- // re-order the parameter blocks.
- //
- // If the parameter_block_ordering contains more than one
- // elimination group and support for constrained fill-reducing
- // ordering is available in the sparse linear algebra library
- // (SuiteSparse version >= 4.2.0) then the fill reducing
- // ordering will take it into account, otherwise it will be ignored.
- static bool ReorderProgramForSparseNormalCholesky(
- const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
- const ParameterBlockOrdering* parameter_block_ordering,
- Program* program,
- string* error);
-
- // Schur type solvers require that all parameter blocks eliminated
- // by the Schur eliminator occur before others and the residuals be
- // sorted in lexicographic order of their parameter blocks.
- //
- // If the parameter_block_ordering only contains one elimination
- // group then a maximal independent set is computed and used as the
- // first elimination group, otherwise the user's ordering is used.
- //
- // If the linear solver type is SPARSE_SCHUR and support for
- // constrained fill-reducing ordering is available in the sparse
- // linear algebra library (SuiteSparse version >= 4.2.0) then
- // columns of the schur complement matrix are ordered to reduce the
- // fill-in the Cholesky factorization.
- //
- // Upon return, ordering contains the parameter block ordering that
- // was used to order the program.
- static bool ReorderProgramForSchurTypeLinearSolver(
- const LinearSolverType linear_solver_type,
- const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
- const ProblemImpl::ParameterMap& parameter_map,
- ParameterBlockOrdering* parameter_block_ordering,
- Program* program,
- string* error);
-
- // array contains a list of (possibly repeating) non-negative
- // integers. Let us assume that we have constructed another array
- // `p` by sorting and uniqueing the entries of array.
- // CompactifyArray replaces each entry in "array" with its position
- // in `p`.
- static void CompactifyArray(vector<int>* array);
};
} // namespace internal
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc
index 583ef4e..2d517c6 100644
--- a/internal/ceres/solver_impl_test.cc
+++ b/internal/ceres/solver_impl_test.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2014 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -42,660 +42,6 @@
namespace ceres {
namespace internal {
-// A cost function that sipmply returns its argument.
-class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
- public:
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- residuals[0] = parameters[0][0];
- if (jacobians != NULL && jacobians[0] != NULL) {
- jacobians[0][0] = 1.0;
- }
- return true;
- }
-};
-
-// Templated base class for the CostFunction signatures.
-template <int kNumResiduals, int N0, int N1, int N2>
-class MockCostFunctionBase : public
-SizedCostFunction<kNumResiduals, N0, N1, N2> {
- public:
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- // Do nothing. This is never called.
- return true;
- }
-};
-
-class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
-class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
-class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
-
-TEST(SolverImpl, RemoveFixedBlocksNothingConstant) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
- problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
-
- string error;
- {
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
- ordering.AddElementToGroup(&y, 0);
- ordering.AddElementToGroup(&z, 0);
-
- Program program(*problem.mutable_program());
- EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
- &ordering,
- NULL,
- &error));
- EXPECT_EQ(program.NumParameterBlocks(), 3);
- EXPECT_EQ(program.NumResidualBlocks(), 3);
- EXPECT_EQ(ordering.NumElements(), 3);
- }
-}
-
-TEST(SolverImpl, RemoveFixedBlocksAllParameterBlocksConstant) {
- ProblemImpl problem;
- double x;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
- problem.SetParameterBlockConstant(&x);
-
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
-
- Program program(problem.program());
- string error;
- EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
- &ordering,
- NULL,
- &error));
- EXPECT_EQ(program.NumParameterBlocks(), 0);
- EXPECT_EQ(program.NumResidualBlocks(), 0);
- EXPECT_EQ(ordering.NumElements(), 0);
-}
-
-TEST(SolverImpl, RemoveFixedBlocksNoResidualBlocks) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
-
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
- ordering.AddElementToGroup(&y, 0);
- ordering.AddElementToGroup(&z, 0);
-
-
- Program program(problem.program());
- string error;
- EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
- &ordering,
- NULL,
- &error));
- EXPECT_EQ(program.NumParameterBlocks(), 0);
- EXPECT_EQ(program.NumResidualBlocks(), 0);
- EXPECT_EQ(ordering.NumElements(), 0);
-}
-
-TEST(SolverImpl, RemoveFixedBlocksOneParameterBlockConstant) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
-
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
- ordering.AddElementToGroup(&y, 0);
- ordering.AddElementToGroup(&z, 0);
-
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
- problem.SetParameterBlockConstant(&x);
-
-
- Program program(problem.program());
- string error;
- EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
- &ordering,
- NULL,
- &error));
- EXPECT_EQ(program.NumParameterBlocks(), 1);
- EXPECT_EQ(program.NumResidualBlocks(), 1);
- EXPECT_EQ(ordering.NumElements(), 1);
-}
-
-TEST(SolverImpl, RemoveFixedBlocksNumEliminateBlocks) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
- problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
- problem.SetParameterBlockConstant(&x);
-
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
- ordering.AddElementToGroup(&y, 0);
- ordering.AddElementToGroup(&z, 1);
-
- Program program(problem.program());
- string error;
- EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
- &ordering,
- NULL,
- &error));
- EXPECT_EQ(program.NumParameterBlocks(), 2);
- EXPECT_EQ(program.NumResidualBlocks(), 2);
- EXPECT_EQ(ordering.NumElements(), 2);
- EXPECT_EQ(ordering.GroupId(&y), 0);
- EXPECT_EQ(ordering.GroupId(&z), 1);
-}
-
-TEST(SolverImpl, RemoveFixedBlocksFixedCost) {
- ProblemImpl problem;
- double x = 1.23;
- double y = 4.56;
- double z = 7.89;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
- problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x);
- problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
- problem.SetParameterBlockConstant(&x);
-
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
- ordering.AddElementToGroup(&y, 0);
- ordering.AddElementToGroup(&z, 1);
-
- double fixed_cost = 0.0;
- Program program(problem.program());
-
- double expected_fixed_cost;
- ResidualBlock *expected_removed_block = program.residual_blocks()[0];
- scoped_array<double> scratch(
- new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
- expected_removed_block->Evaluate(true,
- &expected_fixed_cost,
- NULL,
- NULL,
- scratch.get());
-
- string error;
- EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
- &ordering,
- &fixed_cost,
- &error));
- EXPECT_EQ(program.NumParameterBlocks(), 2);
- EXPECT_EQ(program.NumResidualBlocks(), 2);
- EXPECT_EQ(ordering.NumElements(), 2);
- EXPECT_EQ(ordering.GroupId(&y), 0);
- EXPECT_EQ(ordering.GroupId(&z), 1);
- EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
-}
-
-TEST(SolverImpl, ReorderResidualBlockNormalFunction) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
-
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);
-
- ParameterBlockOrdering* ordering = new ParameterBlockOrdering;
- ordering->AddElementToGroup(&x, 0);
- ordering->AddElementToGroup(&y, 0);
- ordering->AddElementToGroup(&z, 1);
-
- Solver::Options options;
- options.linear_solver_type = DENSE_SCHUR;
- options.linear_solver_ordering = ordering;
-
- const vector<ResidualBlock*>& residual_blocks =
- problem.program().residual_blocks();
-
- vector<ResidualBlock*> expected_residual_blocks;
-
- // This is a bit fragile, but it serves the purpose. We know the
- // bucketing algorithm that the reordering function uses, so we
- // expect the order for residual blocks for each e_block to be
- // filled in reverse.
- expected_residual_blocks.push_back(residual_blocks[4]);
- expected_residual_blocks.push_back(residual_blocks[1]);
- expected_residual_blocks.push_back(residual_blocks[0]);
- expected_residual_blocks.push_back(residual_blocks[5]);
- expected_residual_blocks.push_back(residual_blocks[2]);
- expected_residual_blocks.push_back(residual_blocks[3]);
-
- Program* program = problem.mutable_program();
- program->SetParameterOffsetsAndIndex();
-
- string error;
- EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks(
- 2,
- problem.mutable_program(),
- &error));
- EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size());
- for (int i = 0; i < expected_residual_blocks.size(); ++i) {
- EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]);
- }
-}
-
-TEST(SolverImpl, ReorderResidualBlockNormalFunctionWithFixedBlocks) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
-
- // Set one parameter block constant.
- problem.SetParameterBlockConstant(&z);
-
- // Mark residuals for x's row block with "x" for readability.
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); // 0 x
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); // 1 x
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 2
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 3
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 4 x
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 5
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 6 x
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); // 7
-
- ParameterBlockOrdering* ordering = new ParameterBlockOrdering;
- ordering->AddElementToGroup(&x, 0);
- ordering->AddElementToGroup(&z, 0);
- ordering->AddElementToGroup(&y, 1);
-
- Solver::Options options;
- options.linear_solver_type = DENSE_SCHUR;
- options.linear_solver_ordering = ordering;
-
- // Create the reduced program. This should remove the fixed block "z",
- // marking the index to -1 at the same time. x and y also get indices.
- string error;
- scoped_ptr<Program> reduced_program(
- SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error));
-
- const vector<ResidualBlock*>& residual_blocks =
- problem.program().residual_blocks();
-
- // This is a bit fragile, but it serves the purpose. We know the
- // bucketing algorithm that the reordering function uses, so we
- // expect the order for residual blocks for each e_block to be
- // filled in reverse.
-
- vector<ResidualBlock*> expected_residual_blocks;
-
- // Row block for residuals involving "x". These are marked "x" in the block
- // of code calling AddResidual() above.
- expected_residual_blocks.push_back(residual_blocks[6]);
- expected_residual_blocks.push_back(residual_blocks[4]);
- expected_residual_blocks.push_back(residual_blocks[1]);
- expected_residual_blocks.push_back(residual_blocks[0]);
-
- // Row block for residuals involving "y".
- expected_residual_blocks.push_back(residual_blocks[7]);
- expected_residual_blocks.push_back(residual_blocks[5]);
- expected_residual_blocks.push_back(residual_blocks[3]);
- expected_residual_blocks.push_back(residual_blocks[2]);
-
- EXPECT_EQ(reduced_program->residual_blocks().size(),
- expected_residual_blocks.size());
- for (int i = 0; i < expected_residual_blocks.size(); ++i) {
- EXPECT_EQ(reduced_program->residual_blocks()[i],
- expected_residual_blocks[i]);
- }
-}
-
-TEST(SolverImpl, AutomaticSchurReorderingRespectsConstantBlocks) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
-
- // Set one parameter block constant.
- problem.SetParameterBlockConstant(&z);
-
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
- problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z);
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);
- problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);
-
- ParameterBlockOrdering* ordering = new ParameterBlockOrdering;
- ordering->AddElementToGroup(&x, 0);
- ordering->AddElementToGroup(&z, 0);
- ordering->AddElementToGroup(&y, 0);
-
- Solver::Options options;
- options.linear_solver_type = DENSE_SCHUR;
- options.linear_solver_ordering = ordering;
-
- string error;
- scoped_ptr<Program> reduced_program(
- SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error));
-
- const vector<ResidualBlock*>& residual_blocks =
- reduced_program->residual_blocks();
- const vector<ParameterBlock*>& parameter_blocks =
- reduced_program->parameter_blocks();
-
- const vector<ResidualBlock*>& original_residual_blocks =
- problem.program().residual_blocks();
-
- EXPECT_EQ(residual_blocks.size(), 8);
- EXPECT_EQ(reduced_program->parameter_blocks().size(), 2);
-
- // Verify that right parmeter block and the residual blocks have
- // been removed.
- for (int i = 0; i < 8; ++i) {
- EXPECT_NE(residual_blocks[i], original_residual_blocks.back());
- }
- for (int i = 0; i < 2; ++i) {
- EXPECT_NE(parameter_blocks[i]->mutable_user_state(), &z);
- }
-}
-
-TEST(SolverImpl, ApplyUserOrderingOrderingTooSmall) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
-
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
- ordering.AddElementToGroup(&y, 1);
-
- Program program(problem.program());
- string error;
- EXPECT_FALSE(SolverImpl::ApplyUserOrdering(problem.parameter_map(),
- &ordering,
- &program,
- &error));
-}
-
-TEST(SolverImpl, ApplyUserOrderingNormal) {
- ProblemImpl problem;
- double x;
- double y;
- double z;
-
- problem.AddParameterBlock(&x, 1);
- problem.AddParameterBlock(&y, 1);
- problem.AddParameterBlock(&z, 1);
-
- ParameterBlockOrdering ordering;
- ordering.AddElementToGroup(&x, 0);
- ordering.AddElementToGroup(&y, 2);
- ordering.AddElementToGroup(&z, 1);
-
- Program* program = problem.mutable_program();
- string error;
-
- EXPECT_TRUE(SolverImpl::ApplyUserOrdering(problem.parameter_map(),
- &ordering,
- program,
- &error));
- const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
-
- EXPECT_EQ(parameter_blocks.size(), 3);
- EXPECT_EQ(parameter_blocks[0]->user_state(), &x);
- EXPECT_EQ(parameter_blocks[1]->user_state(), &z);
- EXPECT_EQ(parameter_blocks[2]->user_state(), &y);
-}
-
-#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
-TEST(SolverImpl, CreateLinearSolverNoSuiteSparse) {
- Solver::Options options;
- options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
- // CreateLinearSolver assumes a non-empty ordering.
- options.linear_solver_ordering = new ParameterBlockOrdering;
- string error;
- EXPECT_FALSE(SolverImpl::CreateLinearSolver(&options, &error));
-}
-#endif
-
-TEST(SolverImpl, CreateLinearSolverNegativeMaxNumIterations) {
- Solver::Options options;
- options.linear_solver_type = DENSE_QR;
- options.max_linear_solver_iterations = -1;
- // CreateLinearSolver assumes a non-empty ordering.
- options.linear_solver_ordering = new ParameterBlockOrdering;
- string error;
- EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
- static_cast<LinearSolver*>(NULL));
-}
-
-TEST(SolverImpl, CreateLinearSolverNegativeMinNumIterations) {
- Solver::Options options;
- options.linear_solver_type = DENSE_QR;
- options.min_linear_solver_iterations = -1;
- // CreateLinearSolver assumes a non-empty ordering.
- options.linear_solver_ordering = new ParameterBlockOrdering;
- string error;
- EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
- static_cast<LinearSolver*>(NULL));
-}
-
-TEST(SolverImpl, CreateLinearSolverMaxLessThanMinIterations) {
- Solver::Options options;
- options.linear_solver_type = DENSE_QR;
- options.min_linear_solver_iterations = 10;
- options.max_linear_solver_iterations = 5;
- options.linear_solver_ordering = new ParameterBlockOrdering;
- string error;
- EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
- static_cast<LinearSolver*>(NULL));
-}
-
-TEST(SolverImpl, CreateLinearSolverDenseSchurMultipleThreads) {
- Solver::Options options;
- options.linear_solver_type = DENSE_SCHUR;
- options.num_linear_solver_threads = 2;
- // The Schur type solvers can only be created with the Ordering
- // contains at least one elimination group.
- options.linear_solver_ordering = new ParameterBlockOrdering;
- double x;
- double y;
- options.linear_solver_ordering->AddElementToGroup(&x, 0);
- options.linear_solver_ordering->AddElementToGroup(&y, 0);
-
- string error;
- scoped_ptr<LinearSolver> solver(
- SolverImpl::CreateLinearSolver(&options, &error));
- EXPECT_TRUE(solver != NULL);
- EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR);
- EXPECT_EQ(options.num_linear_solver_threads, 2);
-}
-
-TEST(SolverImpl, CreateIterativeLinearSolverForDogleg) {
- Solver::Options options;
- options.trust_region_strategy_type = DOGLEG;
- // CreateLinearSolver assumes a non-empty ordering.
- options.linear_solver_ordering = new ParameterBlockOrdering;
- string error;
- options.linear_solver_type = ITERATIVE_SCHUR;
- EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
- static_cast<LinearSolver*>(NULL));
-
- options.linear_solver_type = CGNR;
- EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
- static_cast<LinearSolver*>(NULL));
-}
-
-TEST(SolverImpl, CreateLinearSolverNormalOperation) {
- Solver::Options options;
- scoped_ptr<LinearSolver> solver;
- options.linear_solver_type = DENSE_QR;
- // CreateLinearSolver assumes a non-empty ordering.
- options.linear_solver_ordering = new ParameterBlockOrdering;
- string error;
- solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
- EXPECT_EQ(options.linear_solver_type, DENSE_QR);
- EXPECT_TRUE(solver.get() != NULL);
-
- options.linear_solver_type = DENSE_NORMAL_CHOLESKY;
- solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
- EXPECT_EQ(options.linear_solver_type, DENSE_NORMAL_CHOLESKY);
- EXPECT_TRUE(solver.get() != NULL);
-
-#ifndef CERES_NO_SUITESPARSE
- options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
- options.sparse_linear_algebra_library_type = SUITE_SPARSE;
- solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
- EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
- EXPECT_TRUE(solver.get() != NULL);
-#endif
-
-#ifndef CERES_NO_CXSPARSE
- options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
- options.sparse_linear_algebra_library_type = CX_SPARSE;
- solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
- EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
- EXPECT_TRUE(solver.get() != NULL);
-#endif
-
- double x;
- double y;
- options.linear_solver_ordering->AddElementToGroup(&x, 0);
- options.linear_solver_ordering->AddElementToGroup(&y, 0);
-
- options.linear_solver_type = DENSE_SCHUR;
- solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
- EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR);
- EXPECT_TRUE(solver.get() != NULL);
-
- options.linear_solver_type = SPARSE_SCHUR;
- solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
-
-#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
- EXPECT_TRUE(SolverImpl::CreateLinearSolver(&options, &error) == NULL);
-#else
- EXPECT_TRUE(solver.get() != NULL);
- EXPECT_EQ(options.linear_solver_type, SPARSE_SCHUR);
-#endif
-
- options.linear_solver_type = ITERATIVE_SCHUR;
- solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
- EXPECT_EQ(options.linear_solver_type, ITERATIVE_SCHUR);
- EXPECT_TRUE(solver.get() != NULL);
-}
-
-struct QuadraticCostFunction {
- template <typename T> bool operator()(const T* const x,
- T* residual) const {
- residual[0] = T(5.0) - *x;
- return true;
- }
-};
-
-struct RememberingCallback : public IterationCallback {
- explicit RememberingCallback(double *x) : calls(0), x(x) {}
- virtual ~RememberingCallback() {}
- virtual CallbackReturnType operator()(const IterationSummary& summary) {
- x_values.push_back(*x);
- return SOLVER_CONTINUE;
- }
- int calls;
- double *x;
- vector<double> x_values;
-};
-
-TEST(SolverImpl, UpdateStateEveryIterationOption) {
- double x = 50.0;
- const double original_x = x;
-
- scoped_ptr<CostFunction> cost_function(
- new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>(
- new QuadraticCostFunction));
-
- Problem::Options problem_options;
- problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP;
- ProblemImpl problem(problem_options);
- problem.AddResidualBlock(cost_function.get(), NULL, &x);
-
- Solver::Options options;
- options.linear_solver_type = DENSE_QR;
-
- RememberingCallback callback(&x);
- options.callbacks.push_back(&callback);
-
- Solver::Summary summary;
-
- int num_iterations;
-
- // First try: no updating.
- SolverImpl::Solve(options, &problem, &summary);
- num_iterations = summary.num_successful_steps +
- summary.num_unsuccessful_steps;
- EXPECT_GT(num_iterations, 1);
- for (int i = 0; i < callback.x_values.size(); ++i) {
- EXPECT_EQ(50.0, callback.x_values[i]);
- }
-
- // Second try: with updating
- x = 50.0;
- options.update_state_every_iteration = true;
- callback.x_values.clear();
- SolverImpl::Solve(options, &problem, &summary);
- num_iterations = summary.num_successful_steps +
- summary.num_unsuccessful_steps;
- EXPECT_GT(num_iterations, 1);
- EXPECT_EQ(original_x, callback.x_values[0]);
- EXPECT_NE(original_x, callback.x_values[1]);
-}
-
// The parameters must be in separate blocks so that they can be individually
// set constant or not.
struct Quadratic4DCostFunction {
@@ -753,289 +99,8 @@
EXPECT_EQ(&y, problem.program().parameter_blocks()[1]->state());
EXPECT_EQ(&z, problem.program().parameter_blocks()[2]->state());
EXPECT_EQ(&w, problem.program().parameter_blocks()[3]->state());
-}
-TEST(SolverImpl, NoParameterBlocks) {
- ProblemImpl problem_impl;
- Solver::Options options;
- Solver::Summary summary;
- SolverImpl::Solve(options, &problem_impl, &summary);
- EXPECT_EQ(summary.termination_type, DID_NOT_RUN);
- EXPECT_EQ(summary.error, "Problem contains no parameter blocks.");
-}
-
-TEST(SolverImpl, NoResiduals) {
- ProblemImpl problem_impl;
- Solver::Options options;
- Solver::Summary summary;
- double x = 1;
- problem_impl.AddParameterBlock(&x, 1);
- SolverImpl::Solve(options, &problem_impl, &summary);
- EXPECT_EQ(summary.termination_type, DID_NOT_RUN);
- EXPECT_EQ(summary.error, "Problem contains no residual blocks.");
-}
-
-
-TEST(SolverImpl, ProblemIsConstant) {
- ProblemImpl problem_impl;
- Solver::Options options;
- Solver::Summary summary;
- double x = 1;
- problem_impl.AddResidualBlock(new UnaryIdentityCostFunction, NULL, &x);
- problem_impl.SetParameterBlockConstant(&x);
- SolverImpl::Solve(options, &problem_impl, &summary);
- EXPECT_EQ(summary.termination_type, FUNCTION_TOLERANCE);
- EXPECT_EQ(summary.initial_cost, 1.0 / 2.0);
- EXPECT_EQ(summary.final_cost, 1.0 / 2.0);
-}
-
-TEST(SolverImpl, AlternateLinearSolverForSchurTypeLinearSolver) {
- Solver::Options options;
-
- options.linear_solver_type = DENSE_QR;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, DENSE_QR);
-
- options.linear_solver_type = DENSE_NORMAL_CHOLESKY;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, DENSE_NORMAL_CHOLESKY);
-
- options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
-
- options.linear_solver_type = CGNR;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, CGNR);
-
- options.linear_solver_type = DENSE_SCHUR;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, DENSE_QR);
-
- options.linear_solver_type = SPARSE_SCHUR;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
-
- options.linear_solver_type = ITERATIVE_SCHUR;
- options.preconditioner_type = IDENTITY;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, CGNR);
- EXPECT_EQ(options.preconditioner_type, IDENTITY);
-
- options.linear_solver_type = ITERATIVE_SCHUR;
- options.preconditioner_type = JACOBI;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, CGNR);
- EXPECT_EQ(options.preconditioner_type, JACOBI);
-
- options.linear_solver_type = ITERATIVE_SCHUR;
- options.preconditioner_type = SCHUR_JACOBI;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, CGNR);
- EXPECT_EQ(options.preconditioner_type, JACOBI);
-
- options.linear_solver_type = ITERATIVE_SCHUR;
- options.preconditioner_type = CLUSTER_JACOBI;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, CGNR);
- EXPECT_EQ(options.preconditioner_type, JACOBI);
-
- options.linear_solver_type = ITERATIVE_SCHUR;
- options.preconditioner_type = CLUSTER_TRIDIAGONAL;
- SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
- EXPECT_EQ(options.linear_solver_type, CGNR);
- EXPECT_EQ(options.preconditioner_type, JACOBI);
-}
-
-TEST(SolverImpl, CreateJacobianBlockSparsityTranspose) {
- ProblemImpl problem;
- double x[2];
- double y[3];
- double z;
-
- problem.AddParameterBlock(x, 2);
- problem.AddParameterBlock(y, 3);
- problem.AddParameterBlock(&z, 1);
-
- problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 0, 0>(), NULL, x);
- problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2, 0>(), NULL, &z, x);
- problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3, 0>(), NULL, &z, y);
- problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3, 0>(), NULL, &z, y);
- problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1, 0>(), NULL, x, &z);
- problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3, 0>(), NULL, &z, y);
- problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1, 0>(), NULL, x, &z);
- problem.AddResidualBlock(new MockCostFunctionBase<1, 3, 0, 0>(), NULL, y);
-
- TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
- {
- int* rows = expected_block_sparse_jacobian.mutable_rows();
- int* cols = expected_block_sparse_jacobian.mutable_cols();
- double* values = expected_block_sparse_jacobian.mutable_values();
- rows[0] = 0;
- cols[0] = 0;
-
- rows[1] = 2;
- cols[1] = 1;
- rows[2] = 0;
- cols[2] = 1;
-
- rows[3] = 2;
- cols[3] = 2;
- rows[4] = 1;
- cols[4] = 2;
-
- rows[5] = 2;
- cols[5] = 3;
- rows[6] = 1;
- cols[6] = 3;
-
- rows[7] = 0;
- cols[7] = 4;
- rows[8] = 2;
- cols[8] = 4;
-
- rows[9] = 2;
- cols[9] = 5;
- rows[10] = 1;
- cols[10] = 5;
-
- rows[11] = 0;
- cols[11] = 6;
- rows[12] = 2;
- cols[12] = 6;
-
- rows[13] = 1;
- cols[13] = 7;
- fill(values, values + 14, 1.0);
- expected_block_sparse_jacobian.set_num_nonzeros(14);
- }
-
- Program* program = problem.mutable_program();
- program->SetParameterOffsetsAndIndex();
-
- scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
-
- Matrix expected_dense_jacobian;
- expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
-
- Matrix actual_dense_jacobian;
- actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
- EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
-}
-
-template <int kNumResiduals, int kNumParameterBlocks>
-class NumParameterBlocksCostFunction : public CostFunction {
- public:
- NumParameterBlocksCostFunction() {
- set_num_residuals(kNumResiduals);
- for (int i = 0; i < kNumParameterBlocks; ++i) {
- mutable_parameter_block_sizes()->push_back(1);
- }
- }
-
- virtual ~NumParameterBlocksCostFunction() {
- }
-
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- return true;
- }
-};
-
-TEST(SolverImpl, ReallocationInCreateJacobianBlockSparsityTranspose) {
- // CreateJacobianBlockSparsityTranspose starts with a conservative
- // estimate of the size of the sparsity pattern. This test ensures
- // that when those estimates are violated, the reallocation/resizing
- // logic works correctly.
-
- ProblemImpl problem;
- double x[20];
-
- vector<double*> parameter_blocks;
- for (int i = 0; i < 20; ++i) {
- problem.AddParameterBlock(x + i, 1);
- parameter_blocks.push_back(x + i);
- }
-
- problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
- NULL,
- parameter_blocks);
-
- TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
- {
- int* rows = expected_block_sparse_jacobian.mutable_rows();
- int* cols = expected_block_sparse_jacobian.mutable_cols();
- for (int i = 0; i < 20; ++i) {
- rows[i] = i;
- cols[i] = 0;
- }
-
- double* values = expected_block_sparse_jacobian.mutable_values();
- fill(values, values + 20, 1.0);
- expected_block_sparse_jacobian.set_num_nonzeros(20);
- }
-
- Program* program = problem.mutable_program();
- program->SetParameterOffsetsAndIndex();
-
- scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
-
- Matrix expected_dense_jacobian;
- expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
-
- Matrix actual_dense_jacobian;
- actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
- EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
-}
-
-TEST(CompactifyArray, ContiguousEntries) {
- vector<int> array;
- array.push_back(0);
- array.push_back(1);
- vector<int> expected = array;
- SolverImpl::CompactifyArray(&array);
- EXPECT_EQ(array, expected);
- array.clear();
-
- array.push_back(1);
- array.push_back(0);
- expected = array;
- SolverImpl::CompactifyArray(&array);
- EXPECT_EQ(array, expected);
-}
-
-TEST(CompactifyArray, NonContiguousEntries) {
- vector<int> array;
- array.push_back(0);
- array.push_back(2);
- vector<int> expected;
- expected.push_back(0);
- expected.push_back(1);
- SolverImpl::CompactifyArray(&array);
- EXPECT_EQ(array, expected);
-}
-
-TEST(CompactifyArray, NonContiguousRepeatingEntries) {
- vector<int> array;
- array.push_back(3);
- array.push_back(1);
- array.push_back(0);
- array.push_back(0);
- array.push_back(0);
- array.push_back(5);
- vector<int> expected;
- expected.push_back(2);
- expected.push_back(1);
- expected.push_back(0);
- expected.push_back(0);
- expected.push_back(0);
- expected.push_back(3);
-
- SolverImpl::CompactifyArray(&array);
- EXPECT_EQ(array, expected);
+ EXPECT_TRUE(problem.program().IsValid());
}
} // namespace internal
diff --git a/internal/ceres/solver_test.cc b/internal/ceres/solver_test.cc
new file mode 100644
index 0000000..2a136f7
--- /dev/null
+++ b/internal/ceres/solver_test.cc
@@ -0,0 +1,298 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/solver.h"
+
+#include <limits>
+#include <cmath>
+#include <vector>
+#include "gtest/gtest.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/autodiff_cost_function.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/problem.h"
+#include "ceres/problem_impl.h"
+
+namespace ceres {
+namespace internal {
+
+TEST(SolverOptions, DefaultTrustRegionOptionsAreValid) {
+ Solver::Options options;
+ options.minimizer_type = TRUST_REGION;
+ string error;
+ EXPECT_TRUE(options.IsValid(&error)) << error;
+}
+
+TEST(SolverOptions, DefaultLineSearchOptionsAreValid) {
+ Solver::Options options;
+ options.minimizer_type = LINE_SEARCH;
+ string error;
+ EXPECT_TRUE(options.IsValid(&error)) << error;
+}
+
+struct QuadraticCostFunctor {
+ template <typename T> bool operator()(const T* const x,
+ T* residual) const {
+ residual[0] = T(5.0) - *x;
+ return true;
+ }
+
+ static CostFunction* Create() {
+ return new AutoDiffCostFunction<QuadraticCostFunctor, 1, 1>(
+ new QuadraticCostFunctor);
+ }
+};
+
+struct RememberingCallback : public IterationCallback {
+ explicit RememberingCallback(double *x) : calls(0), x(x) {}
+ virtual ~RememberingCallback() {}
+ virtual CallbackReturnType operator()(const IterationSummary& summary) {
+ x_values.push_back(*x);
+ return SOLVER_CONTINUE;
+ }
+ int calls;
+ double *x;
+ vector<double> x_values;
+};
+
+TEST(Solver, UpdateStateEveryIterationOption) {
+ double x = 50.0;
+ const double original_x = x;
+
+ scoped_ptr<CostFunction> cost_function(QuadraticCostFunctor::Create());
+ Problem::Options problem_options;
+ problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP;
+ Problem problem(problem_options);
+ problem.AddResidualBlock(cost_function.get(), NULL, &x);
+
+ Solver::Options options;
+ options.linear_solver_type = DENSE_QR;
+
+ RememberingCallback callback(&x);
+ options.callbacks.push_back(&callback);
+
+ Solver::Summary summary;
+
+ int num_iterations;
+
+ // First try: no updating.
+ Solve(options, &problem, &summary);
+ num_iterations = summary.num_successful_steps +
+ summary.num_unsuccessful_steps;
+ EXPECT_GT(num_iterations, 1);
+ for (int i = 0; i < callback.x_values.size(); ++i) {
+ EXPECT_EQ(50.0, callback.x_values[i]);
+ }
+
+ // Second try: with updating
+ x = 50.0;
+ options.update_state_every_iteration = true;
+ callback.x_values.clear();
+ Solve(options, &problem, &summary);
+ num_iterations = summary.num_successful_steps +
+ summary.num_unsuccessful_steps;
+ EXPECT_GT(num_iterations, 1);
+ EXPECT_EQ(original_x, callback.x_values[0]);
+ EXPECT_NE(original_x, callback.x_values[1]);
+}
+
+// The parameters must be in separate blocks so that they can be individually
+// set constant or not.
+struct Quadratic4DCostFunction {
+ template <typename T> bool operator()(const T* const x,
+ const T* const y,
+ const T* const z,
+ const T* const w,
+ T* residual) const {
+ // A 4-dimension axis-aligned quadratic.
+ residual[0] = T(10.0) - *x +
+ T(20.0) - *y +
+ T(30.0) - *z +
+ T(40.0) - *w;
+ return true;
+ }
+
+ static CostFunction* Create() {
+ return new AutoDiffCostFunction<Quadratic4DCostFunction, 1, 1, 1, 1, 1>(
+ new Quadratic4DCostFunction);
+ }
+};
+
+// A cost function that simply returns its argument.
+class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ residuals[0] = parameters[0][0];
+ if (jacobians != NULL && jacobians[0] != NULL) {
+ jacobians[0][0] = 1.0;
+ }
+ return true;
+ }
+};
+
+TEST(Solver, TrustRegionProblemHasNoParameterBlocks) {
+ Problem problem;
+ Solver::Options options;
+ options.minimizer_type = TRUST_REGION;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_EQ(summary.termination_type, CONVERGENCE);
+ EXPECT_EQ(summary.message,
+ "Function tolerance reached. "
+ "No non-constant parameter blocks found.");
+}
+
+TEST(Solver, LineSearchProblemHasNoParameterBlocks) {
+ Problem problem;
+ Solver::Options options;
+ options.minimizer_type = LINE_SEARCH;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_EQ(summary.termination_type, CONVERGENCE);
+ EXPECT_EQ(summary.message,
+ "Function tolerance reached. "
+ "No non-constant parameter blocks found.");
+}
+
+TEST(Solver, TrustRegionProblemHasZeroResiduals) {
+ Problem problem;
+ double x = 1;
+ problem.AddParameterBlock(&x, 1);
+ Solver::Options options;
+ options.minimizer_type = TRUST_REGION;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_EQ(summary.termination_type, CONVERGENCE);
+ EXPECT_EQ(summary.message,
+ "Function tolerance reached. "
+ "No non-constant parameter blocks found.");
+}
+
+TEST(Solver, LineSearchProblemHasZeroResiduals) {
+ Problem problem;
+ double x = 1;
+ problem.AddParameterBlock(&x, 1);
+ Solver::Options options;
+ options.minimizer_type = LINE_SEARCH;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_EQ(summary.termination_type, CONVERGENCE);
+ EXPECT_EQ(summary.message,
+ "Function tolerance reached. "
+ "No non-constant parameter blocks found.");
+}
+
+TEST(Solver, TrustRegionProblemIsConstant) {
+ Problem problem;
+ double x = 1;
+ problem.AddResidualBlock(new UnaryIdentityCostFunction, NULL, &x);
+ problem.SetParameterBlockConstant(&x);
+ Solver::Options options;
+ options.minimizer_type = TRUST_REGION;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_EQ(summary.termination_type, CONVERGENCE);
+ EXPECT_EQ(summary.initial_cost, 1.0 / 2.0);
+ EXPECT_EQ(summary.final_cost, 1.0 / 2.0);
+}
+
+TEST(Solver, LineSearchProblemIsConstant) {
+ Problem problem;
+ double x = 1;
+ problem.AddResidualBlock(new UnaryIdentityCostFunction, NULL, &x);
+ problem.SetParameterBlockConstant(&x);
+ Solver::Options options;
+ options.minimizer_type = LINE_SEARCH;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_EQ(summary.termination_type, CONVERGENCE);
+ EXPECT_EQ(summary.initial_cost, 1.0 / 2.0);
+ EXPECT_EQ(summary.final_cost, 1.0 / 2.0);
+}
+
+#if defined(CERES_NO_SUITESPARSE)
+TEST(Solver, SparseNormalCholeskyNoSuiteSparse) {
+ Solver::Options options;
+ options.sparse_linear_algebra_library_type = SUITE_SPARSE;
+ options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
+ string message;
+ EXPECT_FALSE(options.IsValid(&message));
+}
+#endif
+
+#if defined(CERES_NO_CXSPARSE)
+TEST(Solver, SparseNormalCholeskyNoCXSparse) {
+ Solver::Options options;
+ options.sparse_linear_algebra_library_type = CX_SPARSE;
+ options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
+ string message;
+ EXPECT_FALSE(options.IsValid(&message));
+}
+#endif
+
+TEST(Solver, IterativeLinearSolverForDogleg) {
+ Solver::Options options;
+ options.trust_region_strategy_type = DOGLEG;
+ string message;
+ options.linear_solver_type = ITERATIVE_SCHUR;
+ EXPECT_FALSE(options.IsValid(&message));
+
+ options.linear_solver_type = CGNR;
+ EXPECT_FALSE(options.IsValid(&message));
+}
+
+TEST(Solver, LinearSolverTypeNormalOperation) {
+ Solver::Options options;
+ options.linear_solver_type = DENSE_QR;
+
+ string message;
+ EXPECT_TRUE(options.IsValid(&message));
+
+ options.linear_solver_type = DENSE_NORMAL_CHOLESKY;
+ EXPECT_TRUE(options.IsValid(&message));
+
+ options.linear_solver_type = DENSE_SCHUR;
+ EXPECT_TRUE(options.IsValid(&message));
+
+ options.linear_solver_type = SPARSE_SCHUR;
+#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
+ EXPECT_FALSE(options.IsValid(&message));
+#else
+ EXPECT_TRUE(options.IsValid(&message));
+#endif
+
+ options.linear_solver_type = ITERATIVE_SCHUR;
+ EXPECT_TRUE(options.IsValid(&message));
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/sparse_normal_cholesky_solver.cc b/internal/ceres/sparse_normal_cholesky_solver.cc
index f1a5237..0940815 100644
--- a/internal/ceres/sparse_normal_cholesky_solver.cc
+++ b/internal/ceres/sparse_normal_cholesky_solver.cc
@@ -28,7 +28,8 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
-#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
#include "ceres/sparse_normal_cholesky_solver.h"
@@ -45,6 +46,8 @@
#include "ceres/triplet_sparse_matrix.h"
#include "ceres/types.h"
#include "ceres/wall_time.h"
+#include "Eigen/SparseCore"
+
namespace ceres {
namespace internal {
@@ -53,23 +56,23 @@
const LinearSolver::Options& options)
: factor_(NULL),
cxsparse_factor_(NULL),
- options_(options) {
+ options_(options){
}
-SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {
-#ifndef CERES_NO_SUITESPARSE
+void SparseNormalCholeskySolver::FreeFactorization() {
if (factor_ != NULL) {
ss_.Free(factor_);
factor_ = NULL;
}
-#endif
-#ifndef CERES_NO_CXSPARSE
if (cxsparse_factor_ != NULL) {
cxsparse_.Free(cxsparse_factor_);
cxsparse_factor_ = NULL;
}
-#endif // CERES_NO_CXSPARSE
+}
+
+SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {
+ FreeFactorization();
}
LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl(
@@ -77,177 +80,303 @@
const double* b,
const LinearSolver::PerSolveOptions& per_solve_options,
double * x) {
+
+ const int num_cols = A->num_cols();
+ VectorRef(x, num_cols).setZero();
+ A->LeftMultiply(b, x);
+
+ if (per_solve_options.D != NULL) {
+ // Temporarily append a diagonal block to the A matrix, but undo
+ // it before returning the matrix to the user.
+ scoped_ptr<CompressedRowSparseMatrix> regularizer;
+ if (A->col_blocks().size() > 0) {
+ regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(
+ per_solve_options.D, A->col_blocks()));
+ } else {
+ regularizer.reset(new CompressedRowSparseMatrix(
+ per_solve_options.D, num_cols));
+ }
+ A->AppendRows(*regularizer);
+ }
+
+ LinearSolver::Summary summary;
switch (options_.sparse_linear_algebra_library_type) {
case SUITE_SPARSE:
- return SolveImplUsingSuiteSparse(A, b, per_solve_options, x);
+ summary = SolveImplUsingSuiteSparse(A, per_solve_options, x);
+ break;
case CX_SPARSE:
- return SolveImplUsingCXSparse(A, b, per_solve_options, x);
+ summary = SolveImplUsingCXSparse(A, per_solve_options, x);
+ break;
+ case EIGEN_SPARSE:
+ summary = SolveImplUsingEigen(A, per_solve_options, x);
+ break;
default:
LOG(FATAL) << "Unknown sparse linear algebra library : "
<< options_.sparse_linear_algebra_library_type;
}
- LOG(FATAL) << "Unknown sparse linear algebra library : "
- << options_.sparse_linear_algebra_library_type;
- return LinearSolver::Summary();
+ if (per_solve_options.D != NULL) {
+ A->DeleteRows(num_cols);
+ }
+
+ return summary;
}
-#ifndef CERES_NO_CXSPARSE
+LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen(
+ CompressedRowSparseMatrix* A,
+ const LinearSolver::PerSolveOptions& per_solve_options,
+ double * rhs_and_solution) {
+#ifndef CERES_USE_EIGEN_SPARSE
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE "
+ "because Ceres was not built with support for "
+ "Eigen's SimplicialLDLT decomposition. "
+ "This requires enabling building with -DEIGENSPARSE=ON.";
+ return summary;
+
+#else
+
+ EventLogger event_logger("SparseNormalCholeskySolver::Eigen::Solve");
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 1;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
+
+ // Compute the normal equations. J'J delta = J'f and solve them
+ // using a sparse Cholesky factorization. Notice that when compared
+ // to SuiteSparse we have to explicitly compute the normal equations
+ // before they can be factorized. CHOLMOD/SuiteSparse on the other
+ // hand can just work off of Jt to compute the Cholesky
+ // factorization of the normal equations.
+ //
+ // TODO(sameeragarwal): See note about how this maybe a bad idea for
+ // dynamic sparsity.
+ if (outer_product_.get() == NULL) {
+ outer_product_.reset(
+ CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
+ *A, &pattern_));
+ }
+
+ CompressedRowSparseMatrix::ComputeOuterProduct(
+ *A, pattern_, outer_product_.get());
+
+ // Map to an upper triangular column major matrix.
+ //
+ // outer_product_ is a compressed row sparse matrix and in lower
+ // triangular form, when mapped to a compressed column sparse
+ // matrix, it becomes an upper triangular matrix.
+ Eigen::MappedSparseMatrix<double, Eigen::ColMajor> AtA(
+ outer_product_->num_rows(),
+ outer_product_->num_rows(),
+ outer_product_->num_nonzeros(),
+ outer_product_->mutable_rows(),
+ outer_product_->mutable_cols(),
+ outer_product_->mutable_values());
+
+ const Vector b = VectorRef(rhs_and_solution, outer_product_->num_rows());
+ if (simplicial_ldlt_.get() == NULL || options_.dynamic_sparsity) {
+ simplicial_ldlt_.reset(new SimplicialLDLT);
+ // This is a crappy way to be doing this. But right now Eigen does
+ // not expose a way to do symbolic analysis with a given
+ // permutation pattern, so we cannot use a block analysis of the
+ // Jacobian.
+ simplicial_ldlt_->analyzePattern(AtA);
+ if (simplicial_ldlt_->info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "Eigen failure. Unable to find symbolic factorization.";
+ return summary;
+ }
+ }
+ event_logger.AddEvent("Analysis");
+
+ simplicial_ldlt_->factorize(AtA);
+ if(simplicial_ldlt_->info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message =
+ "Eigen failure. Unable to find numeric factorization.";
+ return summary;
+ }
+
+ VectorRef(rhs_and_solution, outer_product_->num_rows()) =
+ simplicial_ldlt_->solve(b);
+ if(simplicial_ldlt_->info() != Eigen::Success) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message =
+ "Eigen failure. Unable to do triangular solve.";
+ return summary;
+ }
+
+ event_logger.AddEvent("Solve");
+ return summary;
+#endif // EIGEN_USE_EIGEN_SPARSE
+}
+
+
+
LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
CompressedRowSparseMatrix* A,
- const double* b,
const LinearSolver::PerSolveOptions& per_solve_options,
- double * x) {
+ double * rhs_and_solution) {
+#ifdef CERES_NO_CXSPARSE
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE "
+ "because Ceres was not built with support for CXSparse. "
+ "This requires enabling building with -DCXSPARSE=ON.";
+
+ return summary;
+
+#else
+
EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve");
LinearSolver::Summary summary;
summary.num_iterations = 1;
- const int num_cols = A->num_cols();
- Vector Atb = Vector::Zero(num_cols);
- A->LeftMultiply(b, Atb.data());
-
- if (per_solve_options.D != NULL) {
- // Temporarily append a diagonal block to the A matrix, but undo
- // it before returning the matrix to the user.
- CompressedRowSparseMatrix D(per_solve_options.D, num_cols);
- A->AppendRows(D);
- }
-
- VectorRef(x, num_cols).setZero();
-
- // Wrap the augmented Jacobian in a compressed sparse column matrix.
- cs_di At = cxsparse_.CreateSparseMatrixTransposeView(A);
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.message = "Success.";
// Compute the normal equations. J'J delta = J'f and solve them
// using a sparse Cholesky factorization. Notice that when compared
- // to SuiteSparse we have to explicitly compute the transpose of Jt,
- // and then the normal equations before they can be
- // factorized. CHOLMOD/SuiteSparse on the other hand can just work
- // off of Jt to compute the Cholesky factorization of the normal
- // equations.
- cs_di* A2 = cxsparse_.TransposeMatrix(&At);
- cs_di* AtA = cxsparse_.MatrixMatrixMultiply(&At, A2);
-
- cxsparse_.Free(A2);
- if (per_solve_options.D != NULL) {
- A->DeleteRows(num_cols);
+ // to SuiteSparse we have to explicitly compute the normal equations
+ // before they can be factorized. CHOLMOD/SuiteSparse on the other
+ // hand can just work off of Jt to compute the Cholesky
+ // factorization of the normal equations.
+ //
+ // TODO(sameeragarwal): If dynamic sparsity is enabled, then this is
+ // not a good idea performance wise, since the jacobian has far too
+ // many entries and the program will go crazy with memory.
+ if (outer_product_.get() == NULL) {
+ outer_product_.reset(
+ CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
+ *A, &pattern_));
}
+
+ CompressedRowSparseMatrix::ComputeOuterProduct(
+ *A, pattern_, outer_product_.get());
+ cs_di AtA_view =
+ cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get());
+ cs_di* AtA = &AtA_view;
+
event_logger.AddEvent("Setup");
// Compute symbolic factorization if not available.
+ if (options_.dynamic_sparsity) {
+ FreeFactorization();
+ }
if (cxsparse_factor_ == NULL) {
if (options_.use_postordering) {
- cxsparse_factor_ =
- CHECK_NOTNULL(cxsparse_.BlockAnalyzeCholesky(AtA,
- A->col_blocks(),
- A->col_blocks()));
+ cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(AtA,
+ A->col_blocks(),
+ A->col_blocks());
} else {
- cxsparse_factor_ =
- CHECK_NOTNULL(cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA));
+ if (options_.dynamic_sparsity) {
+ cxsparse_factor_ = cxsparse_.AnalyzeCholesky(AtA);
+ } else {
+ cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA);
+ }
}
}
event_logger.AddEvent("Analysis");
- // Solve the linear system.
- if (cxsparse_.SolveCholesky(AtA, cxsparse_factor_, Atb.data())) {
- VectorRef(x, Atb.rows()) = Atb;
- summary.termination_type = TOLERANCE;
+ if (cxsparse_factor_ == NULL) {
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "CXSparse failure. Unable to find symbolic factorization.";
+ } else if (!cxsparse_.SolveCholesky(AtA, cxsparse_factor_, rhs_and_solution)) {
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
+ summary.message = "CXSparse::SolveCholesky failed.";
}
event_logger.AddEvent("Solve");
- cxsparse_.Free(AtA);
- event_logger.AddEvent("Teardown");
return summary;
-}
-#else
-LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
- CompressedRowSparseMatrix* A,
- const double* b,
- const LinearSolver::PerSolveOptions& per_solve_options,
- double * x) {
- LOG(FATAL) << "No CXSparse support in Ceres.";
-
- // Unreachable but MSVC does not know this.
- return LinearSolver::Summary();
-}
#endif
+}
-#ifndef CERES_NO_SUITESPARSE
LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
CompressedRowSparseMatrix* A,
- const double* b,
const LinearSolver::PerSolveOptions& per_solve_options,
- double * x) {
+ double * rhs_and_solution) {
+#ifdef CERES_NO_SUITESPARSE
+
+ LinearSolver::Summary summary;
+ summary.num_iterations = 0;
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ summary.message =
+ "SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE "
+ "because Ceres was not built with support for SuiteSparse. "
+ "This requires enabling building with -DSUITESPARSE=ON.";
+ return summary;
+
+#else
+
EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve");
+ LinearSolver::Summary summary;
+ summary.termination_type = LINEAR_SOLVER_SUCCESS;
+ summary.num_iterations = 1;
+ summary.message = "Success.";
const int num_cols = A->num_cols();
- LinearSolver::Summary summary;
- Vector Atb = Vector::Zero(num_cols);
- A->LeftMultiply(b, Atb.data());
-
- if (per_solve_options.D != NULL) {
- // Temporarily append a diagonal block to the A matrix, but undo it before
- // returning the matrix to the user.
- CompressedRowSparseMatrix D(per_solve_options.D, num_cols);
- A->AppendRows(D);
- }
-
- VectorRef(x, num_cols).setZero();
-
cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A);
- cholmod_dense* rhs = ss_.CreateDenseVector(Atb.data(), num_cols, num_cols);
event_logger.AddEvent("Setup");
+ if (options_.dynamic_sparsity) {
+ FreeFactorization();
+ }
if (factor_ == NULL) {
if (options_.use_postordering) {
- factor_ =
- CHECK_NOTNULL(ss_.BlockAnalyzeCholesky(&lhs,
- A->col_blocks(),
- A->row_blocks()));
+ factor_ = ss_.BlockAnalyzeCholesky(&lhs,
+ A->col_blocks(),
+ A->row_blocks(),
+ &summary.message);
} else {
- factor_ =
- CHECK_NOTNULL(ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs));
+ if (options_.dynamic_sparsity) {
+ factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message);
+ } else {
+ factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, &summary.message);
+ }
}
}
-
event_logger.AddEvent("Analysis");
- cholmod_dense* sol = ss_.SolveCholesky(&lhs, factor_, rhs);
+ if (factor_ == NULL) {
+ summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
+ // No need to set message as it has already been set by the
+ // symbolic analysis routines above.
+ return summary;
+ }
+
+ summary.termination_type = ss_.Cholesky(&lhs, factor_, &summary.message);
+ if (summary.termination_type != LINEAR_SOLVER_SUCCESS) {
+ return summary;
+ }
+
+ cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution, num_cols, num_cols);
+ cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message);
event_logger.AddEvent("Solve");
ss_.Free(rhs);
- rhs = NULL;
-
- if (per_solve_options.D != NULL) {
- A->DeleteRows(num_cols);
- }
-
- summary.num_iterations = 1;
- if (sol != NULL) {
- memcpy(x, sol->x, num_cols * sizeof(*x));
-
- ss_.Free(sol);
- sol = NULL;
- summary.termination_type = TOLERANCE;
+ if (solution != NULL) {
+ memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution));
+ ss_.Free(solution);
+ } else {
+ // No need to set message as it has already been set by the
+ // numeric factorization routine above.
+ summary.termination_type = LINEAR_SOLVER_FAILURE;
}
event_logger.AddEvent("Teardown");
return summary;
-}
-#else
-LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
- CompressedRowSparseMatrix* A,
- const double* b,
- const LinearSolver::PerSolveOptions& per_solve_options,
- double * x) {
- LOG(FATAL) << "No SuiteSparse support in Ceres.";
-
- // Unreachable but MSVC does not know this.
- return LinearSolver::Summary();
-}
#endif
+}
} // namespace internal
} // namespace ceres
-
-#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE)
diff --git a/internal/ceres/sparse_normal_cholesky_solver.h b/internal/ceres/sparse_normal_cholesky_solver.h
index 61111b4..6572835 100644
--- a/internal/ceres/sparse_normal_cholesky_solver.h
+++ b/internal/ceres/sparse_normal_cholesky_solver.h
@@ -34,12 +34,17 @@
#ifndef CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_
#define CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_
-#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
-#include "ceres/cxsparse.h"
#include "ceres/internal/macros.h"
#include "ceres/linear_solver.h"
#include "ceres/suitesparse.h"
+#include "ceres/cxsparse.h"
+
+#ifdef CERES_USE_EIGEN_SPARSE
+#include "Eigen/SparseCholesky"
+#endif
namespace ceres {
namespace internal {
@@ -62,16 +67,22 @@
LinearSolver::Summary SolveImplUsingSuiteSparse(
CompressedRowSparseMatrix* A,
- const double* b,
const LinearSolver::PerSolveOptions& options,
- double* x);
+ double* rhs_and_solution);
// Crashes if CSparse is not installed.
LinearSolver::Summary SolveImplUsingCXSparse(
CompressedRowSparseMatrix* A,
- const double* b,
const LinearSolver::PerSolveOptions& options,
- double* x);
+ double* rhs_and_solution);
+
+ // Crashes if CERES_USE_LGPGL_CODE is not defined.
+ LinearSolver::Summary SolveImplUsingEigen(
+ CompressedRowSparseMatrix* A,
+ const LinearSolver::PerSolveOptions& options,
+ double* rhs_and_solution);
+
+ void FreeFactorization();
SuiteSparse ss_;
// Cached factorization
@@ -81,6 +92,14 @@
// Cached factorization
cs_dis* cxsparse_factor_;
+#ifdef CERES_USE_EIGEN_SPARSE
+ typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>,
+ Eigen::Upper> SimplicialLDLT;
+ scoped_ptr<SimplicialLDLT> simplicial_ldlt_;
+#endif
+
+ scoped_ptr<CompressedRowSparseMatrix> outer_product_;
+ vector<int> pattern_;
const LinearSolver::Options options_;
CERES_DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver);
};
@@ -88,5 +107,4 @@
} // namespace internal
} // namespace ceres
-#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE)
#endif // CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_
diff --git a/internal/ceres/stringprintf.cc b/internal/ceres/stringprintf.cc
index ce20467..0f85f05 100644
--- a/internal/ceres/stringprintf.cc
+++ b/internal/ceres/stringprintf.cc
@@ -43,7 +43,9 @@
#ifdef _MSC_VER
enum { IS_COMPILER_MSVC = 1 };
-#define va_copy(d,s) ((d) = (s))
+#if _MSC_VER < 1800
+#define va_copy(d, s) ((d) = (s))
+#endif
#else
enum { IS_COMPILER_MSVC = 0 };
#endif
diff --git a/internal/ceres/suitesparse.cc b/internal/ceres/suitesparse.cc
index 9de32fd..1df7566 100644
--- a/internal/ceres/suitesparse.cc
+++ b/internal/ceres/suitesparse.cc
@@ -28,6 +28,9 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include "ceres/suitesparse.h"
@@ -35,6 +38,7 @@
#include "cholmod.h"
#include "ceres/compressed_col_sparse_matrix_utils.h"
#include "ceres/compressed_row_sparse_matrix.h"
+#include "ceres/linear_solver.h"
#include "ceres/triplet_sparse_matrix.h"
namespace ceres {
@@ -120,7 +124,8 @@
return v;
}
-cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A) {
+cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A,
+ string* message) {
// Cholmod can try multiple re-ordering strategies to find a fill
// reducing ordering. Here we just tell it use AMD with automatic
// matrix dependence choice of supernodal versus simplicial
@@ -130,31 +135,35 @@
cc_.supernodal = CHOLMOD_AUTO;
cholmod_factor* factor = cholmod_analyze(A, &cc_);
- CHECK_EQ(cc_.status, CHOLMOD_OK)
- << "Cholmod symbolic analysis failed " << cc_.status;
- CHECK_NOTNULL(factor);
-
if (VLOG_IS_ON(2)) {
cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
}
- return factor;
+ if (cc_.status != CHOLMOD_OK) {
+ *message = StringPrintf("cholmod_analyze failed. error code: %d",
+ cc_.status);
+ return NULL;
+ }
+
+ return CHECK_NOTNULL(factor);
}
cholmod_factor* SuiteSparse::BlockAnalyzeCholesky(
cholmod_sparse* A,
const vector<int>& row_blocks,
- const vector<int>& col_blocks) {
+ const vector<int>& col_blocks,
+ string* message) {
vector<int> ordering;
if (!BlockAMDOrdering(A, row_blocks, col_blocks, &ordering)) {
return NULL;
}
- return AnalyzeCholeskyWithUserOrdering(A, ordering);
+ return AnalyzeCholeskyWithUserOrdering(A, ordering, message);
}
cholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering(
cholmod_sparse* A,
- const vector<int>& ordering) {
+ const vector<int>& ordering,
+ string* message) {
CHECK_EQ(ordering.size(), A->nrow);
cc_.nmethods = 1;
@@ -162,33 +171,36 @@
cholmod_factor* factor =
cholmod_analyze_p(A, const_cast<int*>(&ordering[0]), NULL, 0, &cc_);
- CHECK_EQ(cc_.status, CHOLMOD_OK)
- << "Cholmod symbolic analysis failed " << cc_.status;
- CHECK_NOTNULL(factor);
-
if (VLOG_IS_ON(2)) {
cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
}
+ if (cc_.status != CHOLMOD_OK) {
+ *message = StringPrintf("cholmod_analyze failed. error code: %d",
+ cc_.status);
+ return NULL;
+ }
- return factor;
+ return CHECK_NOTNULL(factor);
}
cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering(
- cholmod_sparse* A) {
+ cholmod_sparse* A,
+ string* message) {
cc_.nmethods = 1;
cc_.method[0].ordering = CHOLMOD_NATURAL;
cc_.postorder = 0;
cholmod_factor* factor = cholmod_analyze(A, &cc_);
- CHECK_EQ(cc_.status, CHOLMOD_OK)
- << "Cholmod symbolic analysis failed " << cc_.status;
- CHECK_NOTNULL(factor);
-
if (VLOG_IS_ON(2)) {
cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
}
+ if (cc_.status != CHOLMOD_OK) {
+ *message = StringPrintf("cholmod_analyze failed. error code: %d",
+ cc_.status);
+ return NULL;
+ }
- return factor;
+ return CHECK_NOTNULL(factor);
}
bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A,
@@ -233,7 +245,9 @@
return true;
}
-bool SuiteSparse::Cholesky(cholmod_sparse* A, cholmod_factor* L) {
+LinearSolverTerminationType SuiteSparse::Cholesky(cholmod_sparse* A,
+ cholmod_factor* L,
+ string* message) {
CHECK_NOTNULL(A);
CHECK_NOTNULL(L);
@@ -245,7 +259,7 @@
cc_.print = 0;
cc_.quick_return_if_not_posdef = 1;
- int status = cholmod_factorize(A, L, &cc_);
+ int cholmod_status = cholmod_factorize(A, L, &cc_);
cc_.print = old_print_level;
// TODO(sameeragarwal): This switch statement is not consistent. It
@@ -257,84 +271,73 @@
// (e.g. out of memory).
switch (cc_.status) {
case CHOLMOD_NOT_INSTALLED:
- LOG(WARNING) << "CHOLMOD failure: Method not installed.";
- return false;
+ *message = "CHOLMOD failure: Method not installed.";
+ return LINEAR_SOLVER_FATAL_ERROR;
case CHOLMOD_OUT_OF_MEMORY:
- LOG(WARNING) << "CHOLMOD failure: Out of memory.";
- return false;
+ *message = "CHOLMOD failure: Out of memory.";
+ return LINEAR_SOLVER_FATAL_ERROR;
case CHOLMOD_TOO_LARGE:
- LOG(WARNING) << "CHOLMOD failure: Integer overflow occured.";
- return false;
+ *message = "CHOLMOD failure: Integer overflow occured.";
+ return LINEAR_SOLVER_FATAL_ERROR;
case CHOLMOD_INVALID:
- LOG(WARNING) << "CHOLMOD failure: Invalid input.";
- return false;
+ *message = "CHOLMOD failure: Invalid input.";
+ return LINEAR_SOLVER_FATAL_ERROR;
case CHOLMOD_NOT_POSDEF:
- // TODO(sameeragarwal): These two warnings require more
- // sophisticated handling going forward. For now we will be
- // strict and treat them as failures.
- LOG(WARNING) << "CHOLMOD warning: Matrix not positive definite.";
- return false;
+ *message = "CHOLMOD warning: Matrix not positive definite.";
+ return LINEAR_SOLVER_FAILURE;
case CHOLMOD_DSMALL:
- LOG(WARNING) << "CHOLMOD warning: D for LDL' or diag(L) or "
- << "LL' has tiny absolute value.";
- return false;
+ *message = "CHOLMOD warning: D for LDL' or diag(L) or "
+ "LL' has tiny absolute value.";
+ return LINEAR_SOLVER_FAILURE;
case CHOLMOD_OK:
- if (status != 0) {
- return true;
+ if (cholmod_status != 0) {
+ return LINEAR_SOLVER_SUCCESS;
}
- LOG(WARNING) << "CHOLMOD failure: cholmod_factorize returned zero "
- << "but cholmod_common::status is CHOLMOD_OK."
- << "Please report this to ceres-solver@googlegroups.com.";
- return false;
+
+ *message = "CHOLMOD failure: cholmod_factorize returned false "
+ "but cholmod_common::status is CHOLMOD_OK."
+ "Please report this to ceres-solver@googlegroups.com.";
+ return LINEAR_SOLVER_FATAL_ERROR;
default:
- LOG(WARNING) << "Unknown cholmod return code. "
- << "Please report this to ceres-solver@googlegroups.com.";
- return false;
+ *message =
+ StringPrintf("Unknown cholmod return code: %d. "
+ "Please report this to ceres-solver@googlegroups.com.",
+ cc_.status);
+ return LINEAR_SOLVER_FATAL_ERROR;
}
- return false;
+
+ return LINEAR_SOLVER_FATAL_ERROR;
}
cholmod_dense* SuiteSparse::Solve(cholmod_factor* L,
- cholmod_dense* b) {
+ cholmod_dense* b,
+ string* message) {
if (cc_.status != CHOLMOD_OK) {
- LOG(WARNING) << "CHOLMOD status NOT OK";
+ *message = "cholmod_solve failed. CHOLMOD status is not CHOLMOD_OK";
return NULL;
}
return cholmod_solve(CHOLMOD_A, L, b, &cc_);
}
-cholmod_dense* SuiteSparse::SolveCholesky(cholmod_sparse* A,
- cholmod_factor* L,
- cholmod_dense* b) {
- CHECK_NOTNULL(A);
- CHECK_NOTNULL(L);
- CHECK_NOTNULL(b);
-
- if (Cholesky(A, L)) {
- return Solve(L, b);
- }
-
- return NULL;
-}
-
-void SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix,
+bool SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix,
int* ordering) {
- cholmod_amd(matrix, NULL, 0, ordering, &cc_);
+ return cholmod_amd(matrix, NULL, 0, ordering, &cc_);
}
-void SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering(
+bool SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering(
cholmod_sparse* matrix,
int* constraints,
int* ordering) {
#ifndef CERES_NO_CAMD
- cholmod_camd(matrix, NULL, 0, constraints, ordering, &cc_);
+ return cholmod_camd(matrix, NULL, 0, constraints, ordering, &cc_);
#else
LOG(FATAL) << "Congratulations you have found a bug in Ceres."
<< "Ceres Solver was compiled with SuiteSparse "
<< "version 4.1.0 or less. Calling this function "
<< "in that case is a bug. Please contact the"
<< "the Ceres Solver developers.";
+ return false;
#endif
}
diff --git a/internal/ceres/suitesparse.h b/internal/ceres/suitesparse.h
index 16f298e..baab899 100644
--- a/internal/ceres/suitesparse.h
+++ b/internal/ceres/suitesparse.h
@@ -33,6 +33,8 @@
#ifndef CERES_INTERNAL_SUITESPARSE_H_
#define CERES_INTERNAL_SUITESPARSE_H_
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
#ifndef CERES_NO_SUITESPARSE
@@ -41,6 +43,7 @@
#include <vector>
#include "ceres/internal/port.h"
+#include "ceres/linear_solver.h"
#include "cholmod.h"
#include "glog/logging.h"
#include "SuiteSparseQR.hpp"
@@ -138,12 +141,15 @@
// A is not modified, only the pattern of non-zeros of A is used,
// the actual numerical values in A are of no consequence.
//
+ // message contains an explanation of the failures if any.
+ //
// Caller owns the result.
- cholmod_factor* AnalyzeCholesky(cholmod_sparse* A);
+ cholmod_factor* AnalyzeCholesky(cholmod_sparse* A, string* message);
cholmod_factor* BlockAnalyzeCholesky(cholmod_sparse* A,
const vector<int>& row_blocks,
- const vector<int>& col_blocks);
+ const vector<int>& col_blocks,
+ string* message);
// If A is symmetric, then compute the symbolic Cholesky
// factorization of A(ordering, ordering). If A is unsymmetric, then
@@ -153,33 +159,38 @@
// A is not modified, only the pattern of non-zeros of A is used,
// the actual numerical values in A are of no consequence.
//
+ // message contains an explanation of the failures if any.
+ //
// Caller owns the result.
cholmod_factor* AnalyzeCholeskyWithUserOrdering(cholmod_sparse* A,
- const vector<int>& ordering);
+ const vector<int>& ordering,
+ string* message);
// Perform a symbolic factorization of A without re-ordering A. No
// postordering of the elimination tree is performed. This ensures
// that the symbolic factor does not introduce an extra permutation
// on the matrix. See the documentation for CHOLMOD for more details.
- cholmod_factor* AnalyzeCholeskyWithNaturalOrdering(cholmod_sparse* A);
+ //
+ // message contains an explanation of the failures if any.
+ cholmod_factor* AnalyzeCholeskyWithNaturalOrdering(cholmod_sparse* A,
+ string* message);
// Use the symbolic factorization in L, to find the numerical
// factorization for the matrix A or AA^T. Return true if
// successful, false otherwise. L contains the numeric factorization
// on return.
- bool Cholesky(cholmod_sparse* A, cholmod_factor* L);
+ //
+ // message contains an explanation of the failures if any.
+ LinearSolverTerminationType Cholesky(cholmod_sparse* A,
+ cholmod_factor* L,
+ string* message);
// Given a Cholesky factorization of a matrix A = LL^T, solve the
// linear system Ax = b, and return the result. If the Solve fails
// NULL is returned. Caller owns the result.
- cholmod_dense* Solve(cholmod_factor* L, cholmod_dense* b);
-
- // Combine the calls to Cholesky and Solve into a single call. If
- // the cholesky factorization or the solve fails, return
- // NULL. Caller owns the result.
- cholmod_dense* SolveCholesky(cholmod_sparse* A,
- cholmod_factor* L,
- cholmod_dense* b);
+ //
+ // message contains an explanation of the failures if any.
+ cholmod_dense* Solve(cholmod_factor* L, cholmod_dense* b, string* message);
// By virtue of the modeling layer in Ceres being block oriented,
// all the matrices used by Ceres are also block oriented. When
@@ -211,7 +222,7 @@
// Find a fill reducing approximate minimum degree
// ordering. ordering is expected to be large enough to hold the
// ordering.
- void ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, int* ordering);
+ bool ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, int* ordering);
// Before SuiteSparse version 4.2.0, cholmod_camd was only enabled
@@ -241,7 +252,7 @@
//
// If CERES_NO_CAMD is defined then calling this function will
// result in a crash.
- void ConstrainedApproximateMinimumDegreeOrdering(cholmod_sparse* matrix,
+ bool ConstrainedApproximateMinimumDegreeOrdering(cholmod_sparse* matrix,
int* constraints,
int* ordering);
@@ -272,9 +283,24 @@
#else // CERES_NO_SUITESPARSE
-class SuiteSparse {};
typedef void cholmod_factor;
+class SuiteSparse {
+ public:
+ // Defining this static function even when SuiteSparse is not
+ // available, allows client code to check for the presence of CAMD
+ // without checking for the absence of the CERES_NO_CAMD symbol.
+ //
+ // This is safer because the symbol maybe missing due to a user
+ // accidently not including suitesparse.h in their code when
+ // checking for the symbol.
+ static bool IsConstrainedApproximateMinimumDegreeOrderingAvailable() {
+ return false;
+ }
+
+ void Free(void*) {};
+};
+
#endif // CERES_NO_SUITESPARSE
#endif // CERES_INTERNAL_SUITESPARSE_H_
diff --git a/internal/ceres/summary_utils.cc b/internal/ceres/summary_utils.cc
new file mode 100644
index 0000000..243030c
--- /dev/null
+++ b/internal/ceres/summary_utils.cc
@@ -0,0 +1,66 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include <algorithm>
+#include "ceres/summary_utils.h"
+#include "ceres/program.h"
+#include "ceres/solver.h"
+
+namespace ceres {
+namespace internal {
+
+void SetSummaryFinalCost(Solver::Summary* summary) {
+ summary->final_cost = summary->initial_cost;
+ // We need the loop here, instead of just looking at the last
+ // iteration because the minimizer maybe making non-monotonic steps.
+ for (int i = 0; i < summary->iterations.size(); ++i) {
+ const IterationSummary& iteration_summary = summary->iterations[i];
+ summary->final_cost = min(iteration_summary.cost, summary->final_cost);
+ }
+}
+
+void SummarizeGivenProgram(const Program& program, Solver::Summary* summary) {
+ summary->num_parameter_blocks = program.NumParameterBlocks();
+ summary->num_parameters = program.NumParameters();
+ summary->num_effective_parameters = program.NumEffectiveParameters();
+ summary->num_residual_blocks = program.NumResidualBlocks();
+ summary->num_residuals = program.NumResiduals();
+}
+
+void SummarizeReducedProgram(const Program& program, Solver::Summary* summary) {
+ summary->num_parameter_blocks_reduced = program.NumParameterBlocks();
+ summary->num_parameters_reduced = program.NumParameters();
+ summary->num_effective_parameters_reduced = program.NumEffectiveParameters();
+ summary->num_residual_blocks_reduced = program.NumResidualBlocks();
+ summary->num_residuals_reduced = program.NumResiduals();
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/summary_utils.h b/internal/ceres/summary_utils.h
new file mode 100644
index 0000000..9b07987
--- /dev/null
+++ b/internal/ceres/summary_utils.h
@@ -0,0 +1,49 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_INTERNAL_SUMMARY_UTILS_H_
+#define CERES_INTERNAL_SUMMARY_UTILS_H_
+
+#include <vector>
+#include "ceres/solver.h"
+
+namespace ceres {
+namespace internal {
+
+class Program;
+
+void SummarizeGivenProgram(const Program& program, Solver::Summary* summary);
+void SummarizeReducedProgram(const Program& program, Solver::Summary* summary);
+void SetSummaryFinalCost(Solver::Summary* summary);
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_INTERNAL_SUMMARY_UTILS_H_
diff --git a/internal/ceres/symmetric_linear_solver_test.cc b/internal/ceres/symmetric_linear_solver_test.cc
index f33adb4..ac5a774 100644
--- a/internal/ceres/symmetric_linear_solver_test.cc
+++ b/internal/ceres/symmetric_linear_solver_test.cc
@@ -71,7 +71,7 @@
LinearSolver::Summary summary =
solver.Solve(A.get(), b.data(), per_solve_options, x.data());
- EXPECT_EQ(summary.termination_type, TOLERANCE);
+ EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_SUCCESS);
ASSERT_EQ(summary.num_iterations, 1);
ASSERT_DOUBLE_EQ(1, x(0));
@@ -128,7 +128,7 @@
LinearSolver::Summary summary =
solver.Solve(A.get(), b.data(), per_solve_options, x.data());
- EXPECT_EQ(summary.termination_type, TOLERANCE);
+ EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_SUCCESS);
ASSERT_DOUBLE_EQ(0, x(0));
ASSERT_DOUBLE_EQ(1, x(1));
diff --git a/internal/ceres/system_test.cc b/internal/ceres/system_test.cc
index 7b0e02d..be56f20 100644
--- a/internal/ceres/system_test.cc
+++ b/internal/ceres/system_test.cc
@@ -43,6 +43,8 @@
#include <cstdlib>
#include <string>
+#include "ceres/internal/port.h"
+
#include "ceres/autodiff_cost_function.h"
#include "ceres/ordered_groups.h"
#include "ceres/problem.h"
@@ -63,9 +65,10 @@
// Struct used for configuring the solver.
struct SolverConfig {
- SolverConfig(LinearSolverType linear_solver_type,
- SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
- bool use_automatic_ordering)
+ SolverConfig(
+ LinearSolverType linear_solver_type,
+ SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
+ bool use_automatic_ordering)
: linear_solver_type(linear_solver_type),
sparse_linear_algebra_library_type(sparse_linear_algebra_library_type),
use_automatic_ordering(use_automatic_ordering),
@@ -73,10 +76,11 @@
num_threads(1) {
}
- SolverConfig(LinearSolverType linear_solver_type,
- SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
- bool use_automatic_ordering,
- PreconditionerType preconditioner_type)
+ SolverConfig(
+ LinearSolverType linear_solver_type,
+ SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
+ bool use_automatic_ordering,
+ PreconditionerType preconditioner_type)
: linear_solver_type(linear_solver_type),
sparse_linear_algebra_library_type(sparse_linear_algebra_library_type),
use_automatic_ordering(use_automatic_ordering),
@@ -88,7 +92,8 @@
return StringPrintf(
"(%s, %s, %s, %s, %d)",
LinearSolverTypeToString(linear_solver_type),
- SparseLinearAlgebraLibraryTypeToString(sparse_linear_algebra_library_type),
+ SparseLinearAlgebraLibraryTypeToString(
+ sparse_linear_algebra_library_type),
use_automatic_ordering ? "AUTOMATIC" : "USER",
PreconditionerTypeToString(preconditioner_type),
num_threads);
@@ -137,8 +142,7 @@
options.num_linear_solver_threads = config.num_threads;
if (config.use_automatic_ordering) {
- delete options.linear_solver_ordering;
- options.linear_solver_ordering = NULL;
+ options.linear_solver_ordering.reset();
}
LOG(INFO) << "Running solver configuration: "
@@ -157,7 +161,7 @@
NULL,
NULL);
- CHECK_NE(summary.termination_type, ceres::NUMERICAL_FAILURE)
+ CHECK_NE(summary.termination_type, ceres::FAILURE)
<< "Solver configuration " << i << " failed.";
problems.push_back(system_test_problem);
@@ -395,7 +399,7 @@
problem_.AddResidualBlock(cost_function, NULL, camera, point);
}
- options_.linear_solver_ordering = new ParameterBlockOrdering;
+ options_.linear_solver_ordering.reset(new ParameterBlockOrdering);
// The points come before the cameras.
for (int i = 0; i < num_points_; ++i) {
@@ -491,40 +495,45 @@
ordering, \
preconditioner))
+ CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, IDENTITY);
+ CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, kUserOrdering, IDENTITY);
+
+ CONFIGURE(CGNR, SUITE_SPARSE, kAutomaticOrdering, JACOBI);
+
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, JACOBI);
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, JACOBI);
+
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, SCHUR_JACOBI);
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, SCHUR_JACOBI);
+
#ifndef CERES_NO_SUITESPARSE
CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, kAutomaticOrdering, IDENTITY);
CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, kUserOrdering, IDENTITY);
CONFIGURE(SPARSE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, IDENTITY);
CONFIGURE(SPARSE_SCHUR, SUITE_SPARSE, kUserOrdering, IDENTITY);
+
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, CLUSTER_JACOBI);
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, CLUSTER_JACOBI);
+
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, CLUSTER_TRIDIAGONAL);
+ CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, CLUSTER_TRIDIAGONAL);
#endif // CERES_NO_SUITESPARSE
#ifndef CERES_NO_CXSPARSE
+ CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kAutomaticOrdering, IDENTITY);
+ CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kUserOrdering, IDENTITY);
+
CONFIGURE(SPARSE_SCHUR, CX_SPARSE, kAutomaticOrdering, IDENTITY);
CONFIGURE(SPARSE_SCHUR, CX_SPARSE, kUserOrdering, IDENTITY);
#endif // CERES_NO_CXSPARSE
- CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, IDENTITY);
- CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, kUserOrdering, IDENTITY);
-
- CONFIGURE(CGNR, SUITE_SPARSE, kAutomaticOrdering, JACOBI);
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, JACOBI);
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, SCHUR_JACOBI);
-
-#ifndef CERES_NO_SUITESPARSE
-
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, CLUSTER_JACOBI);
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, CLUSTER_TRIDIAGONAL);
-#endif // CERES_NO_SUITESPARSE
-
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, JACOBI);
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, SCHUR_JACOBI);
-
-#ifndef CERES_NO_SUITESPARSE
-
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, CLUSTER_JACOBI);
- CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, CLUSTER_TRIDIAGONAL);
-#endif // CERES_NO_SUITESPARSE
+#ifdef CERES_USE_EIGEN_SPARSE
+ CONFIGURE(SPARSE_SCHUR, EIGEN_SPARSE, kAutomaticOrdering, IDENTITY);
+ CONFIGURE(SPARSE_SCHUR, EIGEN_SPARSE, kUserOrdering, IDENTITY);
+ CONFIGURE(SPARSE_NORMAL_CHOLESKY, EIGEN_SPARSE, kAutomaticOrdering, IDENTITY);
+ CONFIGURE(SPARSE_NORMAL_CHOLESKY, EIGEN_SPARSE, kUserOrdering, IDENTITY);
+#endif // CERES_USE_EIGEN_SPARSE
#undef CONFIGURE
diff --git a/internal/ceres/test_util.cc b/internal/ceres/test_util.cc
index a3f67bd..8af48ab 100644
--- a/internal/ceres/test_util.cc
+++ b/internal/ceres/test_util.cc
@@ -30,6 +30,7 @@
//
// Utility functions useful for testing.
+#include <algorithm>
#include <cmath>
#include "ceres/file.h"
#include "ceres/stringprintf.h"
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
index 03d6c8e..4be5619 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -44,6 +44,7 @@
#include "ceres/file.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
+#include "ceres/line_search.h"
#include "ceres/linear_least_squares_problems.h"
#include "ceres/sparse_matrix.h"
#include "ceres/stringprintf.h"
@@ -55,8 +56,53 @@
namespace ceres {
namespace internal {
namespace {
-// Small constant for various floating point issues.
-const double kEpsilon = 1e-12;
+
+LineSearch::Summary DoLineSearch(const Minimizer::Options& options,
+ const Vector& x,
+ const Vector& gradient,
+ const double cost,
+ const Vector& delta,
+ Evaluator* evaluator) {
+ LineSearchFunction line_search_function(evaluator);
+
+ LineSearch::Options line_search_options;
+ line_search_options.is_silent = true;
+ line_search_options.interpolation_type =
+ options.line_search_interpolation_type;
+ line_search_options.min_step_size = options.min_line_search_step_size;
+ line_search_options.sufficient_decrease =
+ options.line_search_sufficient_function_decrease;
+ line_search_options.max_step_contraction =
+ options.max_line_search_step_contraction;
+ line_search_options.min_step_contraction =
+ options.min_line_search_step_contraction;
+ line_search_options.max_num_iterations =
+ options.max_num_line_search_step_size_iterations;
+ line_search_options.sufficient_curvature_decrease =
+ options.line_search_sufficient_curvature_decrease;
+ line_search_options.max_step_expansion =
+ options.max_line_search_step_expansion;
+ line_search_options.function = &line_search_function;
+
+ string message;
+ scoped_ptr<LineSearch>
+ line_search(CHECK_NOTNULL(
+ LineSearch::Create(ceres::ARMIJO,
+ line_search_options,
+ &message)));
+ LineSearch::Summary summary;
+ line_search_function.Init(x, delta);
+ // Try the trust region step.
+ line_search->Search(1.0, cost, gradient.dot(delta), &summary);
+ if (!summary.success) {
+ // If that was not successful, try the negative gradient as a
+ // search direction.
+ line_search_function.Init(x, -gradient);
+ line_search->Search(1.0, cost, -gradient.squaredNorm(), &summary);
+ }
+ return summary;
+}
+
} // namespace
// Compute a scaling vector that is used to improve the conditioning
@@ -82,22 +128,29 @@
double iteration_start_time = start_time;
Init(options);
- summary->termination_type = NO_CONVERGENCE;
- summary->num_successful_steps = 0;
- summary->num_unsuccessful_steps = 0;
-
Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator);
SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian);
TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy);
+ const bool is_not_silent = !options.is_silent;
+
+ // If the problem is bounds constrained, then enable the use of a
+ // line search after the trust region step has been computed. This
+ // line search will automatically use a projected test point onto
+ // the feasible set, there by guaranteeing the feasibility of the
+ // final output.
+ //
+ // TODO(sameeragarwal): Make line search available more generally.
+ const bool use_line_search = options.is_constrained;
+
+ summary->termination_type = NO_CONVERGENCE;
+ summary->num_successful_steps = 0;
+ summary->num_unsuccessful_steps = 0;
+
const int num_parameters = evaluator->NumParameters();
const int num_effective_parameters = evaluator->NumEffectiveParameters();
const int num_residuals = evaluator->NumResiduals();
- VectorRef x_min(parameters, num_parameters);
- Vector x = x_min;
- double x_norm = x.norm();
-
Vector residuals(num_residuals);
Vector trust_region_step(num_effective_parameters);
Vector delta(num_effective_parameters);
@@ -105,6 +158,8 @@
Vector gradient(num_effective_parameters);
Vector model_residuals(num_residuals);
Vector scale(num_effective_parameters);
+ Vector negative_gradient(num_effective_parameters);
+ Vector projected_gradient_step(num_parameters);
IterationSummary iteration_summary;
iteration_summary.iteration = 0;
@@ -112,15 +167,32 @@
iteration_summary.step_is_successful = false;
iteration_summary.cost_change = 0.0;
iteration_summary.gradient_max_norm = 0.0;
+ iteration_summary.gradient_norm = 0.0;
iteration_summary.step_norm = 0.0;
iteration_summary.relative_decrease = 0.0;
iteration_summary.trust_region_radius = strategy->Radius();
- // TODO(sameeragarwal): Rename eta to linear_solver_accuracy or
- // something similar across the board.
iteration_summary.eta = options_.eta;
iteration_summary.linear_solver_iterations = 0;
iteration_summary.step_solver_time_in_seconds = 0;
+ VectorRef x_min(parameters, num_parameters);
+ Vector x = x_min;
+ // Project onto the feasible set.
+ if (options.is_constrained) {
+ delta.setZero();
+ if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+ summary->message =
+ "Unable to project initial point onto the feasible set.";
+ summary->termination_type = FAILURE;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
+ return;
+ }
+ x_min = x_plus_delta;
+ x = x_plus_delta;
+ }
+
+ double x_norm = x.norm();
+
// Do initial cost and Jacobian evaluation.
double cost = 0.0;
if (!evaluator->Evaluate(x.data(),
@@ -128,38 +200,45 @@
residuals.data(),
gradient.data(),
jacobian)) {
- LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
- summary->termination_type = NUMERICAL_FAILURE;
+ summary->message = "Residual and Jacobian evaluation failed.";
+ summary->termination_type = FAILURE;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
return;
}
- int num_consecutive_nonmonotonic_steps = 0;
- double minimum_cost = cost;
- double reference_cost = cost;
- double accumulated_reference_model_cost_change = 0.0;
- double candidate_cost = cost;
- double accumulated_candidate_model_cost_change = 0.0;
+ negative_gradient = -gradient;
+ if (!evaluator->Plus(x.data(),
+ negative_gradient.data(),
+ projected_gradient_step.data())) {
+ summary->message = "Unable to compute gradient step.";
+ summary->termination_type = FAILURE;
+ LOG(ERROR) << "Terminating: " << summary->message;
+ return;
+ }
summary->initial_cost = cost + summary->fixed_cost;
iteration_summary.cost = cost + summary->fixed_cost;
- iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();
+ iteration_summary.gradient_max_norm =
+ (x - projected_gradient_step).lpNorm<Eigen::Infinity>();
+ iteration_summary.gradient_norm = (x - projected_gradient_step).norm();
- // The initial gradient max_norm is bounded from below so that we do
- // not divide by zero.
- const double initial_gradient_max_norm =
- max(iteration_summary.gradient_max_norm, kEpsilon);
- const double absolute_gradient_tolerance =
- options_.gradient_tolerance * initial_gradient_max_norm;
-
- if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
- summary->termination_type = GRADIENT_TOLERANCE;
- VLOG(1) << "Terminating: Gradient tolerance reached."
- << "Relative gradient max norm: "
- << iteration_summary.gradient_max_norm / initial_gradient_max_norm
- << " <= " << options_.gradient_tolerance;
+ if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) {
+ summary->message = StringPrintf("Gradient tolerance reached. "
+ "Gradient max norm: %e <= %e",
+ iteration_summary.gradient_max_norm,
+ options_.gradient_tolerance);
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
return;
}
+ if (options_.jacobi_scaling) {
+ EstimateScale(*jacobian, scale.data());
+ jacobian->ScaleColumns(scale.data());
+ } else {
+ scale.setOnes();
+ }
+
iteration_summary.iteration_time_in_seconds =
WallTimeInSeconds() - iteration_start_time;
iteration_summary.cumulative_time_in_seconds =
@@ -167,34 +246,35 @@
+ summary->preprocessor_time_in_seconds;
summary->iterations.push_back(iteration_summary);
- if (options_.jacobi_scaling) {
- EstimateScale(*jacobian, scale.data());
- jacobian->ScaleColumns(scale.data());
- } else {
- scale.setOnes();
- }
-
+ int num_consecutive_nonmonotonic_steps = 0;
+ double minimum_cost = cost;
+ double reference_cost = cost;
+ double accumulated_reference_model_cost_change = 0.0;
+ double candidate_cost = cost;
+ double accumulated_candidate_model_cost_change = 0.0;
int num_consecutive_invalid_steps = 0;
bool inner_iterations_are_enabled = options.inner_iteration_minimizer != NULL;
while (true) {
bool inner_iterations_were_useful = false;
- if (!RunCallbacks(options.callbacks, iteration_summary, summary)) {
+ if (!RunCallbacks(options, iteration_summary, summary)) {
return;
}
iteration_start_time = WallTimeInSeconds();
if (iteration_summary.iteration >= options_.max_num_iterations) {
+ summary->message = "Maximum number of iterations reached.";
summary->termination_type = NO_CONVERGENCE;
- VLOG(1) << "Terminating: Maximum number of iterations reached.";
- break;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
+ return;
}
const double total_solver_time = iteration_start_time - start_time +
summary->preprocessor_time_in_seconds;
if (total_solver_time >= options_.max_solver_time_in_seconds) {
+ summary->message = "Maximum solver time reached.";
summary->termination_type = NO_CONVERGENCE;
- VLOG(1) << "Terminating: Maximum solver time reached.";
- break;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
+ return;
}
const double strategy_start_time = WallTimeInSeconds();
@@ -221,6 +301,15 @@
residuals.data(),
trust_region_step.data());
+ if (strategy_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) {
+ summary->message =
+ "Linear solver failed due to unrecoverable "
+ "non-numeric causes. Please see the error log for clues. ";
+ summary->termination_type = FAILURE;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
+ return;
+ }
+
iteration_summary = IterationSummary();
iteration_summary.iteration = summary->iterations.back().iteration + 1;
iteration_summary.step_solver_time_in_seconds =
@@ -231,7 +320,7 @@
iteration_summary.step_is_successful = false;
double model_cost_change = 0.0;
- if (strategy_summary.termination_type != FAILURE) {
+ if (strategy_summary.termination_type != LINEAR_SOLVER_FAILURE) {
// new_model_cost
// = 1/2 [f + J * step]^2
// = 1/2 [ f'f + 2f'J * step + step' * J' * J * step ]
@@ -245,9 +334,10 @@
- model_residuals.dot(residuals + model_residuals / 2.0);
if (model_cost_change < 0.0) {
- VLOG(1) << "Invalid step: current_cost: " << cost
- << " absolute difference " << model_cost_change
- << " relative difference " << (model_cost_change / cost);
+ VLOG_IF(1, is_not_silent)
+ << "Invalid step: current_cost: " << cost
+ << " absolute difference " << model_cost_change
+ << " relative difference " << (model_cost_change / cost);
} else {
iteration_summary.step_is_valid = true;
}
@@ -256,16 +346,15 @@
if (!iteration_summary.step_is_valid) {
// Invalid steps can happen due to a number of reasons, and we
// allow a limited number of successive failures, and return with
- // NUMERICAL_FAILURE if this limit is exceeded.
+ // FAILURE if this limit is exceeded.
if (++num_consecutive_invalid_steps >=
options_.max_num_consecutive_invalid_steps) {
- summary->termination_type = NUMERICAL_FAILURE;
- summary->error = StringPrintf(
- "Terminating. Number of successive invalid steps more "
+ summary->message = StringPrintf(
+ "Number of successive invalid steps more "
"than Solver::Options::max_num_consecutive_invalid_steps: %d",
options_.max_num_consecutive_invalid_steps);
-
- LOG(WARNING) << summary->error;
+ summary->termination_type = FAILURE;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
return;
}
@@ -278,6 +367,8 @@
iteration_summary.cost_change = 0.0;
iteration_summary.gradient_max_norm =
summary->iterations.back().gradient_max_norm;
+ iteration_summary.gradient_norm =
+ summary->iterations.back().gradient_norm;
iteration_summary.step_norm = 0.0;
iteration_summary.relative_decrease = 0.0;
iteration_summary.eta = options_.eta;
@@ -287,26 +378,37 @@
// Undo the Jacobian column scaling.
delta = (trust_region_step.array() * scale.array()).matrix();
- if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
- summary->termination_type = NUMERICAL_FAILURE;
- summary->error =
- "Terminating. Failed to compute Plus(x, delta, x_plus_delta).";
- LOG(WARNING) << summary->error;
- return;
+ // Try improving the step further by using an ARMIJO line
+ // search.
+ //
+ // TODO(sameeragarwal): What happens to trust region sizing as
+ // it interacts with the line search ?
+ if (use_line_search) {
+ const LineSearch::Summary line_search_summary =
+ DoLineSearch(options, x, gradient, cost, delta, evaluator);
+ if (line_search_summary.success) {
+ delta *= line_search_summary.optimal_step_size;
+ }
}
- // Try this step.
- double new_cost = numeric_limits<double>::max();
- if (!evaluator->Evaluate(x_plus_delta.data(),
- &new_cost,
- NULL, NULL, NULL)) {
- // If the evaluation of the new cost fails, treat it as a step
- // with high cost.
- LOG(WARNING) << "Step failed to evaluate. "
- << "Treating it as step with infinite cost";
- new_cost = numeric_limits<double>::max();
+ double new_cost = std::numeric_limits<double>::max();
+ if (evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+ if (!evaluator->Evaluate(x_plus_delta.data(),
+ &new_cost,
+ NULL,
+ NULL,
+ NULL)) {
+ LOG(WARNING) << "Step failed to evaluate. "
+ << "Treating it as a step with infinite cost";
+ new_cost = numeric_limits<double>::max();
+ }
} else {
+ LOG(WARNING) << "x_plus_delta = Plus(x, delta) failed. "
+ << "Treating it as a step with infinite cost";
+ }
+
+ if (new_cost < std::numeric_limits<double>::max()) {
// Check if performing an inner iteration will make it better.
if (inner_iterations_are_enabled) {
++summary->num_inner_iteration_steps;
@@ -320,30 +422,30 @@
if (!evaluator->Evaluate(inner_iteration_x.data(),
&new_cost,
NULL, NULL, NULL)) {
- VLOG(2) << "Inner iteration failed.";
+ VLOG_IF(2, is_not_silent) << "Inner iteration failed.";
new_cost = x_plus_delta_cost;
} else {
x_plus_delta = inner_iteration_x;
// Boost the model_cost_change, since the inner iteration
// improvements are not accounted for by the trust region.
model_cost_change += x_plus_delta_cost - new_cost;
- VLOG(2) << "Inner iteration succeeded; current cost: " << cost
- << " x_plus_delta_cost: " << x_plus_delta_cost
- << " new_cost: " << new_cost;
- const double inner_iteration_relative_progress =
- 1.0 - new_cost / x_plus_delta_cost;
- inner_iterations_are_enabled =
- (inner_iteration_relative_progress >
- options.inner_iteration_tolerance);
+ VLOG_IF(2, is_not_silent)
+ << "Inner iteration succeeded; Current cost: " << cost
+ << " Trust region step cost: " << x_plus_delta_cost
+ << " Inner iteration cost: " << new_cost;
inner_iterations_were_useful = new_cost < cost;
+ const double inner_iteration_relative_progress =
+ 1.0 - new_cost / x_plus_delta_cost;
// Disable inner iterations once the relative improvement
// drops below tolerance.
- if (!inner_iterations_are_enabled) {
- VLOG(2) << "Disabling inner iterations. Progress : "
- << inner_iteration_relative_progress;
- }
+ inner_iterations_are_enabled =
+ (inner_iteration_relative_progress >
+ options.inner_iteration_tolerance);
+ VLOG_IF(2, is_not_silent && !inner_iterations_are_enabled)
+ << "Disabling inner iterations. Progress : "
+ << inner_iteration_relative_progress;
}
summary->inner_iteration_time_in_seconds +=
WallTimeInSeconds() - inner_iteration_start_time;
@@ -356,12 +458,14 @@
const double step_size_tolerance = options_.parameter_tolerance *
(x_norm + options_.parameter_tolerance);
if (iteration_summary.step_norm <= step_size_tolerance) {
- VLOG(1) << "Terminating. Parameter tolerance reached. "
- << "relative step_norm: "
- << iteration_summary.step_norm /
- (x_norm + options_.parameter_tolerance)
- << " <= " << options_.parameter_tolerance;
- summary->termination_type = PARAMETER_TOLERANCE;
+ summary->message =
+ StringPrintf("Parameter tolerance reached. "
+ "Relative step_norm: %e <= %e.",
+ (iteration_summary.step_norm /
+ (x_norm + options_.parameter_tolerance)),
+ options_.parameter_tolerance);
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
return;
}
@@ -369,11 +473,13 @@
const double absolute_function_tolerance =
options_.function_tolerance * cost;
if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) {
- VLOG(1) << "Terminating. Function tolerance reached. "
- << "|cost_change|/cost: "
- << fabs(iteration_summary.cost_change) / cost
- << " <= " << options_.function_tolerance;
- summary->termination_type = FUNCTION_TOLERANCE;
+ summary->message =
+ StringPrintf("Function tolerance reached. "
+ "|cost_change|/cost: %e <= %e",
+ fabs(iteration_summary.cost_change) / cost,
+ options_.function_tolerance);
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
return;
}
@@ -447,10 +553,12 @@
if (!inner_iterations_were_useful &&
relative_decrease <= options_.min_relative_decrease) {
iteration_summary.step_is_nonmonotonic = true;
- VLOG(2) << "Non-monotonic step! "
- << " relative_decrease: " << relative_decrease
- << " historical_relative_decrease: "
- << historical_relative_decrease;
+ VLOG_IF(2, is_not_silent)
+ << "Non-monotonic step! "
+ << " relative_decrease: "
+ << relative_decrease
+ << " historical_relative_decrease: "
+ << historical_relative_decrease;
}
}
}
@@ -458,6 +566,7 @@
if (iteration_summary.step_is_successful) {
++summary->num_successful_steps;
strategy->StepAccepted(iteration_summary.relative_decrease);
+
x = x_plus_delta;
x_norm = x.norm();
@@ -468,22 +577,34 @@
residuals.data(),
gradient.data(),
jacobian)) {
- summary->termination_type = NUMERICAL_FAILURE;
- summary->error =
- "Terminating: Residual and Jacobian evaluation failed.";
- LOG(WARNING) << summary->error;
+ summary->message = "Residual and Jacobian evaluation failed.";
+ summary->termination_type = FAILURE;
+ LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
return;
}
- iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();
+ negative_gradient = -gradient;
+ if (!evaluator->Plus(x.data(),
+ negative_gradient.data(),
+ projected_gradient_step.data())) {
+ summary->message =
+ "projected_gradient_step = Plus(x, -gradient) failed.";
+ summary->termination_type = FAILURE;
+ LOG(ERROR) << "Terminating: " << summary->message;
+ return;
+ }
- if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
- summary->termination_type = GRADIENT_TOLERANCE;
- VLOG(1) << "Terminating: Gradient tolerance reached."
- << "Relative gradient max norm: "
- << (iteration_summary.gradient_max_norm /
- initial_gradient_max_norm)
- << " <= " << options_.gradient_tolerance;
+ iteration_summary.gradient_max_norm =
+ (x - projected_gradient_step).lpNorm<Eigen::Infinity>();
+ iteration_summary.gradient_norm = (x - projected_gradient_step).norm();
+
+ if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) {
+ summary->message = StringPrintf("Gradient tolerance reached. "
+ "Gradient max norm: %e <= %e",
+ iteration_summary.gradient_max_norm,
+ options_.gradient_tolerance);
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
return;
}
@@ -511,7 +632,8 @@
if (cost > candidate_cost) {
// The current iterate is has a higher cost than the
// candidate iterate. Set the candidate to this point.
- VLOG(2) << "Updating the candidate iterate to the current point.";
+ VLOG_IF(2, is_not_silent)
+ << "Updating the candidate iterate to the current point.";
candidate_cost = cost;
accumulated_candidate_model_cost_change = 0.0;
}
@@ -525,7 +647,8 @@
// iterate.
if (num_consecutive_nonmonotonic_steps ==
options.max_consecutive_nonmonotonic_steps) {
- VLOG(2) << "Resetting the reference point to the candidate point";
+ VLOG_IF(2, is_not_silent)
+ << "Resetting the reference point to the candidate point";
reference_cost = candidate_cost;
accumulated_reference_model_cost_change =
accumulated_candidate_model_cost_change;
@@ -544,8 +667,9 @@
iteration_summary.trust_region_radius = strategy->Radius();
if (iteration_summary.trust_region_radius <
options_.min_trust_region_radius) {
- summary->termination_type = PARAMETER_TOLERANCE;
- VLOG(1) << "Termination. Minimum trust region radius reached.";
+ summary->message = "Termination. Minimum trust region radius reached.";
+ summary->termination_type = CONVERGENCE;
+ VLOG_IF(1, is_not_silent) << summary->message;
return;
}
diff --git a/internal/ceres/trust_region_strategy.h b/internal/ceres/trust_region_strategy.h
index 0dcdbfe..998514f 100644
--- a/internal/ceres/trust_region_strategy.h
+++ b/internal/ceres/trust_region_strategy.h
@@ -33,7 +33,7 @@
#include <string>
#include "ceres/internal/port.h"
-#include "ceres/types.h"
+#include "ceres/linear_solver.h"
namespace ceres {
namespace internal {
@@ -106,7 +106,7 @@
Summary()
: residual_norm(0.0),
num_iterations(-1),
- termination_type(FAILURE) {
+ termination_type(LINEAR_SOLVER_FAILURE) {
}
// If the trust region problem is,
diff --git a/internal/ceres/types.cc b/internal/ceres/types.cc
index a97f1a5..4710261 100644
--- a/internal/ceres/types.cc
+++ b/internal/ceres/types.cc
@@ -96,6 +96,7 @@
switch (type) {
CASESTR(SUITE_SPARSE);
CASESTR(CX_SPARSE);
+ CASESTR(EIGEN_SPARSE);
default:
return "UNKNOWN";
}
@@ -107,6 +108,7 @@
UpperCase(&value);
STRENUM(SUITE_SPARSE);
STRENUM(CX_SPARSE);
+ STRENUM(EIGEN_SPARSE);
return false;
}
@@ -240,7 +242,7 @@
NonlinearConjugateGradientType type) {
switch (type) {
CASESTR(FLETCHER_REEVES);
- CASESTR(POLAK_RIBIRERE);
+ CASESTR(POLAK_RIBIERE);
CASESTR(HESTENES_STIEFEL);
default:
return "UNKNOWN";
@@ -252,7 +254,7 @@
NonlinearConjugateGradientType* type) {
UpperCase(&value);
STRENUM(FLETCHER_REEVES);
- STRENUM(POLAK_RIBIRERE);
+ STRENUM(POLAK_RIBIERE);
STRENUM(HESTENES_STIEFEL);
return false;
}
@@ -261,8 +263,8 @@
CovarianceAlgorithmType type) {
switch (type) {
CASESTR(DENSE_SVD);
- CASESTR(SPARSE_CHOLESKY);
- CASESTR(SPARSE_QR);
+ CASESTR(EIGEN_SPARSE_QR);
+ CASESTR(SUITE_SPARSE_QR);
default:
return "UNKNOWN";
}
@@ -273,33 +275,37 @@
CovarianceAlgorithmType* type) {
UpperCase(&value);
STRENUM(DENSE_SVD);
- STRENUM(SPARSE_CHOLESKY);
- STRENUM(SPARSE_QR);
+ STRENUM(EIGEN_SPARSE_QR);
+ STRENUM(SUITE_SPARSE_QR);
return false;
}
-const char* SolverTerminationTypeToString(SolverTerminationType type) {
+const char* VisibilityClusteringTypeToString(
+ VisibilityClusteringType type) {
switch (type) {
- CASESTR(NO_CONVERGENCE);
- CASESTR(FUNCTION_TOLERANCE);
- CASESTR(GRADIENT_TOLERANCE);
- CASESTR(PARAMETER_TOLERANCE);
- CASESTR(NUMERICAL_FAILURE);
- CASESTR(USER_ABORT);
- CASESTR(USER_SUCCESS);
- CASESTR(DID_NOT_RUN);
+ CASESTR(CANONICAL_VIEWS);
+ CASESTR(SINGLE_LINKAGE);
default:
return "UNKNOWN";
}
}
-const char* LinearSolverTerminationTypeToString(
- LinearSolverTerminationType type) {
+bool StringToVisibilityClusteringType(
+ string value,
+ VisibilityClusteringType* type) {
+ UpperCase(&value);
+ STRENUM(CANONICAL_VIEWS);
+ STRENUM(SINGLE_LINKAGE);
+ return false;
+}
+
+const char* TerminationTypeToString(TerminationType type) {
switch (type) {
- CASESTR(TOLERANCE);
- CASESTR(MAX_ITERATIONS);
- CASESTR(STAGNATION);
+ CASESTR(CONVERGENCE);
+ CASESTR(NO_CONVERGENCE);
CASESTR(FAILURE);
+ CASESTR(USER_SUCCESS);
+ CASESTR(USER_FAILURE);
default:
return "UNKNOWN";
}
diff --git a/internal/ceres/unsymmetric_linear_solver_test.cc b/internal/ceres/unsymmetric_linear_solver_test.cc
index af9dffe..0b82e6a 100644
--- a/internal/ceres/unsymmetric_linear_solver_test.cc
+++ b/internal/ceres/unsymmetric_linear_solver_test.cc
@@ -57,7 +57,7 @@
}
void TestSolver(const LinearSolver::Options& options) {
- scoped_ptr<LinearSolver> solver(LinearSolver::Create(options));
+
LinearSolver::PerSolveOptions per_solve_options;
LinearSolver::Summary unregularized_solve_summary;
@@ -84,13 +84,17 @@
} else {
LOG(FATAL) << "Unknown linear solver : " << options.type;
}
+
// Unregularized
+ scoped_ptr<LinearSolver> solver(LinearSolver::Create(options));
unregularized_solve_summary =
solver->Solve(transformed_A.get(),
b_.get(),
per_solve_options,
x_unregularized.data());
+ // Sparsity structure is changing, reset the solver.
+ solver.reset(LinearSolver::Create(options));
// Regularized solution
per_solve_options.D = D_.get();
regularized_solve_summary =
@@ -99,15 +103,23 @@
per_solve_options,
x_regularized.data());
- EXPECT_EQ(unregularized_solve_summary.termination_type, TOLERANCE);
+ EXPECT_EQ(unregularized_solve_summary.termination_type,
+ LINEAR_SOLVER_SUCCESS);
for (int i = 0; i < A_->num_cols(); ++i) {
- EXPECT_NEAR(sol_unregularized_[i], x_unregularized[i], 1e-8);
+ EXPECT_NEAR(sol_unregularized_[i], x_unregularized[i], 1e-8)
+ << "\nExpected: "
+ << ConstVectorRef(sol_unregularized_.get(), A_->num_cols()).transpose()
+ << "\nActual: " << x_unregularized.transpose();
}
- EXPECT_EQ(regularized_solve_summary.termination_type, TOLERANCE);
+ EXPECT_EQ(regularized_solve_summary.termination_type,
+ LINEAR_SOLVER_SUCCESS);
for (int i = 0; i < A_->num_cols(); ++i) {
- EXPECT_NEAR(sol_regularized_[i], x_regularized[i], 1e-8);
+ EXPECT_NEAR(sol_regularized_[i], x_regularized[i], 1e-8)
+ << "\nExpected: "
+ << ConstVectorRef(sol_regularized_.get(), A_->num_cols()).transpose()
+ << "\nActual: " << x_regularized.transpose();
}
}
@@ -166,6 +178,15 @@
options.use_postordering = true;
TestSolver(options);
}
+
+TEST_F(UnsymmetricLinearSolverTest,
+ SparseNormalCholeskyUsingSuiteSparseDynamicSparsity) {
+ LinearSolver::Options options;
+ options.sparse_linear_algebra_library_type = SUITE_SPARSE;
+ options.type = SPARSE_NORMAL_CHOLESKY;
+ options.dynamic_sparsity = true;
+ TestSolver(options);
+}
#endif
#ifndef CERES_NO_CXSPARSE
@@ -186,7 +207,46 @@
options.use_postordering = true;
TestSolver(options);
}
+
+TEST_F(UnsymmetricLinearSolverTest,
+ SparseNormalCholeskyUsingCXSparseDynamicSparsity) {
+ LinearSolver::Options options;
+ options.sparse_linear_algebra_library_type = CX_SPARSE;
+ options.type = SPARSE_NORMAL_CHOLESKY;
+ options.dynamic_sparsity = true;
+ TestSolver(options);
+}
#endif
+#ifdef CERES_USE_EIGEN_SPARSE
+TEST_F(UnsymmetricLinearSolverTest,
+ SparseNormalCholeskyUsingEigenPreOrdering) {
+ LinearSolver::Options options;
+ options.sparse_linear_algebra_library_type = EIGEN_SPARSE;
+ options.type = SPARSE_NORMAL_CHOLESKY;
+ options.use_postordering = false;
+ TestSolver(options);
+}
+
+TEST_F(UnsymmetricLinearSolverTest,
+ SparseNormalCholeskyUsingEigenPostOrdering) {
+ LinearSolver::Options options;
+ options.sparse_linear_algebra_library_type = EIGEN_SPARSE;
+ options.type = SPARSE_NORMAL_CHOLESKY;
+ options.use_postordering = true;
+ TestSolver(options);
+}
+
+TEST_F(UnsymmetricLinearSolverTest,
+ SparseNormalCholeskyUsingEigenDynamicSparsity) {
+ LinearSolver::Options options;
+ options.sparse_linear_algebra_library_type = EIGEN_SPARSE;
+ options.type = SPARSE_NORMAL_CHOLESKY;
+ options.dynamic_sparsity = true;
+ TestSolver(options);
+}
+
+#endif // CERES_USE_EIGEN_SPARSE
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/visibility.cc b/internal/ceres/visibility.cc
index acfa45b..b3ee185 100644
--- a/internal/ceres/visibility.cc
+++ b/internal/ceres/visibility.cc
@@ -28,6 +28,9 @@
//
// Author: kushalav@google.com (Avanish Kushal)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include "ceres/visibility.h"
diff --git a/internal/ceres/visibility.h b/internal/ceres/visibility.h
index 2d1e6f8..5ddd3a5 100644
--- a/internal/ceres/visibility.h
+++ b/internal/ceres/visibility.h
@@ -35,6 +35,9 @@
#ifndef CERES_INTERNAL_VISIBILITY_H_
#define CERES_INTERNAL_VISIBILITY_H_
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include <set>
diff --git a/internal/ceres/visibility_based_preconditioner.cc b/internal/ceres/visibility_based_preconditioner.cc
index 7af1339..695eedc 100644
--- a/internal/ceres/visibility_based_preconditioner.cc
+++ b/internal/ceres/visibility_based_preconditioner.cc
@@ -28,6 +28,9 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include "ceres/visibility_based_preconditioner.h"
@@ -43,12 +46,12 @@
#include "ceres/block_sparse_matrix.h"
#include "ceres/canonical_views_clustering.h"
#include "ceres/collections_port.h"
-#include "ceres/detect_structure.h"
#include "ceres/graph.h"
#include "ceres/graph_algorithms.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/linear_solver.h"
#include "ceres/schur_eliminator.h"
+#include "ceres/single_linkage_clustering.h"
#include "ceres/visibility.h"
#include "glog/logging.h"
@@ -61,8 +64,9 @@
//
// This will require some more work on the clustering algorithm and
// possibly some more refactoring of the code.
-static const double kSizePenaltyWeight = 3.0;
-static const double kSimilarityPenaltyWeight = 0.0;
+static const double kCanonicalViewsSizePenaltyWeight = 3.0;
+static const double kCanonicalViewsSimilarityPenaltyWeight = 0.0;
+static const double kSingleLinkageMinSimilarity = 0.9;
VisibilityBasedPreconditioner::VisibilityBasedPreconditioner(
const CompressedRowBlockStructure& bs,
@@ -188,17 +192,31 @@
scoped_ptr<Graph<int> > schur_complement_graph(
CHECK_NOTNULL(CreateSchurComplementGraph(visibility)));
- CanonicalViewsClusteringOptions options;
- options.size_penalty_weight = kSizePenaltyWeight;
- options.similarity_penalty_weight = kSimilarityPenaltyWeight;
-
- vector<int> centers;
HashMap<int, int> membership;
- ComputeCanonicalViewsClustering(*schur_complement_graph,
- options,
- ¢ers,
- &membership);
- num_clusters_ = centers.size();
+
+ if (options_.visibility_clustering_type == CANONICAL_VIEWS) {
+ vector<int> centers;
+ CanonicalViewsClusteringOptions clustering_options;
+ clustering_options.size_penalty_weight =
+ kCanonicalViewsSizePenaltyWeight;
+ clustering_options.similarity_penalty_weight =
+ kCanonicalViewsSimilarityPenaltyWeight;
+ ComputeCanonicalViewsClustering(clustering_options,
+ *schur_complement_graph,
+ ¢ers,
+ &membership);
+ num_clusters_ = centers.size();
+ } else if (options_.visibility_clustering_type == SINGLE_LINKAGE) {
+ SingleLinkageClusteringOptions clustering_options;
+ clustering_options.min_similarity =
+ kSingleLinkageMinSimilarity;
+ num_clusters_ = ComputeSingleLinkageClustering(clustering_options,
+ *schur_complement_graph,
+ &membership);
+ } else {
+ LOG(FATAL) << "Unknown visibility clustering algorithm.";
+ }
+
CHECK_GT(num_clusters_, 0);
VLOG(2) << "num_clusters: " << num_clusters_;
FlattenMembershipMap(membership, &cluster_membership_);
@@ -313,14 +331,11 @@
LinearSolver::Options eliminator_options;
eliminator_options.elimination_groups = options_.elimination_groups;
eliminator_options.num_threads = options_.num_threads;
-
- DetectStructure(bs, options_.elimination_groups[0],
- &eliminator_options.row_block_size,
- &eliminator_options.e_block_size,
- &eliminator_options.f_block_size);
-
+ eliminator_options.e_block_size = options_.e_block_size;
+ eliminator_options.f_block_size = options_.f_block_size;
+ eliminator_options.row_block_size = options_.row_block_size;
eliminator_.reset(SchurEliminatorBase::Create(eliminator_options));
- eliminator_->Init(options_.elimination_groups[0], &bs);
+ eliminator_->Init(eliminator_options.elimination_groups[0], &bs);
}
// Update the values of the preconditioner matrix and factorize it.
@@ -356,14 +371,18 @@
//
// Doing the factorization like this saves us matrix mass when
// scaling is not needed, which is quite often in our experience.
- bool status = Factorize();
+ LinearSolverTerminationType status = Factorize();
+
+ if (status == LINEAR_SOLVER_FATAL_ERROR) {
+ return false;
+ }
// The scaling only affects the tri-diagonal case, since
// ScaleOffDiagonalBlocks only pays attenion to the cells that
// belong to the edges of the degree-2 forest. In the CLUSTER_JACOBI
// case, the preconditioner is guaranteed to be positive
// semidefinite.
- if (!status && options_.type == CLUSTER_TRIDIAGONAL) {
+ if (status == LINEAR_SOLVER_FAILURE && options_.type == CLUSTER_TRIDIAGONAL) {
VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal "
<< "scaling";
ScaleOffDiagonalCells();
@@ -371,7 +390,7 @@
}
VLOG(2) << "Compute time: " << time(NULL) - start_time;
- return status;
+ return (status == LINEAR_SOLVER_SUCCESS);
}
// Consider the preconditioner matrix as meta-block matrix, whose
@@ -408,7 +427,7 @@
// Compute the sparse Cholesky factorization of the preconditioner
// matrix.
-bool VisibilityBasedPreconditioner::Factorize() {
+LinearSolverTerminationType VisibilityBasedPreconditioner::Factorize() {
// Extract the TripletSparseMatrix that is used for actually storing
// S and convert it into a cholmod_sparse object.
cholmod_sparse* lhs = ss_.CreateSparseMatrix(
@@ -419,14 +438,21 @@
// matrix contains the values.
lhs->stype = 1;
+ // TODO(sameeragarwal): Refactor to pipe this up and out.
+ string status;
+
// Symbolic factorization is computed if we don't already have one handy.
if (factor_ == NULL) {
- factor_ = ss_.BlockAnalyzeCholesky(lhs, block_size_, block_size_);
+ factor_ = ss_.BlockAnalyzeCholesky(lhs, block_size_, block_size_, &status);
}
- bool status = ss_.Cholesky(lhs, factor_);
+ const LinearSolverTerminationType termination_type =
+ (factor_ != NULL)
+ ? ss_.Cholesky(lhs, factor_, &status)
+ : LINEAR_SOLVER_FATAL_ERROR;
+
ss_.Free(lhs);
- return status;
+ return termination_type;
}
void VisibilityBasedPreconditioner::RightMultiply(const double* x,
@@ -437,7 +463,10 @@
const int num_rows = m_->num_rows();
memcpy(CHECK_NOTNULL(tmp_rhs_)->x, x, m_->num_rows() * sizeof(*x));
- cholmod_dense* solution = CHECK_NOTNULL(ss->Solve(factor_, tmp_rhs_));
+ // TODO(sameeragarwal): Better error handling.
+ string status;
+ cholmod_dense* solution =
+ CHECK_NOTNULL(ss->Solve(factor_, tmp_rhs_, &status));
memcpy(y, solution->x, sizeof(*y) * num_rows);
ss->Free(solution);
}
@@ -546,11 +575,17 @@
// cluster ids. Convert this into a flat array for quick lookup. It is
// possible that some of the vertices may not be associated with any
// cluster. In that case, randomly assign them to one of the clusters.
+//
+// The cluster ids can be non-contiguous integers. So as we flatten
+// the membership_map, we also map the cluster ids to a contiguous set
+// of integers so that the cluster ids are in [0, num_clusters_).
void VisibilityBasedPreconditioner::FlattenMembershipMap(
const HashMap<int, int>& membership_map,
vector<int>* membership_vector) const {
CHECK_NOTNULL(membership_vector)->resize(0);
membership_vector->resize(num_blocks_, -1);
+
+ HashMap<int, int> cluster_id_to_index;
// Iterate over the cluster membership map and update the
// cluster_membership_ vector assigning arbitrary cluster ids to
// the few cameras that have not been clustered.
@@ -571,7 +606,16 @@
cluster_id = camera_id % num_clusters_;
}
- membership_vector->at(camera_id) = cluster_id;
+ const int index = FindWithDefault(cluster_id_to_index,
+ cluster_id,
+ cluster_id_to_index.size());
+
+ if (index == cluster_id_to_index.size()) {
+ cluster_id_to_index[cluster_id] = index;
+ }
+
+ CHECK_LT(index, num_clusters_);
+ membership_vector->at(camera_id) = index;
}
}
diff --git a/internal/ceres/visibility_based_preconditioner.h b/internal/ceres/visibility_based_preconditioner.h
index c58b1a7..70cea83 100644
--- a/internal/ceres/visibility_based_preconditioner.h
+++ b/internal/ceres/visibility_based_preconditioner.h
@@ -55,6 +55,7 @@
#include "ceres/graph.h"
#include "ceres/internal/macros.h"
#include "ceres/internal/scoped_ptr.h"
+#include "ceres/linear_solver.h"
#include "ceres/preconditioner.h"
#include "ceres/suitesparse.h"
@@ -147,7 +148,7 @@
void ComputeClusterTridiagonalSparsity(const CompressedRowBlockStructure& bs);
void InitStorage(const CompressedRowBlockStructure& bs);
void InitEliminator(const CompressedRowBlockStructure& bs);
- bool Factorize();
+ LinearSolverTerminationType Factorize();
void ScaleOffDiagonalCells();
void ClusterCameras(const vector< set<int> >& visibility);
diff --git a/internal/ceres/visibility_based_preconditioner_test.cc b/internal/ceres/visibility_based_preconditioner_test.cc
index 2edbb18..c718b5e 100644
--- a/internal/ceres/visibility_based_preconditioner_test.cc
+++ b/internal/ceres/visibility_based_preconditioner_test.cc
@@ -28,6 +28,9 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include "ceres/visibility_based_preconditioner.h"
diff --git a/internal/ceres/visibility_test.cc b/internal/ceres/visibility_test.cc
index 3cfb232..0e22f88 100644
--- a/internal/ceres/visibility_test.cc
+++ b/internal/ceres/visibility_test.cc
@@ -29,6 +29,9 @@
// Author: kushalav@google.com (Avanish Kushal)
// sameeragarwal@google.com (Sameer Agarwal)
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
#ifndef CERES_NO_SUITESPARSE
#include "ceres/visibility.h"