void DepthVideoPoseOptimizer::addStaticSceneLoss()

in lib/PoseOptimizer.cpp [1149:1240]
73 lines of code
17 McCabe index (conditional complexity)

void DepthVideoPoseOptimizer::addStaticSceneLoss(
    const Params& params, const FlowConstraintsCollection& constraints) {
  LOG(INFO) << "  Adding static scene loss...";

  DepthStream& ds = video_->depthStream(depthStream_);

  const double aspect = video_->aspect();
  const double vFocal =
      (aspect >= 1.f ? params.focalLong / aspect : params.focalLong);

  int pairCount = 0;
  int constraintCount = 0;

  for (auto it = constraints.pairBegin(); it != constraints.pairEnd(); it++) {
    const PairKey& pair = *it;
    const int frame0 = pair.first;
    const int frame1 = pair.second;

    if (!params.frameRange.inRange(frame0) ||
        !params.frameRange.inRange(frame1)) {
      continue;
    }
    ++pairCount;

    DepthFrame& df0 = ds.frame(frame0);
    DepthFrame& df1 = ds.frame(frame1);

    const PairFlowConstraints& pairConstraints = constraints(pair);
    for (const PairConstraint& c : pairConstraints) {
      if (!c.isStatic) {
        continue;
      }

      const Vector2fna& loc0 = c[0];
      const Vector2fna& loc1 = c[1];

      auto obs0 = std::make_shared<Observation>(
          df0, poseParams_[frame0].data(), loc0);
      auto obs1 = std::make_shared<Observation>(
          df1, poseParams_[frame1].data(), loc1);

      if (!std::isfinite(obs0->sourceDepth) || obs0->sourceDepth <= 0 ||
          !std::isfinite(obs1->sourceDepth) || obs1->sourceDepth <= 0) {
        continue;
      }

      // Create the cost function.
      ceres::DynamicCostFunction* costFunction = nullptr;
      using LossType = StaticSceneCost;
      using CostFnType = ceres::DynamicAutoDiffCostFunction<LossType, kStride>;
      costFunction = new CostFnType(new LossType(
          obs0, obs1, vFocal, aspect, params.intrOpt,
          params.staticLossType,
          params.staticSpatialWeight, params.staticDepthWeight));

      costFunction->SetNumResiduals(3);

      for (int size : obs0->paramBlockSizes) {
        costFunction->AddParameterBlock(size);
      }
      for (int size : obs1->paramBlockSizes) {
        costFunction->AddParameterBlock(size);
      }
      if (params.intrOpt == IntrinsicsOptimization::Shared) {
        costFunction->AddParameterBlock(1);
      } else if (params.intrOpt == IntrinsicsOptimization::PerFrame) {
        costFunction->AddParameterBlock(1);
        costFunction->AddParameterBlock(1);
      }

      ceres::LossFunction* lossFunction =
          new ceres::CauchyLoss(params.robustness);

      std::vector<double*> paramBlocks;
      insert(paramBlocks, obs0->paramBlockPtrs);
      insert(paramBlocks, obs1->paramBlockPtrs);
      if (params.intrOpt == IntrinsicsOptimization::Shared) {
        paramBlocks.push_back(&poseParams_[0][6]);
      } else if (params.intrOpt == IntrinsicsOptimization::PerFrame) {
        paramBlocks.push_back(&poseParams_[frame0][6]);
        paramBlocks.push_back(&poseParams_[frame1][6]);
      }

      problem_->AddResidualBlock(costFunction, lossFunction, paramBlocks);

      ++constraintCount;
    }
  }

  LOG(INFO) << "    Using " << pairCount << " frame pairs.";
  LOG(INFO) << "    Added " << constraintCount << " constraints.";
}