Ceres Solver相机跟踪：如何动态调整残差块？迭代间能否增删残差块？

针对你在相机跟踪任务中遇到的「迭代后部分重投影点超出视野，需要动态移除/调整残差块」的问题，Ceres-Solver完全支持在优化迭代过程中添加或移除残差块，下面给你两种最实用的方案，以及对应的注意事项：

方案一：直接增删残差块（灵活可控） #

Ceres的Problem类提供了RemoveResidualBlock()和AddResidualBlock()原生接口，能直接修改问题中的残差结构，步骤如下：

1. 预先存储残差块ID
   调用AddResidualBlock()时，一定要保存返回的ceres::ResidualBlockId，比如用一个std::vector<ceres::ResidualBlockId>来管理所有特征点对应的残差块：

   std::vector<ceres::ResidualBlockId> residual_blocks;
   for (const auto& feature : features) {
     auto cost_functor = new ceres::AutoDiffCostFunction<PhotometricCostFunctor, 2, 6>(
       new PhotometricCostFunctor(feature, camera_intrinsics, reference_image)
     );
     auto residual_block = problem.AddResidualBlock(
       cost_functor,
       new ceres::HuberLoss(1.0), // 可选的损失函数
       pose_parameter // 待优化的位姿参数块
     );
     residual_blocks.push_back(residual_block);
   }
   

2. 迭代后检查并移除无效残差
   在每次优化迭代结束后，根据最新的位姿计算所有特征点的重投影位置，判断是否在相机视野内（比如图像宽高范围内），对无效的残差块调用RemoveResidualBlock()：

   // 获取最新位姿
   double current_pose[6];
   problem.GetParameterBlockPointer(pose_parameter)->CopyTo(current_pose, 6);
   
   auto it = residual_blocks.begin();
   int feature_idx = 0;
   while (it != residual_blocks.end()) {
     const auto& feature = features[feature_idx];
     double proj_x, proj_y;
     // 自定义投影函数：用当前位姿将3D点投影到图像平面
     project_point(current_pose, feature.world_point, camera_intrinsics, &proj_x, &proj_y);
   
     // 判断是否超出视野
     if (proj_x < 0 || proj_x >= camera_intrinsics.width || 
         proj_y < 0 || proj_y >= camera_intrinsics.height) {
       // 移除残差块
       problem.RemoveResidualBlock(*it);
       it = residual_blocks.erase(it);
     } else {
       ++it;
     }
     feature_idx++;
   }
   

3. 后续重新添加（如果需要）
   如果后续帧中某个特征点又回到视野内，可以重新调用AddResidualBlock()将其加入优化，同样保存新的残差块ID即可。

方案二：带视野判断的残差项（高效低开销） #

如果频繁增删残差块导致优化效率下降（因为Ceres需要重新构建线性求解器的内部结构），可以在残差计算逻辑中直接跳过无效点，让这类残差对优化没有贡献：

// 自动微分的光度误差残差类
struct PhotometricCostFunctor {
  PhotometricCostFunctor(const FeaturePoint& feature, 
                         const CameraIntrinsics& intrinsics,
                         const cv::Mat& ref_img)
      : feature_(feature), intrinsics_(intrinsics), ref_img_(ref_img) {}

  template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    // 用当前位姿投影3D点到图像平面（注意用模板类型T计算，保证自动微分正确性）
    T proj_x, proj_y;
    project_point(pose, feature_.world_point, intrinsics_, &proj_x, &proj_y);

    // 判断是否在视野内：必须用T类型的常量做比较
    if (proj_x < T(0) || proj_x >= T(intrinsics_.width - 1) ||
        proj_y < T(0) || proj_y >= T(intrinsics_.height - 1)) {
      // 超出视野，残差设为0，不影响优化
      residual[0] = T(0);
      residual[1] = T(0); // 假设是2通道的光度残差（比如灰度+梯度）
      return true;
    }

    // 正常计算光度误差（比如参考图与当前图的像素差）
    T ref_pixel = get_pixel<T>(ref_img_, proj_x, proj_y);
    T curr_pixel = get_pixel<T>(curr_img_, proj_x, proj_y);
    residual[0] = ref_pixel - curr_pixel;
    residual[1] = ...; // 可选的梯度残差
    return true;
  }

private:
  const FeaturePoint& feature_;
  const CameraIntrinsics& intrinsics_;
  const cv::Mat& ref_img_;
};

这种方式不需要修改Problem的结构，残差块始终存在，但无效点的残差为0，不会对优化方向产生影响，适合帧率要求高的场景。

进阶：用迭代回调自动处理 #

可以通过Ceres的IterationCallback接口，让残差块的清理自动在每次迭代后执行，无需手动触发：

class ResidualCleanupCallback : public ceres::IterationCallback {
public:
  ResidualCleanupCallback(ceres::Problem* problem,
                          std::vector<ceres::ResidualBlockId>& residual_blocks,
                          const std::vector<FeaturePoint>& features,
                          const CameraIntrinsics& intrinsics)
      : problem_(problem), residual_blocks_(residual_blocks),
        features_(features), intrinsics_(intrinsics) {}

  ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) {
    // 只在求解迭代结束后执行
    if (summary.iteration_type != ceres::SOLVER_ITERATION) {
      return ceres::SOLVER_CONTINUE;
    }

    // 获取最新位姿
    double pose[6];
    problem_->GetParameterBlockPointer(pose_param_id_)->CopyTo(pose, 6);

    // 清理无效残差块
    auto it = residual_blocks_.begin();
    int idx = 0;
    while (it != residual_blocks_.end()) {
      const auto& feat = features_[idx];
      double proj_x, proj_y;
      project_point(pose, feat.world_point, intrinsics_, &proj_x, &proj_y);
      if (proj_x < 0 || proj_x >= intrinsics_.width || 
          proj_y < 0 || proj_y >= intrinsics_.height) {
        problem_->RemoveResidualBlock(*it);
        it = residual_blocks_.erase(it);
      } else {
        ++it;
      }
      idx++;
    }

    return ceres::SOLVER_CONTINUE;
  }

  // 设置待优化的位姿参数ID
  void set_pose_param_id(double* pose_param) {
    pose_param_id_ = pose_param;
  }

private:
  ceres::Problem* problem_;
  std::vector<ceres::ResidualBlockId>& residual_blocks_;
  const std::vector<FeaturePoint>& features_;
  const CameraIntrinsics& intrinsics_;
  double* pose_param_id_;
};

然后在Solver配置中添加回调：

ceres::SolverOptions options;
ResidualCleanupCallback cleanup_callback(&problem, residual_blocks, features, intrinsics);
cleanup_callback.set_pose_param_id(pose_parameter);
options.callbacks.push_back(&cleanup_callback);
options.update_state_every_iteration = true; // 确保能获取到最新的位姿参数

ceres::SolverSummary summary;
ceres::Solve(options, &problem, &summary);

注意事项 #

1. 自动微分的正确性：在残差类中做视野判断时，必须使用模板类型T的常量（比如T(0)），不能直接用double，否则会导致自动微分计算错误。
2. 效率权衡：直接增删残差块会触发Ceres内部结构的重新构建，适合残差块变化不频繁的场景；带开关的残差项效率更高，适合高频更新的场景。
3. 参数块管理：如果某个参数块不再被任何残差块依赖，可以调用RemoveParameterBlock()移除，但相机跟踪中待优化的位姿是全局参数，一般不需要这么做。

内容的提问来源于stack exchange，提问作者CathIAS