946 lines
36 KiB
C++
946 lines
36 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: sameeragarwal@google.com (Sameer Agarwal)
|
||
|
|
// mierle@gmail.com (Keir Mierle)
|
||
|
|
|
||
|
|
#include "ceres/problem_impl.h"
|
||
|
|
|
||
|
|
#include <algorithm>
|
||
|
|
#include <cstddef>
|
||
|
|
#include <iterator>
|
||
|
|
#include <set>
|
||
|
|
#include <string>
|
||
|
|
#include <utility>
|
||
|
|
#include <vector>
|
||
|
|
#include "ceres/casts.h"
|
||
|
|
#include "ceres/compressed_row_sparse_matrix.h"
|
||
|
|
#include "ceres/cost_function.h"
|
||
|
|
#include "ceres/crs_matrix.h"
|
||
|
|
#include "ceres/evaluator.h"
|
||
|
|
#include "ceres/loss_function.h"
|
||
|
|
#include "ceres/map_util.h"
|
||
|
|
#include "ceres/parameter_block.h"
|
||
|
|
#include "ceres/program.h"
|
||
|
|
#include "ceres/residual_block.h"
|
||
|
|
#include "ceres/stl_util.h"
|
||
|
|
#include "ceres/stringprintf.h"
|
||
|
|
#include "glog/logging.h"
|
||
|
|
|
||
|
|
namespace ceres {
|
||
|
|
namespace internal {
|
||
|
|
|
||
|
|
using std::map;
|
||
|
|
using std::string;
|
||
|
|
using std::vector;
|
||
|
|
typedef std::map<double*, internal::ParameterBlock*> ParameterMap;
|
||
|
|
|
||
|
|
namespace {
|
||
|
|
// Returns true if two regions of memory, a and b, with sizes size_a and size_b
|
||
|
|
// respectively, overlap.
|
||
|
|
bool RegionsAlias(const double* a, int size_a,
|
||
|
|
const double* b, int size_b) {
|
||
|
|
return (a < b) ? b < (a + size_a)
|
||
|
|
: a < (b + size_b);
|
||
|
|
}
|
||
|
|
|
||
|
|
void CheckForNoAliasing(double* existing_block,
|
||
|
|
int existing_block_size,
|
||
|
|
double* new_block,
|
||
|
|
int new_block_size) {
|
||
|
|
CHECK(!RegionsAlias(existing_block, existing_block_size,
|
||
|
|
new_block, new_block_size))
|
||
|
|
<< "Aliasing detected between existing parameter block at memory "
|
||
|
|
<< "location " << existing_block
|
||
|
|
<< " and has size " << existing_block_size << " with new parameter "
|
||
|
|
<< "block that has memory address " << new_block << " and would have "
|
||
|
|
<< "size " << new_block_size << ".";
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace
|
||
|
|
|
||
|
|
ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values,
|
||
|
|
int size) {
|
||
|
|
CHECK(values != NULL) << "Null pointer passed to AddParameterBlock "
|
||
|
|
<< "for a parameter with size " << size;
|
||
|
|
|
||
|
|
// Ignore the request if there is a block for the given pointer already.
|
||
|
|
ParameterMap::iterator it = parameter_block_map_.find(values);
|
||
|
|
if (it != parameter_block_map_.end()) {
|
||
|
|
if (!options_.disable_all_safety_checks) {
|
||
|
|
int existing_size = it->second->Size();
|
||
|
|
CHECK(size == existing_size)
|
||
|
|
<< "Tried adding a parameter block with the same double pointer, "
|
||
|
|
<< values << ", twice, but with different block sizes. Original "
|
||
|
|
<< "size was " << existing_size << " but new size is "
|
||
|
|
<< size;
|
||
|
|
}
|
||
|
|
return it->second;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!options_.disable_all_safety_checks) {
|
||
|
|
// Before adding the parameter block, also check that it doesn't alias any
|
||
|
|
// other parameter blocks.
|
||
|
|
if (!parameter_block_map_.empty()) {
|
||
|
|
ParameterMap::iterator lb = parameter_block_map_.lower_bound(values);
|
||
|
|
|
||
|
|
// If lb is not the first block, check the previous block for aliasing.
|
||
|
|
if (lb != parameter_block_map_.begin()) {
|
||
|
|
ParameterMap::iterator previous = lb;
|
||
|
|
--previous;
|
||
|
|
CheckForNoAliasing(previous->first,
|
||
|
|
previous->second->Size(),
|
||
|
|
values,
|
||
|
|
size);
|
||
|
|
}
|
||
|
|
|
||
|
|
// If lb is not off the end, check lb for aliasing.
|
||
|
|
if (lb != parameter_block_map_.end()) {
|
||
|
|
CheckForNoAliasing(lb->first,
|
||
|
|
lb->second->Size(),
|
||
|
|
values,
|
||
|
|
size);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Pass the index of the new parameter block as well to keep the index in
|
||
|
|
// sync with the position of the parameter in the program's parameter vector.
|
||
|
|
ParameterBlock* new_parameter_block =
|
||
|
|
new ParameterBlock(values, size, program_->parameter_blocks_.size());
|
||
|
|
|
||
|
|
// For dynamic problems, add the list of dependent residual blocks, which is
|
||
|
|
// empty to start.
|
||
|
|
if (options_.enable_fast_removal) {
|
||
|
|
new_parameter_block->EnableResidualBlockDependencies();
|
||
|
|
}
|
||
|
|
parameter_block_map_[values] = new_parameter_block;
|
||
|
|
program_->parameter_blocks_.push_back(new_parameter_block);
|
||
|
|
return new_parameter_block;
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) {
|
||
|
|
CHECK_NOTNULL(residual_block);
|
||
|
|
// Perform no check on the validity of residual_block, that is handled in
|
||
|
|
// the public method: RemoveResidualBlock().
|
||
|
|
|
||
|
|
// If needed, remove the parameter dependencies on this residual block.
|
||
|
|
if (options_.enable_fast_removal) {
|
||
|
|
const int num_parameter_blocks_for_residual =
|
||
|
|
residual_block->NumParameterBlocks();
|
||
|
|
for (int i = 0; i < num_parameter_blocks_for_residual; ++i) {
|
||
|
|
residual_block->parameter_blocks()[i]
|
||
|
|
->RemoveResidualBlock(residual_block);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlockSet::iterator it = residual_block_set_.find(residual_block);
|
||
|
|
residual_block_set_.erase(it);
|
||
|
|
}
|
||
|
|
DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Deletes the residual block in question, assuming there are no other
|
||
|
|
// references to it inside the problem (e.g. by another parameter). Referenced
|
||
|
|
// cost and loss functions are tucked away for future deletion, since it is not
|
||
|
|
// possible to know whether other parts of the problem depend on them without
|
||
|
|
// doing a full scan.
|
||
|
|
void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) {
|
||
|
|
// The const casts here are legit, since ResidualBlock holds these
|
||
|
|
// pointers as const pointers but we have ownership of them and
|
||
|
|
// have the right to destroy them when the destructor is called.
|
||
|
|
if (options_.cost_function_ownership == TAKE_OWNERSHIP &&
|
||
|
|
residual_block->cost_function() != NULL) {
|
||
|
|
cost_functions_to_delete_.push_back(
|
||
|
|
const_cast<CostFunction*>(residual_block->cost_function()));
|
||
|
|
}
|
||
|
|
if (options_.loss_function_ownership == TAKE_OWNERSHIP &&
|
||
|
|
residual_block->loss_function() != NULL) {
|
||
|
|
loss_functions_to_delete_.push_back(
|
||
|
|
const_cast<LossFunction*>(residual_block->loss_function()));
|
||
|
|
}
|
||
|
|
delete residual_block;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Deletes the parameter block in question, assuming there are no other
|
||
|
|
// references to it inside the problem (e.g. by any residual blocks).
|
||
|
|
// Referenced parameterizations are tucked away for future deletion, since it
|
||
|
|
// is not possible to know whether other parts of the problem depend on them
|
||
|
|
// without doing a full scan.
|
||
|
|
void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) {
|
||
|
|
if (options_.local_parameterization_ownership == TAKE_OWNERSHIP &&
|
||
|
|
parameter_block->local_parameterization() != NULL) {
|
||
|
|
local_parameterizations_to_delete_.push_back(
|
||
|
|
parameter_block->mutable_local_parameterization());
|
||
|
|
}
|
||
|
|
parameter_block_map_.erase(parameter_block->mutable_user_state());
|
||
|
|
delete parameter_block;
|
||
|
|
}
|
||
|
|
|
||
|
|
ProblemImpl::ProblemImpl() : program_(new internal::Program) {}
|
||
|
|
ProblemImpl::ProblemImpl(const Problem::Options& options)
|
||
|
|
: options_(options),
|
||
|
|
program_(new internal::Program) {}
|
||
|
|
|
||
|
|
ProblemImpl::~ProblemImpl() {
|
||
|
|
// Collect the unique cost/loss functions and delete the residuals.
|
||
|
|
const int num_residual_blocks = program_->residual_blocks_.size();
|
||
|
|
cost_functions_to_delete_.reserve(num_residual_blocks);
|
||
|
|
loss_functions_to_delete_.reserve(num_residual_blocks);
|
||
|
|
for (int i = 0; i < program_->residual_blocks_.size(); ++i) {
|
||
|
|
DeleteBlock(program_->residual_blocks_[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Collect the unique parameterizations and delete the parameters.
|
||
|
|
for (int i = 0; i < program_->parameter_blocks_.size(); ++i) {
|
||
|
|
DeleteBlock(program_->parameter_blocks_[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Delete the owned cost/loss functions and parameterizations.
|
||
|
|
STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(),
|
||
|
|
local_parameterizations_to_delete_.end());
|
||
|
|
STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(),
|
||
|
|
cost_functions_to_delete_.end());
|
||
|
|
STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(),
|
||
|
|
loss_functions_to_delete_.end());
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
const vector<double*>& parameter_blocks) {
|
||
|
|
CHECK_NOTNULL(cost_function);
|
||
|
|
CHECK_EQ(parameter_blocks.size(),
|
||
|
|
cost_function->parameter_block_sizes().size());
|
||
|
|
|
||
|
|
// Check the sizes match.
|
||
|
|
const vector<int32>& parameter_block_sizes =
|
||
|
|
cost_function->parameter_block_sizes();
|
||
|
|
|
||
|
|
if (!options_.disable_all_safety_checks) {
|
||
|
|
CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size())
|
||
|
|
<< "Number of blocks input is different than the number of blocks "
|
||
|
|
<< "that the cost function expects.";
|
||
|
|
|
||
|
|
// Check for duplicate parameter blocks.
|
||
|
|
vector<double*> sorted_parameter_blocks(parameter_blocks);
|
||
|
|
sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end());
|
||
|
|
vector<double*>::const_iterator duplicate_items =
|
||
|
|
unique(sorted_parameter_blocks.begin(),
|
||
|
|
sorted_parameter_blocks.end());
|
||
|
|
if (duplicate_items != sorted_parameter_blocks.end()) {
|
||
|
|
string blocks;
|
||
|
|
for (int i = 0; i < parameter_blocks.size(); ++i) {
|
||
|
|
blocks += StringPrintf(" %p ", parameter_blocks[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
LOG(FATAL) << "Duplicate parameter blocks in a residual parameter "
|
||
|
|
<< "are not allowed. Parameter block pointers: ["
|
||
|
|
<< blocks << "]";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Add parameter blocks and convert the double*'s to parameter blocks.
|
||
|
|
vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size());
|
||
|
|
for (int i = 0; i < parameter_blocks.size(); ++i) {
|
||
|
|
parameter_block_ptrs[i] =
|
||
|
|
InternalAddParameterBlock(parameter_blocks[i],
|
||
|
|
parameter_block_sizes[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!options_.disable_all_safety_checks) {
|
||
|
|
// Check that the block sizes match the block sizes expected by the
|
||
|
|
// cost_function.
|
||
|
|
for (int i = 0; i < parameter_block_ptrs.size(); ++i) {
|
||
|
|
CHECK_EQ(cost_function->parameter_block_sizes()[i],
|
||
|
|
parameter_block_ptrs[i]->Size())
|
||
|
|
<< "The cost function expects parameter block " << i
|
||
|
|
<< " of size " << cost_function->parameter_block_sizes()[i]
|
||
|
|
<< " but was given a block of size "
|
||
|
|
<< parameter_block_ptrs[i]->Size();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* new_residual_block =
|
||
|
|
new ResidualBlock(cost_function,
|
||
|
|
loss_function,
|
||
|
|
parameter_block_ptrs,
|
||
|
|
program_->residual_blocks_.size());
|
||
|
|
|
||
|
|
// Add dependencies on the residual to the parameter blocks.
|
||
|
|
if (options_.enable_fast_removal) {
|
||
|
|
for (int i = 0; i < parameter_blocks.size(); ++i) {
|
||
|
|
parameter_block_ptrs[i]->AddResidualBlock(new_residual_block);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
program_->residual_blocks_.push_back(new_residual_block);
|
||
|
|
|
||
|
|
if (options_.enable_fast_removal) {
|
||
|
|
residual_block_set_.insert(new_residual_block);
|
||
|
|
}
|
||
|
|
|
||
|
|
return new_residual_block;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Unfortunately, macros don't help much to reduce this code, and var args don't
|
||
|
|
// work because of the ambiguous case that there is no loss function.
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2, double* x3) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
residual_parameters.push_back(x3);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2, double* x3, double* x4) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
residual_parameters.push_back(x3);
|
||
|
|
residual_parameters.push_back(x4);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
residual_parameters.push_back(x3);
|
||
|
|
residual_parameters.push_back(x4);
|
||
|
|
residual_parameters.push_back(x5);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
|
||
|
|
double* x6) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
residual_parameters.push_back(x3);
|
||
|
|
residual_parameters.push_back(x4);
|
||
|
|
residual_parameters.push_back(x5);
|
||
|
|
residual_parameters.push_back(x6);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
|
||
|
|
double* x6, double* x7) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
residual_parameters.push_back(x3);
|
||
|
|
residual_parameters.push_back(x4);
|
||
|
|
residual_parameters.push_back(x5);
|
||
|
|
residual_parameters.push_back(x6);
|
||
|
|
residual_parameters.push_back(x7);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
|
||
|
|
double* x6, double* x7, double* x8) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
residual_parameters.push_back(x3);
|
||
|
|
residual_parameters.push_back(x4);
|
||
|
|
residual_parameters.push_back(x5);
|
||
|
|
residual_parameters.push_back(x6);
|
||
|
|
residual_parameters.push_back(x7);
|
||
|
|
residual_parameters.push_back(x8);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
ResidualBlock* ProblemImpl::AddResidualBlock(
|
||
|
|
CostFunction* cost_function,
|
||
|
|
LossFunction* loss_function,
|
||
|
|
double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
|
||
|
|
double* x6, double* x7, double* x8, double* x9) {
|
||
|
|
vector<double*> residual_parameters;
|
||
|
|
residual_parameters.push_back(x0);
|
||
|
|
residual_parameters.push_back(x1);
|
||
|
|
residual_parameters.push_back(x2);
|
||
|
|
residual_parameters.push_back(x3);
|
||
|
|
residual_parameters.push_back(x4);
|
||
|
|
residual_parameters.push_back(x5);
|
||
|
|
residual_parameters.push_back(x6);
|
||
|
|
residual_parameters.push_back(x7);
|
||
|
|
residual_parameters.push_back(x8);
|
||
|
|
residual_parameters.push_back(x9);
|
||
|
|
return AddResidualBlock(cost_function, loss_function, residual_parameters);
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::AddParameterBlock(double* values, int size) {
|
||
|
|
InternalAddParameterBlock(values, size);
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::AddParameterBlock(
|
||
|
|
double* values,
|
||
|
|
int size,
|
||
|
|
LocalParameterization* local_parameterization) {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
InternalAddParameterBlock(values, size);
|
||
|
|
if (local_parameterization != NULL) {
|
||
|
|
parameter_block->SetParameterization(local_parameterization);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Delete a block from a vector of blocks, maintaining the indexing invariant.
|
||
|
|
// This is done in constant time by moving an element from the end of the
|
||
|
|
// vector over the element to remove, then popping the last element. It
|
||
|
|
// destroys the ordering in the interest of speed.
|
||
|
|
template<typename Block>
|
||
|
|
void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks,
|
||
|
|
Block* block_to_remove) {
|
||
|
|
CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove)
|
||
|
|
<< "You found a Ceres bug! \n"
|
||
|
|
<< "Block requested: "
|
||
|
|
<< block_to_remove->ToString() << "\n"
|
||
|
|
<< "Block present: "
|
||
|
|
<< (*mutable_blocks)[block_to_remove->index()]->ToString();
|
||
|
|
|
||
|
|
// Prepare the to-be-moved block for the new, lower-in-index position by
|
||
|
|
// setting the index to the blocks final location.
|
||
|
|
Block* tmp = mutable_blocks->back();
|
||
|
|
tmp->set_index(block_to_remove->index());
|
||
|
|
|
||
|
|
// Overwrite the to-be-deleted residual block with the one at the end.
|
||
|
|
(*mutable_blocks)[block_to_remove->index()] = tmp;
|
||
|
|
|
||
|
|
DeleteBlock(block_to_remove);
|
||
|
|
|
||
|
|
// The block is gone so shrink the vector of blocks accordingly.
|
||
|
|
mutable_blocks->pop_back();
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) {
|
||
|
|
CHECK_NOTNULL(residual_block);
|
||
|
|
|
||
|
|
// Verify that residual_block identifies a residual in the current problem.
|
||
|
|
const string residual_not_found_message =
|
||
|
|
StringPrintf("Residual block to remove: %p not found. This usually means "
|
||
|
|
"one of three things have happened:\n"
|
||
|
|
" 1) residual_block is uninitialised and points to a random "
|
||
|
|
"area in memory.\n"
|
||
|
|
" 2) residual_block represented a residual that was added to"
|
||
|
|
" the problem, but referred to a parameter block which has "
|
||
|
|
"since been removed, which removes all residuals which "
|
||
|
|
"depend on that parameter block, and was thus removed.\n"
|
||
|
|
" 3) residual_block referred to a residual that has already "
|
||
|
|
"been removed from the problem (by the user).",
|
||
|
|
residual_block);
|
||
|
|
if (options_.enable_fast_removal) {
|
||
|
|
CHECK(residual_block_set_.find(residual_block) !=
|
||
|
|
residual_block_set_.end())
|
||
|
|
<< residual_not_found_message;
|
||
|
|
} else {
|
||
|
|
// Perform a full search over all current residuals.
|
||
|
|
CHECK(std::find(program_->residual_blocks().begin(),
|
||
|
|
program_->residual_blocks().end(),
|
||
|
|
residual_block) != program_->residual_blocks().end())
|
||
|
|
<< residual_not_found_message;
|
||
|
|
}
|
||
|
|
|
||
|
|
InternalRemoveResidualBlock(residual_block);
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::RemoveParameterBlock(double* values) {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, values, NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "it can be removed.";
|
||
|
|
}
|
||
|
|
|
||
|
|
if (options_.enable_fast_removal) {
|
||
|
|
// Copy the dependent residuals from the parameter block because the set of
|
||
|
|
// dependents will change after each call to RemoveResidualBlock().
|
||
|
|
vector<ResidualBlock*> residual_blocks_to_remove(
|
||
|
|
parameter_block->mutable_residual_blocks()->begin(),
|
||
|
|
parameter_block->mutable_residual_blocks()->end());
|
||
|
|
for (int i = 0; i < residual_blocks_to_remove.size(); ++i) {
|
||
|
|
InternalRemoveResidualBlock(residual_blocks_to_remove[i]);
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
// Scan all the residual blocks to remove ones that depend on the parameter
|
||
|
|
// block. Do the scan backwards since the vector changes while iterating.
|
||
|
|
const int num_residual_blocks = NumResidualBlocks();
|
||
|
|
for (int i = num_residual_blocks - 1; i >= 0; --i) {
|
||
|
|
ResidualBlock* residual_block =
|
||
|
|
(*(program_->mutable_residual_blocks()))[i];
|
||
|
|
const int num_parameter_blocks = residual_block->NumParameterBlocks();
|
||
|
|
for (int j = 0; j < num_parameter_blocks; ++j) {
|
||
|
|
if (residual_block->parameter_blocks()[j] == parameter_block) {
|
||
|
|
InternalRemoveResidualBlock(residual_block);
|
||
|
|
// The parameter blocks are guaranteed unique.
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block);
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::SetParameterBlockConstant(double* values) {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, values, NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "it can be set constant.";
|
||
|
|
}
|
||
|
|
|
||
|
|
parameter_block->SetConstant();
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::SetParameterBlockVariable(double* values) {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, values, NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "it can be set varying.";
|
||
|
|
}
|
||
|
|
|
||
|
|
parameter_block->SetVarying();
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::SetParameterization(
|
||
|
|
double* values,
|
||
|
|
LocalParameterization* local_parameterization) {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, values, NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "you can set its local parameterization.";
|
||
|
|
}
|
||
|
|
|
||
|
|
parameter_block->SetParameterization(local_parameterization);
|
||
|
|
}
|
||
|
|
|
||
|
|
const LocalParameterization* ProblemImpl::GetParameterization(
|
||
|
|
double* values) const {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, values, NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "you can get its local parameterization.";
|
||
|
|
}
|
||
|
|
|
||
|
|
return parameter_block->local_parameterization();
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::SetParameterLowerBound(double* values,
|
||
|
|
int index,
|
||
|
|
double lower_bound) {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, values, NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "you can set a lower bound on one of its components.";
|
||
|
|
}
|
||
|
|
|
||
|
|
parameter_block->SetLowerBound(index, lower_bound);
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::SetParameterUpperBound(double* values,
|
||
|
|
int index,
|
||
|
|
double upper_bound) {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, values, NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "you can set an upper bound on one of its components.";
|
||
|
|
}
|
||
|
|
parameter_block->SetUpperBound(index, upper_bound);
|
||
|
|
}
|
||
|
|
|
||
|
|
bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
|
||
|
|
double* cost,
|
||
|
|
vector<double>* residuals,
|
||
|
|
vector<double>* gradient,
|
||
|
|
CRSMatrix* jacobian) {
|
||
|
|
if (cost == NULL &&
|
||
|
|
residuals == NULL &&
|
||
|
|
gradient == NULL &&
|
||
|
|
jacobian == NULL) {
|
||
|
|
LOG(INFO) << "Nothing to do.";
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
// If the user supplied residual blocks, then use them, otherwise
|
||
|
|
// take the residual blocks from the underlying program.
|
||
|
|
Program program;
|
||
|
|
*program.mutable_residual_blocks() =
|
||
|
|
((evaluate_options.residual_blocks.size() > 0)
|
||
|
|
? evaluate_options.residual_blocks : program_->residual_blocks());
|
||
|
|
|
||
|
|
const vector<double*>& parameter_block_ptrs =
|
||
|
|
evaluate_options.parameter_blocks;
|
||
|
|
|
||
|
|
vector<ParameterBlock*> variable_parameter_blocks;
|
||
|
|
vector<ParameterBlock*>& parameter_blocks =
|
||
|
|
*program.mutable_parameter_blocks();
|
||
|
|
|
||
|
|
if (parameter_block_ptrs.size() == 0) {
|
||
|
|
// The user did not provide any parameter blocks, so default to
|
||
|
|
// using all the parameter blocks in the order that they are in
|
||
|
|
// the underlying program object.
|
||
|
|
parameter_blocks = program_->parameter_blocks();
|
||
|
|
} else {
|
||
|
|
// The user supplied a vector of parameter blocks. Using this list
|
||
|
|
// requires a number of steps.
|
||
|
|
|
||
|
|
// 1. Convert double* into ParameterBlock*
|
||
|
|
parameter_blocks.resize(parameter_block_ptrs.size());
|
||
|
|
for (int i = 0; i < parameter_block_ptrs.size(); ++i) {
|
||
|
|
parameter_blocks[i] = FindWithDefault(parameter_block_map_,
|
||
|
|
parameter_block_ptrs[i],
|
||
|
|
NULL);
|
||
|
|
if (parameter_blocks[i] == NULL) {
|
||
|
|
LOG(FATAL) << "No known parameter block for "
|
||
|
|
<< "Problem::Evaluate::Options.parameter_blocks[" << i << "]"
|
||
|
|
<< " = " << parameter_block_ptrs[i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// 2. The user may have only supplied a subset of parameter
|
||
|
|
// blocks, so identify the ones that are not supplied by the user
|
||
|
|
// and are NOT constant. These parameter blocks are stored in
|
||
|
|
// variable_parameter_blocks.
|
||
|
|
//
|
||
|
|
// To ensure that the parameter blocks are not included in the
|
||
|
|
// columns of the jacobian, we need to make sure that they are
|
||
|
|
// constant during evaluation and then make them variable again
|
||
|
|
// after we are done.
|
||
|
|
vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks());
|
||
|
|
vector<ParameterBlock*> included_parameter_blocks(
|
||
|
|
program.parameter_blocks());
|
||
|
|
|
||
|
|
vector<ParameterBlock*> excluded_parameter_blocks;
|
||
|
|
sort(all_parameter_blocks.begin(), all_parameter_blocks.end());
|
||
|
|
sort(included_parameter_blocks.begin(), included_parameter_blocks.end());
|
||
|
|
set_difference(all_parameter_blocks.begin(),
|
||
|
|
all_parameter_blocks.end(),
|
||
|
|
included_parameter_blocks.begin(),
|
||
|
|
included_parameter_blocks.end(),
|
||
|
|
back_inserter(excluded_parameter_blocks));
|
||
|
|
|
||
|
|
variable_parameter_blocks.reserve(excluded_parameter_blocks.size());
|
||
|
|
for (int i = 0; i < excluded_parameter_blocks.size(); ++i) {
|
||
|
|
ParameterBlock* parameter_block = excluded_parameter_blocks[i];
|
||
|
|
if (!parameter_block->IsConstant()) {
|
||
|
|
variable_parameter_blocks.push_back(parameter_block);
|
||
|
|
parameter_block->SetConstant();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Setup the Parameter indices and offsets before an evaluator can
|
||
|
|
// be constructed and used.
|
||
|
|
program.SetParameterOffsetsAndIndex();
|
||
|
|
|
||
|
|
Evaluator::Options evaluator_options;
|
||
|
|
|
||
|
|
// Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or
|
||
|
|
// CXSparse, here it just being used for telling the evaluator to
|
||
|
|
// use a SparseRowCompressedMatrix for the jacobian. This is because
|
||
|
|
// the Evaluator decides the storage for the Jacobian based on the
|
||
|
|
// type of linear solver being used.
|
||
|
|
evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
|
||
|
|
#ifndef CERES_USE_OPENMP
|
||
|
|
LOG_IF(WARNING, evaluate_options.num_threads > 1)
|
||
|
|
<< "OpenMP support is not compiled into this binary; "
|
||
|
|
<< "only evaluate_options.num_threads = 1 is supported. Switching "
|
||
|
|
<< "to single threaded mode.";
|
||
|
|
evaluator_options.num_threads = 1;
|
||
|
|
#else
|
||
|
|
evaluator_options.num_threads = evaluate_options.num_threads;
|
||
|
|
#endif // CERES_USE_OPENMP
|
||
|
|
|
||
|
|
string error;
|
||
|
|
scoped_ptr<Evaluator> evaluator(
|
||
|
|
Evaluator::Create(evaluator_options, &program, &error));
|
||
|
|
if (evaluator.get() == NULL) {
|
||
|
|
LOG(ERROR) << "Unable to create an Evaluator object. "
|
||
|
|
<< "Error: " << error
|
||
|
|
<< "This is a Ceres bug; please contact the developers!";
|
||
|
|
|
||
|
|
// Make the parameter blocks that were temporarily marked
|
||
|
|
// constant, variable again.
|
||
|
|
for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
|
||
|
|
variable_parameter_blocks[i]->SetVarying();
|
||
|
|
}
|
||
|
|
|
||
|
|
program_->SetParameterBlockStatePtrsToUserStatePtrs();
|
||
|
|
program_->SetParameterOffsetsAndIndex();
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (residuals !=NULL) {
|
||
|
|
residuals->resize(evaluator->NumResiduals());
|
||
|
|
}
|
||
|
|
|
||
|
|
if (gradient != NULL) {
|
||
|
|
gradient->resize(evaluator->NumEffectiveParameters());
|
||
|
|
}
|
||
|
|
|
||
|
|
scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian;
|
||
|
|
if (jacobian != NULL) {
|
||
|
|
tmp_jacobian.reset(
|
||
|
|
down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian()));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Point the state pointers to the user state pointers. This is
|
||
|
|
// needed so that we can extract a parameter vector which is then
|
||
|
|
// passed to Evaluator::Evaluate.
|
||
|
|
program.SetParameterBlockStatePtrsToUserStatePtrs();
|
||
|
|
|
||
|
|
// Copy the value of the parameter blocks into a vector, since the
|
||
|
|
// Evaluate::Evaluate method needs its input as such. The previous
|
||
|
|
// call to SetParameterBlockStatePtrsToUserStatePtrs ensures that
|
||
|
|
// these values are the ones corresponding to the actual state of
|
||
|
|
// the parameter blocks, rather than the temporary state pointer
|
||
|
|
// used for evaluation.
|
||
|
|
Vector parameters(program.NumParameters());
|
||
|
|
program.ParameterBlocksToStateVector(parameters.data());
|
||
|
|
|
||
|
|
double tmp_cost = 0;
|
||
|
|
|
||
|
|
Evaluator::EvaluateOptions evaluator_evaluate_options;
|
||
|
|
evaluator_evaluate_options.apply_loss_function =
|
||
|
|
evaluate_options.apply_loss_function;
|
||
|
|
bool status = evaluator->Evaluate(evaluator_evaluate_options,
|
||
|
|
parameters.data(),
|
||
|
|
&tmp_cost,
|
||
|
|
residuals != NULL ? &(*residuals)[0] : NULL,
|
||
|
|
gradient != NULL ? &(*gradient)[0] : NULL,
|
||
|
|
tmp_jacobian.get());
|
||
|
|
|
||
|
|
// Make the parameter blocks that were temporarily marked constant,
|
||
|
|
// variable again.
|
||
|
|
for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
|
||
|
|
variable_parameter_blocks[i]->SetVarying();
|
||
|
|
}
|
||
|
|
|
||
|
|
if (status) {
|
||
|
|
if (cost != NULL) {
|
||
|
|
*cost = tmp_cost;
|
||
|
|
}
|
||
|
|
if (jacobian != NULL) {
|
||
|
|
tmp_jacobian->ToCRSMatrix(jacobian);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
program_->SetParameterBlockStatePtrsToUserStatePtrs();
|
||
|
|
program_->SetParameterOffsetsAndIndex();
|
||
|
|
return status;
|
||
|
|
}
|
||
|
|
|
||
|
|
int ProblemImpl::NumParameterBlocks() const {
|
||
|
|
return program_->NumParameterBlocks();
|
||
|
|
}
|
||
|
|
|
||
|
|
int ProblemImpl::NumParameters() const {
|
||
|
|
return program_->NumParameters();
|
||
|
|
}
|
||
|
|
|
||
|
|
int ProblemImpl::NumResidualBlocks() const {
|
||
|
|
return program_->NumResidualBlocks();
|
||
|
|
}
|
||
|
|
|
||
|
|
int ProblemImpl::NumResiduals() const {
|
||
|
|
return program_->NumResiduals();
|
||
|
|
}
|
||
|
|
|
||
|
|
int ProblemImpl::ParameterBlockSize(const double* values) const {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "you can get its size.";
|
||
|
|
}
|
||
|
|
|
||
|
|
return parameter_block->Size();
|
||
|
|
}
|
||
|
|
|
||
|
|
int ProblemImpl::ParameterBlockLocalSize(const double* values) const {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "you can get its local size.";
|
||
|
|
}
|
||
|
|
|
||
|
|
return parameter_block->LocalSize();
|
||
|
|
}
|
||
|
|
|
||
|
|
bool ProblemImpl::HasParameterBlock(const double* parameter_block) const {
|
||
|
|
return (parameter_block_map_.find(const_cast<double*>(parameter_block)) !=
|
||
|
|
parameter_block_map_.end());
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const {
|
||
|
|
CHECK_NOTNULL(parameter_blocks);
|
||
|
|
parameter_blocks->resize(0);
|
||
|
|
for (ParameterMap::const_iterator it = parameter_block_map_.begin();
|
||
|
|
it != parameter_block_map_.end();
|
||
|
|
++it) {
|
||
|
|
parameter_blocks->push_back(it->first);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::GetResidualBlocks(
|
||
|
|
vector<ResidualBlockId>* residual_blocks) const {
|
||
|
|
CHECK_NOTNULL(residual_blocks);
|
||
|
|
*residual_blocks = program().residual_blocks();
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::GetParameterBlocksForResidualBlock(
|
||
|
|
const ResidualBlockId residual_block,
|
||
|
|
vector<double*>* parameter_blocks) const {
|
||
|
|
int num_parameter_blocks = residual_block->NumParameterBlocks();
|
||
|
|
CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks);
|
||
|
|
for (int i = 0; i < num_parameter_blocks; ++i) {
|
||
|
|
(*parameter_blocks)[i] =
|
||
|
|
residual_block->parameter_blocks()[i]->mutable_user_state();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
const CostFunction* ProblemImpl::GetCostFunctionForResidualBlock(
|
||
|
|
const ResidualBlockId residual_block) const {
|
||
|
|
return residual_block->cost_function();
|
||
|
|
}
|
||
|
|
|
||
|
|
const LossFunction* ProblemImpl::GetLossFunctionForResidualBlock(
|
||
|
|
const ResidualBlockId residual_block) const {
|
||
|
|
return residual_block->loss_function();
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProblemImpl::GetResidualBlocksForParameterBlock(
|
||
|
|
const double* values,
|
||
|
|
vector<ResidualBlockId>* residual_blocks) const {
|
||
|
|
ParameterBlock* parameter_block =
|
||
|
|
FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL);
|
||
|
|
if (parameter_block == NULL) {
|
||
|
|
LOG(FATAL) << "Parameter block not found: " << values
|
||
|
|
<< ". You must add the parameter block to the problem before "
|
||
|
|
<< "you can get the residual blocks that depend on it.";
|
||
|
|
}
|
||
|
|
|
||
|
|
if (options_.enable_fast_removal) {
|
||
|
|
// In this case the residual blocks that depend on the parameter block are
|
||
|
|
// stored in the parameter block already, so just copy them out.
|
||
|
|
CHECK_NOTNULL(residual_blocks)->resize(
|
||
|
|
parameter_block->mutable_residual_blocks()->size());
|
||
|
|
std::copy(parameter_block->mutable_residual_blocks()->begin(),
|
||
|
|
parameter_block->mutable_residual_blocks()->end(),
|
||
|
|
residual_blocks->begin());
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Find residual blocks that depend on the parameter block.
|
||
|
|
CHECK_NOTNULL(residual_blocks)->clear();
|
||
|
|
const int num_residual_blocks = NumResidualBlocks();
|
||
|
|
for (int i = 0; i < num_residual_blocks; ++i) {
|
||
|
|
ResidualBlock* residual_block =
|
||
|
|
(*(program_->mutable_residual_blocks()))[i];
|
||
|
|
const int num_parameter_blocks = residual_block->NumParameterBlocks();
|
||
|
|
for (int j = 0; j < num_parameter_blocks; ++j) {
|
||
|
|
if (residual_block->parameter_blocks()[j] == parameter_block) {
|
||
|
|
residual_blocks->push_back(residual_block);
|
||
|
|
// The parameter blocks are guaranteed unique.
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace internal
|
||
|
|
} // namespace ceres
|