Camera::Rig alignPointCloud()

in source/rig/AlignPointCloud.cpp [285:352]
57 lines of code
9 McCabe index (conditional complexity)

Camera::Rig alignPointCloud(
    const Camera::Rig& rig,
    const std::string& includeCamList,
    const std::vector<FeatureList>& allFeatures,
    bool lockRotation = false,
    bool lockTranslation = false,
    bool lockScale = false) {
  ceres::Problem problem;
  Camera::Vector3 rotation(0, 0, 0);
  Camera::Vector3 translation(0, 0, 0);
  Eigen::UniformScaling<double> scale(1);

  const std::vector<FeatureList>& inlyingFeatures = removeOutliers(rig, allFeatures);

  logMedianErrors(rig, inlyingFeatures);

  std::vector<std::string> includeCams;
  boost::split(includeCams, includeCamList, [](char c) { return c == ','; });

  int alignmentCameras = 0;
  for (int i = 0; i < int(rig.size()); ++i) {
    if (!includeCamList.empty() &&
        std::find(includeCams.begin(), includeCams.end(), rig[i].id) == includeCams.end()) {
      LOG(INFO) << folly::sformat("Excluding camera {} from calibration ", rig[i].id);
      continue;
    }
    alignmentCameras++;
    for (const Match3D& feature : inlyingFeatures[i]) {
      PointCloudFunctor::addResidual(problem, rotation, translation, scale, rig[i], feature);
    }
  }

  if (alignmentCameras == 1) {
    LOG(INFO) << "Single camera aligment detected. Locking rig scale to 1.";
    lockScale = true;
  }

  problem.SetParameterLowerBound(&scale.factor(), 0, 0.25);
  problem.SetParameterLowerBound(rotation.data(), 0, -M_PI);
  problem.SetParameterLowerBound(rotation.data(), 1, -M_PI);
  problem.SetParameterLowerBound(rotation.data(), 2, -M_PI / 2);
  problem.SetParameterUpperBound(rotation.data(), 0, M_PI);
  problem.SetParameterUpperBound(rotation.data(), 1, M_PI);
  problem.SetParameterUpperBound(rotation.data(), 2, M_PI / 2);

  if (lockRotation) {
    problem.SetParameterBlockConstant(rotation.data());
  }
  if (lockTranslation) {
    problem.SetParameterBlockConstant(translation.data());
  }
  if (lockScale) {
    problem.SetParameterBlockConstant(&scale.factor());
  }

  solve(problem);

  LOG(INFO) << folly::sformat(
      "New rotation values: {} {} {}", rotation[0], rotation[1], rotation[2]);
  LOG(INFO) << folly::sformat(
      "New translation values: {} {} {}", translation[0], translation[1], translation[2]);
  LOG(INFO) << folly::sformat("New scale: {}", scale.factor());
  const Camera::Rig transformedRig = transformRig(rig, rotation, translation, scale);

  logMedianErrors(transformedRig, inlyingFeatures);

  return transformedRig;
}