void DepthVideoPoseOptimizer::addSceneFlowSmoothnessLoss()

in lib/PoseOptimizer.cpp [1242:1339]
81 lines of code
21 McCabe index (conditional complexity)

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

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

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

  int tripletCount = 0;
  int constraintCount = 0;

  for (int frame = params.frameRange.firstFrame();
       frame < params.frameRange.lastFrame() - 1; ++frame) {
    if (!params.frameRange.inRange(frame) ||
        !params.frameRange.inRange(frame + 1) ||
        !params.frameRange.inRange(frame + 2)) {
      continue;
    }
    ++tripletCount;

    DepthFrame& df0 = ds.frame(frame + 0);
    DepthFrame& df1 = ds.frame(frame + 1);
    DepthFrame& df2 = ds.frame(frame + 2);

    const int triplet = frame + 1;
    const TripletFlowConstraints& tripletConstraints = constraints(triplet);
    for (const TripletConstraint& c : tripletConstraints) {
      const Vector2fna& loc0 = c[0];
      auto obs0 = std::make_shared<Observation>(
          df0, poseParams_[frame + 0].data(), loc0);

      const Vector2fna& loc1 = c[1];
      auto obs1 = std::make_shared<Observation>(
          df1, poseParams_[frame + 1].data(), loc1);

      const Vector2fna& loc2 = c[2];
      auto obs2 = std::make_shared<Observation>(
          df2, poseParams_[frame + 2].data(), loc2);

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

      using LossType = SceneFlowSmoothnessLoss;
      using CostFnType = ceres::DynamicAutoDiffCostFunction<LossType, kStride>;
      auto* costFunction = new CostFnType(new LossType(
          obs0, obs1, obs2, vFocal, aspect, params.intrOpt,
          params.smoothLossType));

      costFunction->SetNumResiduals(3);

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

      double lossWeight = (c.isStatic ?
          params.smoothStaticWeight : params.smoothDynamicWeight);
      ceres::LossFunction* lossFunction = new ceres::ScaledLoss(
          nullptr, lossWeight, ceres::TAKE_OWNERSHIP);

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

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

      ++constraintCount;
    }
  }

  LOG(INFO) << "    Using " << tripletCount << " frame triplets.";
  LOG(INFO) << "    Added " << constraintCount << " constraints.";
}