[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 passed 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->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;
  }

  //这个参数默认是false，默认开启安全检测，检测是否有相同的参数块
  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->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.
  //这个参数默认是false
  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-set 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 use 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_.reset(
        new double[local_parameterization_->GlobalSize() *
                   local_parameterization_->LocalSize()]);
    CHECK(UpdateLocalParameterizationJacobian())
        << "Local parameterization Jacobian computation failed for x: "
        << ConstVectorRef(state_, Size()).transpose();
  }

下面写个例子就比较清晰了
错误试例：
这么写是会报错的，是因为前后两次设置的local_parameterization的地址是不一样的，会报
”Can’t re-set 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& 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 sorted_parameter_blocks(
        parameter_blocks, parameter_blocks + num_parameter_blocks);
    sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end());
    //这个函数用来查找相邻两个元素是否为相等，因为上一行代码已经排序了
    const bool has_duplicate_items =
        (std::adjacent_find(sorted_parameter_blocks.begin(),
                            sorted_parameter_blocks.end())
         != sorted_parameter_blocks.end());
    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 "
                 << "are not allowed. Parameter block pointers: ["
                 << blocks << "]";
    }
  }

  // Add parameter blocks and convert the double*'s to parameter blocks.
  vector 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_set_.insert(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)

这个函数是设置参数块为常数，不被优化，与**Problem::SetParameterBlockVariable(double* values)**相反。

这个实现就很简单了，利用了parameter_block_map这个std::map来找相对应的参数块，Class ParameterBlock中有个变量是 is_set_constant，通过设置该变量来让该参数块是否被优化。

void ProblemImpl::SetParameterBlockConstant(double* values) {
  //通过parameter_block_map_来查询相对应的参数块
  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.";
  }
  //设置is_set_constant的这个变量
  parameter_block->SetConstant();
}