saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 16:40:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Bug fix in DynamicAutoDiffCostFunction

Browse Source 
Add handling of constant parameter blocks.

Change-Id: I8b2ea79f47e190604fc4bed27705798240689f71
This commit is contained in:
Sameer Agarwal
2013-02-12 14:22:40 -08:00
parent 3130b3cea4
commit 974513a41f
2 changed files with 144 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/dynamic_autodiff_cost_function.h
+36 -15
View File
@@ -121,10 +121,22 @@ class DynamicAutoDiffCostFunction : public CostFunction {
vector<Jet<double, Stride> > output_jets(num_residuals());
// Make the parameter pack that is sent to the functor (reused).
vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks);
vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks, NULL);
int num_active_parameters = 0;
int start_derivative_section = -1;
for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) {
jet_parameters[i] = &input_jets[parameter_cursor];
for (int j = 0; j < parameter_block_sizes()[i]; ++j, parameter_cursor++) {
const int parameter_block_size = parameter_block_sizes()[i];
if (jacobians[i] != NULL) {
start_derivative_section =
(start_derivative_section == -1)
? parameter_cursor
: start_derivative_section;
num_active_parameters += parameter_block_size;
}
for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) {
input_jets[parameter_cursor].a = parameters[i][j];
}
}
@@ -132,21 +144,24 @@ class DynamicAutoDiffCostFunction : public CostFunction {
// Evaluate all of the strides. Each stride is a chunk of the derivative to
// evaluate, typically some size proportional to the size of the SIMD
// registers of the CPU.
int num_strides = int(ceil(num_parameters / float(Stride)));
int num_strides = int(ceil(num_active_parameters / float(Stride)));
for (int pass = 0; pass < num_strides; ++pass) {
const int start_derivative_section = pass * Stride;
const int end_derivative_section = std::min((pass + 1) * Stride,
num_parameters);
// Set most of the jet components to zero, except for the active
// parameters, which occur in a contiguos block of size Stride.
// Set most of the jet components to zero, except for
// non-constant #Stride parameters.
int active_parameter_count = 0;
int end_derivative_section = start_derivative_section;
for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) {
for (int j = 0; j < parameter_block_sizes()[i];
++j, parameter_cursor++) {
input_jets[parameter_cursor].v.setZero();
if (parameter_cursor >= start_derivative_section &&
parameter_cursor < end_derivative_section) {
input_jets[parameter_cursor]
.v[parameter_cursor - start_derivative_section] = 1.0;
active_parameter_count < Stride) {
if (jacobians[i] != NULL) {
input_jets[parameter_cursor]
.v[parameter_cursor - start_derivative_section] = 1.0;
++active_parameter_count;
}
++end_derivative_section;
}
}
}
@@ -156,14 +171,18 @@ class DynamicAutoDiffCostFunction : public CostFunction {
}
// Copy the pieces of the jacobians into their final place.
active_parameter_count = 0;
for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) {
for (int j = 0; j < parameter_block_sizes()[i];
++j, parameter_cursor++) {
if (parameter_cursor >= start_derivative_section &&
parameter_cursor < end_derivative_section) {
for (int k = 0; k < num_residuals(); ++k) {
jacobians[i][k * parameter_block_sizes()[i] + j] =
output_jets[k].v[parameter_cursor - start_derivative_section];
active_parameter_count < Stride) {
if (jacobians[i] != NULL) {
for (int k = 0; k < num_residuals(); ++k) {
jacobians[i][k * parameter_block_sizes()[i] + j] =
output_jets[k].v[parameter_cursor - start_derivative_section];
}
++active_parameter_count;
}
}
}
@@ -176,6 +195,8 @@ class DynamicAutoDiffCostFunction : public CostFunction {
residuals[k] = output_jets[k].a;
}
}
start_derivative_section = end_derivative_section;
}
return true;
}
internal/ceres/dynamic_autodiff_cost_function_test.cc
+108
View File
@@ -162,5 +162,113 @@ TEST(DynamicAutodiffCostFunctionTest, TestJacobian) {
}
}
TEST(DynamicAutodiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) {
// 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);
DynamicAutoDiffCostFunction<MyCostFunctor, 3> 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] = &param_block_0[0];
parameter_blocks[1] = &param_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_EQ(1.0, jacobian_vect[1][20 * 5 + p]);
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(DynamicAutodiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) {
// 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);
DynamicAutoDiffCostFunction<MyCostFunctor, 3> 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] = &param_block_0[0];
parameter_blocks[1] = &param_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_EQ(-1.0, jacobian_vect[0][2*p * 10 + p]);
// Check "B" Jacobian.
EXPECT_EQ(+1.0, jacobian_vect[0][(2*p+1) * 10 + p]);
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_EQ(4 * p - 8, jacobian_vect[0][20 * 10 + p]);
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]);
}
}
} // namespace internal
} // namespace ceres