void BAHelpers::AlignmentConstraints()

in opensfm/src/sfm/src/ba_helpers.cc [791:834]

• 43 lines of code
• 14 McCabe index (conditional complexity)

void BAHelpers::AlignmentConstraints(
    const map::Map& map, const py::dict& config,
    const AlignedVector<map::GroundControlPoint>& gcp, MatX3d& Xp, MatX3d& X) {
  size_t reserve_size = 0;
  const auto& shots = map.GetShots();
  if (!gcp.empty() && config["bundle_use_gcp"].cast<bool>()) {
    reserve_size += gcp.size();
  }
  if (config["bundle_use_gps"].cast<bool>()) {
    for (const auto& shot_p : shots) {
      const auto& shot = shot_p.second;
      if (shot.GetShotMeasurements().gps_position_.HasValue()) {
        reserve_size += 1;
      }
    }
  }
  Xp.conservativeResize(reserve_size, Eigen::NoChange);
  X.conservativeResize(reserve_size, Eigen::NoChange);
  const auto& topocentricConverter = map.GetTopocentricConverter();
  size_t idx = 0;
  // Triangulated vs measured points
  if (!gcp.empty() && config["bundle_use_gcp"].cast<bool>()) {
    for (const auto& point : gcp) {
      if (point.lla_.empty()) continue;
      Vec3d coordinates;
      if (TriangulateGCP(point, shots, coordinates)) {
        Xp.row(idx) = topocentricConverter.ToTopocentric(point.GetLlaVec3d());
        X.row(idx) = coordinates;
        ++idx;
      }
    }
  }
  if (config["bundle_use_gps"].cast<bool>()) {
    for (const auto& shot_p : shots) {
      const auto& shot = shot_p.second;
      const auto pos = shot.GetShotMeasurements().gps_position_;
      if (pos.HasValue()) {
        Xp.row(idx) = pos.Value();
        X.row(idx) = shot.GetPose()->GetOrigin();
        ++idx;
      }
    }
  }
}