ceres库AddResidualBlock函数解析

在定义损失函数时，我们会预先告诉parameter_blocks的个数，class ProjectionFactorXYZ : public ceres::SizedCostFunction<2, 7, 3> 表示2个损失函数，2个参数块，第一个参数块有7个数，第二个参数块有3个数。我们在传给ceres时，传进去的是参数块的首地址。又知道参数块里面的参数个数。所以ceres就可以一个个把参数拿出来。

ResidualBlock* ProblemImpl::AddResidualBlock(
    CostFunction* cost_function,   
    LossFunction* loss_function,
    const vector<double*>& parameter_blocks) {  //传进来的参数块（注意是参数块，不是参数）的地址放在vector<double*>里面，两个参数块则vector里面有两个
  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(); //比如我们定义的参数块是2个，里面分别有 7 3个参数，parameter_block_sizes里面放的是7 3

  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());
    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 < 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.
  //这里是把参数块里的数据取出来放到ceres自己定义的参数块里面去，对参数块做了检查
  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);
  }

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

parameter_block_map_是一个map，里面放着参数块的地址和对应生成的ParameterBlock。这个变量应该是便于在增加参数块时，判断该参数块是否之前已经加入。

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; //如果已经加入则返回已经生成的paramblock
  }  //

  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()) {//其实就是找到values附近地址的参数块，看是否和values参数块的内存有重叠
      ParameterMap::iterator lb = parameter_block_map_.lower_bound(values);
		//找到比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);
      }
    }
  }
//上面既检查了是否已经存在该参数块，又检查了是否有参数块和该参数块内存出现重叠，如果都没有，说明该参数块是好的，则生成ParameterBlock
  // 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;
}

CheckForNoAliasing是判断两个参数块有没有重叠，比如参数块1的首地址是a,参数个数是size_a, 参数块2的首地址是b,参数个数是size_b.
我们已经知道这两个参数块是挨在一起的，假设a<b ，那么如果b < a+size_a，说明这两个参数块内存有部分重叠了，这肯定就有问题了。