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

Refactored DynamicNumericDiffCostFunction to use NumericDiff

Browse Source 
Change-Id: I2fc4b203e984beaa7af96fb3cbe8ce14e5bca614
This commit is contained in:
Tal Ben-Nun
2015-05-06 19:38:10 +03:00
parent 4707eb22e9
commit b2dcef36e7
4 changed files with 46 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/internal/numeric_diff.h
+20 -8
View File
@@ -91,6 +91,8 @@ struct NumericDiff {
double const* residuals_at_eval_point,
const double relative_step_size,
int num_residuals,
int parameter_block_index,
int parameter_block_size,
double **parameters,
double *jacobian) {
using Eigen::Map;
@@ -98,8 +100,14 @@ struct NumericDiff {
using Eigen::RowMajor;
using Eigen::ColMajor;
const int NUM_RESIDUALS =
const int num_residuals_internal =
(kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals);
const int parameter_block_index_internal =
(kParameterBlock != ceres::DYNAMIC ? kParameterBlock :
parameter_block_index);
const int parameter_block_size_internal =
(kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize :
parameter_block_size);
typedef Matrix<double, kNumResiduals, 1> ResidualVector;
typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
@@ -115,12 +123,13 @@ struct NumericDiff {
JacobianMatrix;
Map<JacobianMatrix> parameter_jacobian(jacobian,
NUM_RESIDUALS,
kParameterBlockSize);
num_residuals_internal,
parameter_block_size_internal);
// Mutate 1 element at a time and then restore.
Map<ParameterVector> x_plus_delta(parameters[kParameterBlock],
kParameterBlockSize);
Map<ParameterVector> x_plus_delta(
parameters[parameter_block_index_internal],
parameter_block_size_internal);
ParameterVector x(x_plus_delta);
ParameterVector step_size = x.array().abs() * relative_step_size;
@@ -136,8 +145,8 @@ struct NumericDiff {
// For each parameter in the parameter block, use finite differences to
// compute the derivative for that parameter.
ResidualVector residuals(NUM_RESIDUALS);
for (int j = 0; j < kParameterBlockSize; ++j) {
ResidualVector residuals(num_residuals_internal);
for (int j = 0; j < parameter_block_size_internal; ++j) {
const double delta =
(step_size(j) == 0.0) ? fallback_step_size : step_size(j);
@@ -169,7 +178,8 @@ struct NumericDiff {
} else {
// Forward difference only; reuse existing residuals evaluation.
parameter_jacobian.col(j) -=
Map<const ResidualVector>(residuals_at_eval_point, NUM_RESIDUALS);
Map<const ResidualVector>(residuals_at_eval_point,
num_residuals_internal);
}
x_plus_delta(j) = x(j); // Restore x_plus_delta.
@@ -195,6 +205,8 @@ struct NumericDiff<CostFunctor, kMethod, kNumResiduals,
double const* residuals_at_eval_point,
const double relative_step_size,
const int num_residuals,
const int parameter_block_index,
const int parameter_block_size,
double **parameters,
double *jacobian) {
LOG(FATAL) << "Control should never reach here.";