Initial commit of Ceres Solver.
diff --git a/internal/ceres/conditioned_cost_function.cc b/internal/ceres/conditioned_cost_function.cc
new file mode 100644
index 0000000..ca80bfb
--- /dev/null
+++ b/internal/ceres/conditioned_cost_function.cc
@@ -0,0 +1,130 @@
+// 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: wjr@google.com (William Rucklidge)
+//
+// This file contains the implementation of the conditioned cost function.
+
+#include "ceres/conditioned_cost_function.h"
+
+#include <cstddef>
+
+#include <glog/logging.h>
+#include "ceres/stl_util.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/types.h"
+
+namespace ceres {
+
+// This cost function has the same dimensions (parameters, residuals) as
+// the one it's wrapping.
+ConditionedCostFunction::ConditionedCostFunction(
+ CostFunction* wrapped_cost_function,
+ const vector<CostFunction*>& conditioners,
+ Ownership ownership)
+ : wrapped_cost_function_(wrapped_cost_function),
+ conditioners_(conditioners),
+ ownership_(ownership) {
+ // Set up our dimensions.
+ set_num_residuals(wrapped_cost_function_->num_residuals());
+ *mutable_parameter_block_sizes() =
+ wrapped_cost_function_->parameter_block_sizes();
+
+ // Sanity-check the conditioners' dimensions.
+ CHECK_EQ(wrapped_cost_function_->num_residuals(), conditioners_.size());
+ for (int i = 0; i < wrapped_cost_function_->num_residuals(); i++) {
+ if (conditioners[i]) {
+ CHECK_EQ(1, conditioners[i]->num_residuals());
+ CHECK_EQ(1, conditioners[i]->parameter_block_sizes().size());
+ CHECK_EQ(1, conditioners[i]->parameter_block_sizes()[0]);
+ }
+ }
+}
+
+ConditionedCostFunction::~ConditionedCostFunction() {
+ if (ownership_ == TAKE_OWNERSHIP) {
+ STLDeleteElements(&conditioners_);
+ } else {
+ wrapped_cost_function_.release();
+ }
+}
+
+bool ConditionedCostFunction::Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ bool success = wrapped_cost_function_->Evaluate(parameters, residuals,
+ jacobians);
+ if (!success) {
+ return false;
+ }
+
+ for (int r = 0; r < wrapped_cost_function_->num_residuals(); r++) {
+ // On output, we want to have
+ // residuals[r] = conditioners[r](wrapped_residuals[r])
+ // For parameter block i, column c,
+ // jacobians[i][r*parameter_block_size_[i] + c] =
+ // = d residual[r] / d parameters[i][c]
+ // = conditioners[r]'(wrapped_residuals[r]) *
+ // d wrapped_residuals[r] / d parameters[i][c]
+ if (conditioners_[r]) {
+ double conditioner_derivative;
+ double* conditioner_derivative_pointer = &conditioner_derivative;
+ double** conditioner_derivative_pointer2 =
+ &conditioner_derivative_pointer;
+ if (!jacobians) {
+ conditioner_derivative_pointer2 = NULL;
+ }
+
+ double unconditioned_residual = residuals[r];
+ double* parameter_pointer = &unconditioned_residual;
+ success = conditioners_[r]->Evaluate(¶meter_pointer,
+ &residuals[r],
+ conditioner_derivative_pointer2);
+ if (!success) {
+ return false;
+ }
+
+ if (jacobians) {
+ for (int i = 0;
+ i < wrapped_cost_function_->parameter_block_sizes().size();
+ i++) {
+ if (jacobians[i]) {
+ int parameter_block_size =
+ wrapped_cost_function_->parameter_block_sizes()[i];
+ VectorRef jacobian_row(jacobians[i] + r * parameter_block_size,
+ parameter_block_size, 1);
+ jacobian_row *= conditioner_derivative;
+ }
+ }
+ }
+ }
+ }
+ return true;
+}
+
+} // namespace ceres
