Initial commit of Ceres Solver.
diff --git a/internal/ceres/residual_block_test.cc b/internal/ceres/residual_block_test.cc
new file mode 100644
index 0000000..92b79f6
--- /dev/null
+++ b/internal/ceres/residual_block_test.cc
@@ -0,0 +1,326 @@
+// 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)
+
+#include "ceres/residual_block.h"
+
+#include "gtest/gtest.h"
+#include "ceres/parameter_block.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/local_parameterization.h"
+
+namespace ceres {
+namespace internal {
+
+// Trivial cost function that accepts three arguments.
+class TernaryCostFunction: public CostFunction {
+ public:
+ TernaryCostFunction(int num_residuals,
+ int16 parameter_block1_size,
+ int16 parameter_block2_size,
+ int16 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);
+ mutable_parameter_block_sizes()->push_back(parameter_block3_size);
+ }
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ for (int i = 0; i < num_residuals(); ++i) {
+ residuals[i] = i;
+ }
+ if (jacobians) {
+ for (int k = 0; k < 3; ++k) {
+ if (jacobians[k] != NULL) {
+ MatrixRef jacobian(jacobians[k],
+ num_residuals(),
+ parameter_block_sizes()[k]);
+ jacobian.setConstant(k);
+ }
+ }
+ }
+ return true;
+ }
+};
+
+TEST(ResidualBlock, EvaluteWithNoLossFunctionOrLocalParameterizations) {
+ double scratch[64];
+
+ // Prepare the parameter blocks.
+ double values_x[2];
+ ParameterBlock x(values_x, 2);
+
+ double values_y[3];
+ ParameterBlock y(values_y, 3);
+
+ double values_z[4];
+ ParameterBlock z(values_z, 4);
+
+ vector<ParameterBlock*> parameters;
+ parameters.push_back(&x);
+ parameters.push_back(&y);
+ parameters.push_back(&z);
+
+ TernaryCostFunction cost_function(3, 2, 3, 4);
+
+ // Create the object under tests.
+ ResidualBlock residual_block(&cost_function, NULL, parameters);
+
+ // Verify getters.
+ EXPECT_EQ(&cost_function, residual_block.cost_function());
+ EXPECT_EQ(NULL, residual_block.loss_function());
+ EXPECT_EQ(parameters[0], residual_block.parameter_blocks()[0]);
+ EXPECT_EQ(parameters[1], residual_block.parameter_blocks()[1]);
+ EXPECT_EQ(parameters[2], residual_block.parameter_blocks()[2]);
+ EXPECT_EQ(3, residual_block.NumScratchDoublesForEvaluate());
+
+ // Verify cost-only evaluation.
+ double cost;
+ residual_block.Evaluate(&cost, NULL, NULL, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+
+ // Verify cost and residual evaluation.
+ double residuals[3];
+ residual_block.Evaluate(&cost, residuals, NULL, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+ EXPECT_EQ(0.0, residuals[0]);
+ EXPECT_EQ(1.0, residuals[1]);
+ EXPECT_EQ(2.0, residuals[2]);
+
+ // Verify cost, residual, and jacobian evaluation.
+ cost = 0.0;
+ VectorRef(residuals, 3).setConstant(0.0);
+
+ Matrix jacobian_rx(3, 2);
+ Matrix jacobian_ry(3, 3);
+ Matrix jacobian_rz(3, 4);
+
+ jacobian_rx.setConstant(-1.0);
+ jacobian_ry.setConstant(-1.0);
+ jacobian_rz.setConstant(-1.0);
+
+ double *jacobian_ptrs[3] = {
+ jacobian_rx.data(),
+ jacobian_ry.data(),
+ jacobian_rz.data()
+ };
+
+ residual_block.Evaluate(&cost, residuals, jacobian_ptrs, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+ EXPECT_EQ(0.0, residuals[0]);
+ EXPECT_EQ(1.0, residuals[1]);
+ EXPECT_EQ(2.0, residuals[2]);
+
+ EXPECT_TRUE((jacobian_rx.array() == 0.0).all()) << "\n" << jacobian_rx;
+ EXPECT_TRUE((jacobian_ry.array() == 1.0).all()) << "\n" << jacobian_ry;
+ EXPECT_TRUE((jacobian_rz.array() == 2.0).all()) << "\n" << jacobian_rz;
+
+ // Verify cost, residual, and partial jacobian evaluation.
+ cost = 0.0;
+ VectorRef(residuals, 3).setConstant(0.0);
+ jacobian_rx.setConstant(-1.0);
+ jacobian_ry.setConstant(-1.0);
+ jacobian_rz.setConstant(-1.0);
+
+ jacobian_ptrs[1] = NULL; // Don't compute the jacobian for y.
+
+ residual_block.Evaluate(&cost, residuals, jacobian_ptrs, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+ EXPECT_EQ(0.0, residuals[0]);
+ EXPECT_EQ(1.0, residuals[1]);
+ EXPECT_EQ(2.0, residuals[2]);
+
+ EXPECT_TRUE((jacobian_rx.array() == 0.0).all()) << "\n" << jacobian_rx;
+ EXPECT_TRUE((jacobian_ry.array() == -1.0).all()) << "\n" << jacobian_ry;
+ EXPECT_TRUE((jacobian_rz.array() == 2.0).all()) << "\n" << jacobian_rz;
+}
+
+// Trivial cost function that accepts three arguments.
+class LocallyParameterizedCostFunction: public SizedCostFunction<3, 2, 3, 4> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ for (int i = 0; i < num_residuals(); ++i) {
+ residuals[i] = i;
+ }
+ if (jacobians) {
+ for (int k = 0; k < 3; ++k) {
+ // The jacobians here are full sized, but they are transformed in the
+ // evaluator into the "local" jacobian. In the tests, the "subset
+ // constant" parameterization is used, which should pick out columns
+ // from these jacobians. Put values in the jacobian that make this
+ // obvious; in particular, make the jacobians like this:
+ //
+ // 0 1 2 3 4 ...
+ // 0 1 2 3 4 ...
+ // 0 1 2 3 4 ...
+ //
+ if (jacobians[k] != NULL) {
+ MatrixRef jacobian(jacobians[k],
+ num_residuals(),
+ parameter_block_sizes()[k]);
+ for (int j = 0; j < k + 2; ++j) {
+ jacobian.col(j).setConstant(j);
+ }
+ }
+ }
+ }
+ return true;
+ }
+};
+
+TEST(ResidualBlock, EvaluteWithLocalParameterizations) {
+ double scratch[64];
+
+ // Prepare the parameter blocks.
+ double values_x[2];
+ ParameterBlock x(values_x, 2);
+
+ double values_y[3];
+ ParameterBlock y(values_y, 3);
+
+ double values_z[4];
+ ParameterBlock z(values_z, 4);
+
+ vector<ParameterBlock*> parameters;
+ parameters.push_back(&x);
+ parameters.push_back(&y);
+ parameters.push_back(&z);
+
+ // Make x have the first component fixed.
+ vector<int> x_fixed;
+ x_fixed.push_back(0);
+ SubsetParameterization x_parameterization(2, x_fixed);
+ x.SetParameterization(&x_parameterization);
+
+ // Make z have the last and last component fixed.
+ vector<int> z_fixed;
+ z_fixed.push_back(2);
+ SubsetParameterization z_parameterization(4, z_fixed);
+ z.SetParameterization(&z_parameterization);
+
+ LocallyParameterizedCostFunction cost_function;
+
+ // Create the object under tests.
+ ResidualBlock residual_block(&cost_function, NULL, parameters);
+
+ // Verify getters.
+ EXPECT_EQ(&cost_function, residual_block.cost_function());
+ EXPECT_EQ(NULL, residual_block.loss_function());
+ EXPECT_EQ(parameters[0], residual_block.parameter_blocks()[0]);
+ EXPECT_EQ(parameters[1], residual_block.parameter_blocks()[1]);
+ EXPECT_EQ(parameters[2], residual_block.parameter_blocks()[2]);
+ EXPECT_EQ(3*(2 + 4) + 3, residual_block.NumScratchDoublesForEvaluate());
+
+ // Verify cost-only evaluation.
+ double cost;
+ residual_block.Evaluate(&cost, NULL, NULL, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+
+ // Verify cost and residual evaluation.
+ double residuals[3];
+ residual_block.Evaluate(&cost, residuals, NULL, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+ EXPECT_EQ(0.0, residuals[0]);
+ EXPECT_EQ(1.0, residuals[1]);
+ EXPECT_EQ(2.0, residuals[2]);
+
+ // Verify cost, residual, and jacobian evaluation.
+ cost = 0.0;
+ VectorRef(residuals, 3).setConstant(0.0);
+
+ Matrix jacobian_rx(3, 1); // Since the first element is fixed.
+ Matrix jacobian_ry(3, 3);
+ Matrix jacobian_rz(3, 3); // Since the third element is fixed.
+
+ jacobian_rx.setConstant(-1.0);
+ jacobian_ry.setConstant(-1.0);
+ jacobian_rz.setConstant(-1.0);
+
+ double *jacobian_ptrs[3] = {
+ jacobian_rx.data(),
+ jacobian_ry.data(),
+ jacobian_rz.data()
+ };
+
+ residual_block.Evaluate(&cost, residuals, jacobian_ptrs, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+ EXPECT_EQ(0.0, residuals[0]);
+ EXPECT_EQ(1.0, residuals[1]);
+ EXPECT_EQ(2.0, residuals[2]);
+
+ Matrix expected_jacobian_rx(3, 1);
+ expected_jacobian_rx << 1.0, 1.0, 1.0;
+
+ Matrix expected_jacobian_ry(3, 3);
+ expected_jacobian_ry << 0.0, 1.0, 2.0,
+ 0.0, 1.0, 2.0,
+ 0.0, 1.0, 2.0;
+
+ Matrix expected_jacobian_rz(3, 3);
+ expected_jacobian_rz << 0.0, 1.0, /* 2.0, */ 3.0, // 3rd parameter constant.
+ 0.0, 1.0, /* 2.0, */ 3.0,
+ 0.0, 1.0, /* 2.0, */ 3.0;
+
+ EXPECT_EQ(expected_jacobian_rx, jacobian_rx)
+ << "\nExpected:\n" << expected_jacobian_rx
+ << "\nActual:\n" << jacobian_rx;
+ EXPECT_EQ(expected_jacobian_ry, jacobian_ry)
+ << "\nExpected:\n" << expected_jacobian_ry
+ << "\nActual:\n" << jacobian_ry;
+ EXPECT_EQ(expected_jacobian_rz, jacobian_rz)
+ << "\nExpected:\n " << expected_jacobian_rz
+ << "\nActual:\n" << jacobian_rz;
+
+ // Verify cost, residual, and partial jacobian evaluation.
+ cost = 0.0;
+ VectorRef(residuals, 3).setConstant(0.0);
+ jacobian_rx.setConstant(-1.0);
+ jacobian_ry.setConstant(-1.0);
+ jacobian_rz.setConstant(-1.0);
+
+ jacobian_ptrs[1] = NULL; // Don't compute the jacobian for y.
+
+ residual_block.Evaluate(&cost, residuals, jacobian_ptrs, scratch);
+ EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
+ EXPECT_EQ(0.0, residuals[0]);
+ EXPECT_EQ(1.0, residuals[1]);
+ EXPECT_EQ(2.0, residuals[2]);
+
+ EXPECT_EQ(expected_jacobian_rx, jacobian_rx);
+ EXPECT_TRUE((jacobian_ry.array() == -1.0).all()) << "\n" << jacobian_ry;
+ EXPECT_EQ(expected_jacobian_rz, jacobian_rz);
+}
+
+} // namespace internal
+} // namespace ceres
