[Ceres]problem中⼏个重要函数
[Ceres]problem中⼏个重要函数
简介
这⼏天在写⾥程计，需要⽤到 Ceres，对于Ceres虽然有⼤致的概念，但对于其实现的部分还是不了解，所以去看了⼏个函数的源码，之后会继续看下去。感觉看了源码之后⽤起来才会舒服⼀些，哈哈哈哈…
函数探究
problem::AddParameterBlock(double* values, int size)
这个函数的作⽤是将参数块添加到problem中。Class Problem类只是个提供接⼝，实现都在ProblemImpl中。函数内容看以下源码
void ProblemImpl::AddParameterBlock(double* values, int size) {
InternalAddParameterBlock(values, size);
}
ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values,
int size) {
CHECK(values != NULL) << "Null pointer pasd to AddParameterBlock "
<< "for a parameter with size " << size;
// parameter_block_map_是⼀个std::map，能检测出重复添加的参数块
ParameterMap::iterator it = parameter_block_map_.find(values);
//若通过参数块的地址可以找到之前有添加相同的参数块，则返回这个参数块
if (it != parameter_block_map_.end()) {
if (!options_.disable_all_safety_checks) {
int existing_size = it->cond->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->cond;
}
//这个参数默认是fal，默认开启安全检测，检测是否有相同的参数块
if (!options_.disable_all_safety_checks) {
if (!parameter_block_map_.empty()) {
/
/返回⼀个迭代器，指向键值>= key的第⼀个元素
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->cond->Size(),
values,
size);
}
/
/ If lb is not off the end, check lb for aliasing.
if (lb != parameter_block_map_.end()) {
CheckForNoAliasing(lb->first,
lb->cond->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.
//这个参数默认是fal
if (options_.enable_fast_removal) {
new_parameter_block->EnableResidualBlockDependencies();
}
//更新std::map，并将新的参数块存⼊program_->parameter_blocks_中
parameter_block_map_[values] = new_parameter_block;
program_->parameter_blocks_.push_back(new_parameter_block);
return new_parameter_block;
}
所以即使重复添加同⼀个参数，也是没关系的。下⾯来看第⼆个**AddParameterBlock()**函数
void ProblemImpl::AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization)
这个函数是加了local_parameterization，⽐如在四元数的计算时要⽤到这个参数。这个函数⽤的时候要注意⼀点。 如果是相同的参数化⽅法，⽐如添加的参数块都是四元数，那么这个local_paramerterizatio最好全局的，也就是说，多个参数块公⽤⼀个
local_parameterization.下⾯来看⼀下源码就很清楚了。
void ProblemImpl::AddParameterBlock(
double* values,
int size,
LocalParameterization* local_parameterization){
//这⾏代码和上⾯的函数是⼀样的
ParameterBlock* parameter_block =
InternalAddParameterBlock(values, size);
if(local_parameterization !=NULL){
// 之后再对参数块的设置local_parameterization
parameter_block->SetParameterization(local_parameterization);
}
}
再上源码
void SetParameterization(LocalParameterization* new_parameterization){
CHECK(new_parameterization !=nullptr)
<<"nullptr parameterization invalid.";
// 检测原本的参数化⽅法是否和新添加的参数化⽅法⼀样，若⼀样则直接返回，当然还是⽤地址来判断
if(new_parameterization == local_parameterization_){
return;
}
//若不⼀样，则检测原本的参数化⽅法是否为空，这说明⼀个参数块不能设置两个参数化⽅法，会报错的
CHECK(local_parameterization_ ==nullptr)
<<"Can't re-t the local parameterization; it leads to "
<<"ambiguous ownership. Current local parameterization is: "
<< local_parameterization_;
CHECK(new_parameterization->GlobalSize()== size_)
<<"Invalid parameterization for parameter block. The parameter block "
<<"has size "<< size_ <<" while the parameterization has a global "
<<"size of "<< new_parameterization->GlobalSize()<<". Did you "
<<"accidentally u the wrong parameter block or parameterization?";
CHECK_GT(new_parameterization->LocalSize(),0)
<<"Invalid parameterization. Parameterizations must have a "
<<"positive dimensional tangent space.";
//将新的参数化⽅法设置到本地的参数化⽅法中去
local_parameterization_ = new_parameterization;
local_parameterization_jacobian_.ret(
new double[local_parameterization_->GlobalSize()*
local_parameterization_->LocalSize()]);
CHECK(UpdateLocalParameterizationJacobian())
<<"Local parameterization Jacobian computation failed for x: "
<<ConstVectorRef(state_,Size()).transpo();
}
下⾯写个例⼦就⽐较清晰了
错误试例：
这么写是会报错的，是因为前后两次设置的local_parameterization的地址是不⼀样的，会报
”Can’t re-t the local parameterization; it leads to ambiguous ownership. Current local parameterization is: xxxxx"的错误。
for(int i =0; i < num_knots; i++){
ceres::LocalParameterization* local_parameterization =
new LieLocalParameterization<SO3d>();
problem.AddParameterBlock(knots[i].data(),4, local_parameterization);
}
for(int i =0; i < num_knots; i++){
ceres::LocalParameterization* local_parameterization =
new LieLocalParameterization<SO3d>();
problem.AddParameterBlock(knots[i].data(),4, local_parameterization);
}
正确试例：
这么写就是对的，前后两次添加相同的参数块，他们的local_parameterization地址是⼀样的。
ceres::LocalParameterization* local_parameterization =
new LieLocalParameterization<SO3d>();
for(int i =0; i < num_knots; i++){
problem.AddParameterBlock(knots[i].data(),4, local_parameterization);
}
for(int i =0; i < num_knots; i++){
problem.AddParameterBlock(knots[i].data(),4, local_parameterization);
}
Problem::AddResidualBlock()
这个函数是添加残差块，很重要的⼀个函数，problem中重载很多种，我⽤最常见的⼀种来说明，同样实现部分在ProblemImpl中。有⼀点很重要，AddResidualBlock()中添加的参数块不能重复，即不能重复添加相同的参数（这个坑我也掉过，程序崩了）
ResidualBlockId ProblemImpl::AddResidualBlock(
CostFunction* cost_function,
LossFunction* loss_function,
double* const* const parameter_blocks,
int num_parameter_blocks) {
//parameter_blocks.size()与cost_function->parameter_block_sizes_.size()应该是⼀样的
//cost_function->parameter_block_sizes_是⼀个vector，存放的是每个优化参数的⼤⼩
CHECK(cost_function != nullptr);
CHECK_EQ(num_parameter_blocks,
cost_function->parameter_block_sizes().size());
// Check the sizes match.
const vector<int32_t>& parameter_block_sizes =
cost_function->parameter_block_sizes();
//安全检测，是否有相同的参数块，若有相同的参数块，就会报错
if (!options_.disable_all_safety_checks) {
CHECK_EQ(parameter_block_sizes.size(), num_parameter_blocks)
<< "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, parameter_blocks + num_parameter_blocks);
sort(sorted_parameter_blocks.begin(), sorted_d());
//这个函数⽤来查找相邻两个元素是否为相等，因为上⼀⾏代码已经排序了
const bool has_duplicate_items =
(std::adjacent_find(sorted_parameter_blocks.begin(),
sorted_d())
!= sorted_d());
if (has_duplicate_items) {
string blocks;
for (int i = 0; i < num_parameter_blocks; ++i) {
blocks += StringPrintf(" %p ", parameter_blocks[i]);
}
LOG(FATAL) << "Duplicate parameter blocks in a residual parameter "
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(num_parameter_blocks);
//这⾥⼜调⽤InternalAddParameterBlock()函数来⽣成参数块，所以AddParameterBlock()这个函数在构造problem时不⼀定要出现 for (int i = 0; i < num_parameter_blocks; ++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 < num_parameter_blocks; ++i) {
parameter_block_ptrs[i]->AddResidualBlock(new_residual_block);
}
}
//保存到program_->residual_blocks_的vector中
program_->residual_blocks_.push_back(new_residual_block);
if (options_.enable_fast_removal) {
residual_block_t_.inrt(new_residual_block);
}
if (options_.cost_function_ownership == TAKE_OWNERSHIP) {
// Increment the reference count, creating an entry in the table if
// needed. Note: C++ maps guarantee that new entries have default
// constructed values; this implies integers are zero initialized.
++cost_function_ref_count_[cost_function];
}
if (options_.loss_function_ownership == TAKE_OWNERSHIP &&
loss_function != NULL) {
++loss_function_ref_count_[loss_function];
}
return new_residual_block;
}
Problem::SetParameterBlockConstant(double* values)
