void DepthVideoPoseOptimizer::poseOptimization()

in lib/PoseOptimizer.cpp [788:888]
80 lines of code
11 McCabe index (conditional complexity)

void DepthVideoPoseOptimizer::poseOptimization(
    const Params& params, const FlowConstraintsCollection& constraints) {
  LOG(INFO) << "------------------------";
  LOG(INFO) << "Pose optimization (depth stream " << depthStream_ << ")...";

  params.printParams();

  int ctfRows = params.ctfLong;
  int ctfCols = params.ctfShort;
  int dsoRows = params.dsoLong;
  int dsoCols = params.dsoShort;
  if (video_->aspect() >= 1.f) {
    std::swap(ctfCols, ctfRows);
    std::swap(dsoCols, dsoRows);
  }

  auto gridSize = [&](const XformDescriptor& desc) {
    if (desc.depthType == DepthXformType::Grid) {
      return Vector3i(desc.gridSize.x(), desc.gridSize.y(), desc.gridSize.z());
    } else {
      return Vector3i(1, 1, 1);
    }
  };

  // Get initial grid size.
  const DepthStream& ds = video_->depthStream(depthStream_);
  const Vector3i initGrid = gridSize(ds.depthXformDesc());

  if (params.deferredSpatialOpt) {
    LOG(INFO) << "Setting spatial transforms to identity transform.";
    DepthVideoProcessor::Params pp;
    pp.depthStream = depthStream_;
    pp.spatialXformDesc.type = XformType::Spatial;
    pp.spatialXformDesc.spatialType = SpatialXformType::Identity;

    DepthVideoProcessor processor(video_);
    processor.resetSpatialXforms(pp);
  }

  for (int step = 0; step < params.numSteps; ++step) {
    LOG(INFO) << "----------------";
    LOG(INFO) << "Step " << (step + 1) << " / " << params.numSteps << "...";

    const double stepIter =
        (params.numSteps > 1 ? step / double(params.numSteps - 1) : 0.0);

    double depthDeformReg = params.depthDeformRegFinal;

    if (params.graduateDepthDeformReg) {
      const double defRegInit = log(params.depthDeformRegInitial);
      const double defRegFinal = log(params.depthDeformRegFinal);

      depthDeformReg = exp(defRegInit + (defRegFinal - defRegInit) * stepIter);
    }

    LOG(INFO) << "Depth Deformation regularization: " << depthDeformReg;

    poseOptimizationStep(params, constraints, depthDeformReg);

    if (params.coarseToFine && step < params.numSteps - 1) {
      double ctfIter = (step + 1) / double(params.numSteps - 1);
      const Vector3i curGrid = gridSize(ds.depthXformDesc());

      DepthVideoProcessor::Params splitParams;
      splitParams.depthStream = depthStream_;
      splitParams.depthXformDesc = ds.depthXformDesc();
      if (splitParams.depthXformDesc.depthType == DepthXformType::Global) {
        splitParams.depthXformDesc.depthType = DepthXformType::Grid;
      }

      Vector3i& newGrid = splitParams.depthXformDesc.gridSize;
      newGrid.x() =
          int(initGrid.x() + (ctfCols - initGrid.x()) * ctfIter + 0.5);
      newGrid.y() =
          int(initGrid.y() + (ctfRows - initGrid.y()) * ctfIter + 0.5);
      newGrid.z() = initGrid.z();
      LOG(INFO) << "Splitting grid " <<
          curGrid.x() << " x " << curGrid.y() << " x " << curGrid.z() <<
          " --> " <<
          newGrid.x() << " x " << newGrid.y() << " x " << newGrid.z() << "...";

      DepthVideoProcessor processor(video_);
      processor.gridXformSplit(splitParams);
    }
  }

  if (params.deferredSpatialOpt) {
    LOG(INFO) << "Setting spatial transforms to bicubic grid.";
    DepthVideoProcessor::Params pp;
    pp.depthStream = depthStream_;
    pp.spatialXformDesc.type = XformType::Spatial;
    pp.spatialXformDesc.spatialType = SpatialXformType::BicubicGrid;
    pp.spatialXformDesc.gridSize.y() = dsoRows;
    pp.spatialXformDesc.gridSize.x() = dsoCols;

    DepthVideoProcessor processor(video_);
    processor.resetSpatialXforms(pp);

    poseOptimizationStep(params, constraints, params.depthDeformRegFinal);
  }
}