MYNT-EYE-S-SDK/3rdparty/ceres-solver-1.11.0/internal/ceres/residual_block_test.cc
2019-01-03 16:25:18 +08:00

329 lines
11 KiB
C++

// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2015 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
#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 {
using std::vector;
// Trivial cost function that accepts three arguments.
class TernaryCostFunction: public CostFunction {
public:
TernaryCostFunction(int num_residuals,
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);
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, -1);
double values_y[3];
ParameterBlock y(values_y, 3, -1);
double values_z[4];
ParameterBlock z(values_z, 4, -1);
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, -1);
// 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(true, &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(true, &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(true, &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(true, &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, -1);
double values_y[3];
ParameterBlock y(values_y, 3, -1);
double values_z[4];
ParameterBlock z(values_z, 4, -1);
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, -1);
// 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(true, &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(true, &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(true, &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(true, &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