void DepthVideoPoseOptimizer::normalizeDepth()

in lib/PoseOptimizer.cpp [992:1147]
118 lines of code
25 McCabe index (conditional complexity)

void DepthVideoPoseOptimizer::normalizeDepth(
    const Params& params, const FlowConstraintsCollection& constraints) {
  LOG(INFO) << "------------------------";
  LOG(INFO) << "Depth Normalization (depth stream " << depthStream_ << ")...";

  params.printParams();

  LOG(INFO) << "Building problem...";
  problem_ = std::make_unique<ceres::Problem>();

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

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

    if (!params.frameRange.inRange(i0) || !params.frameRange.inRange(i1)) {
      continue;
    }

    if (params.normalizeDepthFromFirstFrame) {
      // When we normalize from the first frame, we don't need any pairs, but
      // instead rely on addScaleRegularization() below.
      break;
    }

    DepthFrame& df0 = ds.frame(i0);
    DepthFrame& df1 = ds.frame(i1);
    const Mat1f* depthImg0Ptr = df0.sourceDepth();
    const Mat1f* depthImg1Ptr = df1.sourceDepth();
    if (!depthImg0Ptr || !depthImg1Ptr) {
      throw std::runtime_error("Missing depth image.");
    }
    const Mat1f& depthImg0 = *depthImg0Ptr;
    const Mat1f& depthImg1 = *depthImg1Ptr;

    const PairFlowConstraints& pairConstraints = constraints(pair);
    for (const PairFlowConstraints::Constraint& c : pairConstraints) {
      const Vector2fna& obs0 = c[0];
      const Vector2fna& obs1 = c[1];

      const int x0 = obs0.x() * depthImg0.cols;
      const int y0 = obs0.y() / invAspect * depthImg0.rows;
      const float depth0 = depthImg0(y0, x0);

      const int x1 = obs1.x() * depthImg1.cols;
      const int y1 = obs1.y() / invAspect * depthImg1.rows;
      const float depth1 = depthImg1(y1, x1);

      if (!std::isfinite(depth0) || depth0 <= 0 ||
          !std::isfinite(depth1) || depth1 <= 0) {
        continue;
      }

      const Vector2f obs0Scaled(
          -1.f + 2.f * obs0.x(), 1.f - 2.f * obs0.y() / invAspect);
      const Vector2f obs1Scaled(
          -1.f + 2.f * obs1.x(), 1.f - 2.f * obs1.y() / invAspect);

      std::unique_ptr<DepthFunctor> depth0Functor =
          df0.depthXform().createFunctor(depth0, obs0Scaled);
      std::unique_ptr<DepthFunctor> depth1Functor =
          df1.depthXform().createFunctor(depth1, obs1Scaled);

      std::vector<int> depth0ParamBlockSizes = depth0Functor->paramBlockSizes();
      std::vector<double*> depth0ParamBlocks = depth0Functor->paramBlocks();
      std::vector<int> depth1ParamBlockSizes = depth1Functor->paramBlockSizes();
      std::vector<double*> depth1ParamBlocks = depth1Functor->paramBlocks();

      // Create the cost function
      using CostFunction = ceres::DynamicAutoDiffCostFunction<
          DisparityDissimilarityCost, kStride>;
      CostFunction* costFunction =
          new CostFunction(new DisparityDissimilarityCost(
              std::move(depth0Functor), std::move(depth1Functor)));

      // Set the parameter block sizes for the two cameras and depth functors.
      for (int size : depth0ParamBlockSizes) {
        costFunction->AddParameterBlock(size);
      }
      for (int size : depth1ParamBlockSizes) {
        costFunction->AddParameterBlock(size);
      }

      costFunction->SetNumResiduals(1);

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

      // Create a vector of all the parameter blocks for the two cameras and
      // depth functors, and add the cost to the problem.
      std::vector<double*> paramBlocks;
      for (double* block : depth0ParamBlocks) {
        paramBlocks.push_back(block);
      }
      for (double* block : depth1ParamBlocks) {
        paramBlocks.push_back(block);
      }

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

  if (params.scaleReg > 0.0) {
    addScaleRegularization(params);
  }

  if (params.depthDeformRegInitial > 0.0) {
    addDepthDeformRegularization(params, params.depthDeformRegInitial);
  }

  // I was having some problem with normalization producing some negative scale
  // parameters, because the initialization was too far off. Setting the lower
  // bound for these parameters resolves this issue.
  for (const int frame : params.frameRange) {
    DepthFrame& df = ds.frame(frame);
    for (double* paramBlock : df.depthXform().paramBlocks()) {
      if (problem_->HasParameterBlock(paramBlock)) {
        problem_->SetParameterLowerBound(paramBlock, 0, 0.0);
      }
    }
  }

  LOG(INFO) << "Solving...";
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = params.maxIterations;
  options.num_threads = params.numThreads;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &*problem_, &summary);
  LOG(INFO) << summary.BriefReport();

  if (params.normalizeDepthFromFirstFrame) {
    // Copy first frame's depth xform to all other frames.
    const int firstFrame = params.frameRange.firstFrame();
    const DepthXform& firstXform =
        video_->depthFrame(depthStream_, firstFrame).depthXform();
    for (const int frame : params.frameRange) {
      if (frame == firstFrame) {
        continue;
      }
      video_->depthFrame(depthStream_, frame).depthXform().copyFrom(firstXform);
    }
  }

  // Clear depth frame transform caches.
  for (int frame : params.frameRange) {
    DepthFrame& df = video_->depthStream(depthStream_).frame(frame);
    df.clearXformedCache();
  }

  LOG(INFO) << "";
}