diff options
Diffstat (limited to 'extern/ceres/internal/ceres/residual_block.cc')
-rw-r--r-- | extern/ceres/internal/ceres/residual_block.cc | 48 |
1 files changed, 23 insertions, 25 deletions
diff --git a/extern/ceres/internal/ceres/residual_block.cc b/extern/ceres/internal/ceres/residual_block.cc index 9a123cf132e..0bf30bcf446 100644 --- a/extern/ceres/internal/ceres/residual_block.cc +++ b/extern/ceres/internal/ceres/residual_block.cc @@ -35,13 +35,13 @@ #include <cstddef> #include <vector> #include "ceres/corrector.h" -#include "ceres/parameter_block.h" -#include "ceres/residual_block_utils.h" #include "ceres/cost_function.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" #include "ceres/local_parameterization.h" #include "ceres/loss_function.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block_utils.h" #include "ceres/small_blas.h" using Eigen::Dynamic; @@ -50,16 +50,14 @@ namespace ceres { namespace internal { ResidualBlock::ResidualBlock( - const CostFunction* cost_function, - const LossFunction* loss_function, - const std::vector<ParameterBlock*>& parameter_blocks, - int index) + const CostFunction* cost_function, const LossFunction* loss_function, + const std::vector<ParameterBlock*>& parameter_blocks, int index) : cost_function_(cost_function), loss_function_(loss_function), parameter_blocks_( - new ParameterBlock* [ - cost_function->parameter_block_sizes().size()]), + new ParameterBlock*[cost_function->parameter_block_sizes().size()]), index_(index) { + CHECK(cost_function_ != nullptr); std::copy(parameter_blocks.begin(), parameter_blocks.end(), parameter_blocks_.get()); @@ -82,11 +80,11 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, // Put pointers into the scratch space into global_jacobians as appropriate. FixedArray<double*, 8> global_jacobians(num_parameter_blocks); - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; - if (jacobians[i] != NULL && - parameter_block->LocalParameterizationJacobian() != NULL) { + if (jacobians[i] != nullptr && + parameter_block->LocalParameterizationJacobian() != nullptr) { global_jacobians[i] = scratch; scratch += num_residuals * parameter_block->Size(); } else { @@ -96,7 +94,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, } // If the caller didn't request residuals, use the scratch space for them. - bool outputting_residuals = (residuals != NULL); + bool outputting_residuals = (residuals != nullptr); if (!outputting_residuals) { residuals = scratch; } @@ -104,16 +102,17 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, // Invalidate the evaluation buffers so that we can check them after // the CostFunction::Evaluate call, to see if all the return values // that were required were written to and that they are finite. - double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL; + double** eval_jacobians = + (jacobians != nullptr) ? global_jacobians.data() : nullptr; InvalidateEvaluation(*this, cost, residuals, eval_jacobians); - if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians)) { + if (!cost_function_->Evaluate(parameters.data(), residuals, eval_jacobians)) { return false; } if (!IsEvaluationValid(*this, - parameters.get(), + parameters.data(), cost, residuals, eval_jacobians)) { @@ -124,7 +123,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n" // NOLINT "generated during the or jacobian computation. \n\n" + EvaluationToString(*this, - parameters.get(), + parameters.data(), cost, residuals, eval_jacobians); @@ -135,13 +134,13 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); // Update the jacobians with the local parameterizations. - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { - if (jacobians[i] != NULL) { + if (jacobians[i] != nullptr) { const ParameterBlock* parameter_block = parameter_blocks_[i]; // Apply local reparameterization to the jacobians. - if (parameter_block->LocalParameterizationJacobian() != NULL) { + if (parameter_block->LocalParameterizationJacobian() != nullptr) { // jacobians[i] = global_jacobians[i] * global_to_local_jacobian. MatrixMatrixMultiply<Dynamic, Dynamic, Dynamic, Dynamic, 0>( global_jacobians[i], @@ -156,7 +155,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, } } - if (loss_function_ == NULL || !apply_loss_function) { + if (loss_function_ == nullptr || !apply_loss_function) { *cost = 0.5 * squared_norm; return true; } @@ -167,16 +166,16 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, // No jacobians and not outputting residuals? All done. Doing an early exit // here avoids constructing the "Corrector" object below in a common case. - if (jacobians == NULL && !outputting_residuals) { + if (jacobians == nullptr && !outputting_residuals) { return true; } // Correct for the effects of the loss function. The jacobians need to be // corrected before the residuals, since they use the uncorrected residuals. Corrector correct(squared_norm, rho); - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { - if (jacobians[i] != NULL) { + if (jacobians[i] != nullptr) { const ParameterBlock* parameter_block = parameter_blocks_[i]; // Correct the jacobians for the loss function. @@ -206,8 +205,7 @@ int ResidualBlock::NumScratchDoublesForEvaluate() const { int scratch_doubles = 1; for (int i = 0; i < num_parameters; ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; - if (!parameter_block->IsConstant() && - parameter_block->LocalParameterizationJacobian() != NULL) { + if (parameter_block->LocalParameterizationJacobian() != nullptr) { scratch_doubles += parameter_block->Size(); } } |