void DepthVideoPoseOptimizer::poseOptimizationStep()

in lib/PoseOptimizer.cpp [890:990]
82 lines of code
19 McCabe index (conditional complexity)

void DepthVideoPoseOptimizer::poseOptimizationStep(
    const Params& params,
    const FlowConstraintsCollection& constraints,
    const double depthDeformReg) {
  LOG(INFO) << "Building problem...";
  problem_ = std::make_unique<ceres::Problem>();

  addStaticSceneLoss(params, constraints);

  if (params.smoothStaticWeight > 0.0 || params.smoothDynamicWeight > 0.0) {
    addSceneFlowSmoothnessLoss(params, constraints);
  }

  if (params.positionReg > 0.0) {
    addPositionRegularization(params);
  }

  if (depthDeformReg > 0.0) {
    addDepthDeformRegularization(params, depthDeformReg);
  }

  if (params.spatialDeformReg > 0.0) {
    addSpatialDeformRegularization(params);
  }

  if (params.fixPoses) {
    for (int frame : params.frameRange) {
      double* values = poseParams_[frame].data();
      if (problem_->HasParameterBlock(values)) {
        problem_->SetParameterBlockConstant(values);
      }
    }
  }

  if (params.fixDepthXforms) {
    DepthStream& ds = video_->depthStream(depthStream_);
    for (int frame : params.frameRange) {
      DepthXform& x = ds.frame(frame).depthXform();
      for (double* block : x.paramBlocks()) {
        problem_->SetParameterBlockConstant(block);
      }
    }
  } else {
    if (params.scaleReg > 0.0) {
      // Compute a good initialization for the first frame's transform and fix
      // it.
      addScaleRegularization(params);
    }
  }

  if (params.fixSpatialXforms) {
    DepthStream& ds = video_->depthStream(depthStream_);
    for (int frame : params.frameRange) {
      SpatialXform& x = ds.frame(frame).spatialXform();
      for (double* block : x.paramBlocks()) {
        problem_->SetParameterBlockConstant(block);
      }
    }
  }

  if (params.focalReg > 0.0) {
    addFocalRegularization(params);
  }

  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();

  // Writing back camera poses
  for (int frame : params.frameRange) {
    const std::array<double, 7>& pose = poseParams_[frame];

    Extrinsics extr;
    extr.position = Vector3f(pose[0], pose[1], pose[2]);

    Matrix3d rotation;
    ceres::AngleAxisToRotationMatrix(&pose[3], rotation.data());

    extr.orientation = Quaterniond(rotation).cast<float>();
    DepthFrame& df = video_->depthStream(depthStream_).frame(frame);
    df.extrinsics = extr;
    df.clearXformedCache();

    if (params.intrOpt == IntrinsicsOptimization::Shared) {
      df.intrinsics.vFov = std::atan(poseParams_[0][6]) * 2.f;
      df.intrinsics.hFov =
          std::atan(poseParams_[0][6] * video_->aspect()) * 2.f;
    } else {
      df.intrinsics.vFov = std::atan(pose[6]) * 2.f;
      df.intrinsics.hFov = std::atan(pose[6] * video_->aspect()) * 2.f;
    }
  }

  LOG(INFO) << "";
}