Issues with Pose Prior in Glomap

[StonerLing]

Hi, development team.

When I attempted to employ pose prior to address the issue detailed in #138 (comment), I discovered that:

For the pose prior utilized in Glomap, there exists a branch user/joschonb/pose-prior-fix aimed at resolving certain bugs, yet some issues continue to persist.

• The pose prior read from colmap database is (lat, lon, alt) with the latest colmap version f5469f57, which could not be used as a valid Rigid3d position.
  

  glomap/glomap/io/colmap_converter.cc

  Lines 191 to 199 in 3224311

  	const colmap::PosePrior prior = database.ReadPosePrior(image_id);
  	if (prior.IsValid()) {
  	const colmap::Rigid3d world_from_cam_prior(Eigen::Quaterniond::Identity(),
  	prior.position);
  	ite.first->second.cam_from_world = Rigid3d(Inverse(world_from_cam_prior));
  	} else {
  	ite.first->second.cam_from_world = Rigid3d();
  	}
  	}

It may be modified like this

colmap::PosePrior prior = database.ReadPosePrior(image_id);
    if (prior.IsValid()) {
      // Thransform prior position to XYZ if needed.
      if (prior.coordinate_system == colmap::PosePrior::CoordinateSystem::WGS84) {
          colmap::GPSTransform gps_transform;
          prior.position = gps_transform.EllToXYZ(
              std::vector<Eigen::Vector3d>{prior.position})[0];
          prior.coordinate_system = colmap::PosePrior::CoordinateSystem::CARTESIAN;
      }
      else if(prior.coordinate_system == colmap::PosePrior::CoordinateSystem::UNDEFINED){
        LOG(WARNING) << "Unknown coordinate system for image " << image_id
                     << ", assuming cartesian.";
      }
      
      const colmap::Rigid3d world_from_cam_prior(Eigen::Quaterniond::Identity(),
                                                 prior.position);
      ite.first->second.cam_from_world = Rigid3d(Inverse(world_from_cam_prior));

• As comments mentioned here https://github.com/colmap/glomap/pull/57/files/32243119187cac9363636cdaa439eddfd357c8c2#r1860575476, the image.cam_from_world.translation can't be used as t_ori since the function EstimateRotations will be called twice.
  

  glomap/glomap/estimators/global_rotation_averaging.cc

  Lines 59 to 61 in 3224311

  	// Restore the prior position (t = -Rc = R * R_ori * t_ori = R * t_ori)
  	image.cam_from_world.translation =
  	(image.cam_from_world.rotation * image.cam_from_world.translation);

Therefore, considering the possibility of continuing to use pose prior in the bundle adjustment process later on, it might be better to add a PosePrior member variable within the Image class. And then,

//Restore the prior position (t = -Rc)
    image.cam_from_world.translation =
      -(image.cam_from_world.rotation * image.pose_prior.position);

But this would restore the original position every time rotation averaging is performed, or, from the semantic distinction of the process, it would be appropriate to write a separate function called RestoreOriginPosition in the GlobalMapper and call it after rotation averaging.

• If we can obtain an valid PosePrior, then should we no longer randomly generate camera positions in the global positioning step based on PosePrior? I understand that there may be cases where not all images have valid PosePrior. In that case, for images with valid PosePrior, we should not automatically generate positions, while for images without valid PosePrior, we should base the generation on the existing valid PosePriors.

Of course, this can be set by generate_random_positions = false. However 3d points can still only be generated randomly, rather than triangulate with the camera position originally set from PosePrior. Of course, this can be set by generate_random_positions = false. However, 3D points can still only be generated randomly, rather than being triangulated with the camera position originally set from PosePrior.

• If I want to add pose position prior constaints in bundle adjustment, this may require having a correctly scaled estimated value in the global positioning process. It seems that the current global positioning step is more suitable for randomly initializing 3D points, camera positions, and scales.

void BundleAdjuster::AddPosePositionPriorConstraints(
    std::unordered_map<image_t, Image>& images) {
  for (auto& [image_id, image] : images) {
    ceres::CostFunction* cost_function = colmap::CreateAutoDiffCostFunction<
        colmap::AbsolutePosePositionPriorCostFunctor>(
        new colmap::AbsolutePosePositionPriorCostFunctor(
            image.pose_prior.position));

    if (cost_function != nullptr) {
      problem_->AddResidualBlock(
          cost_function,
          options_.loss_function.get(), 
          image.cam_from_world.rotation.coeffs().data(),
          image.cam_from_world.translation.data());
    } else {
      LOG(ERROR) << "Could not Create position prior cost function for image: "
                 << image_id;
    }
  }
}

[h1063135843]

Just create a local variable before changing rotation in function EstimateRotations and InitializeFromMaximumSpanningTree. It is enough.

  for (auto& [image_id, image] : images) {
    if (!image.is_registered) continue;

    auto center = image.Center();
    if (options_.use_gravity && image.gravity_info.has_gravity) {
      image.cam_from_world.rotation = Eigen::Quaterniond(
          image.gravity_info.GetRAlign() *
          AngleToRotUp(rotation_estimated_[image_id_to_idx_[image_id]]));
    } else {
      image.cam_from_world.rotation = Eigen::Quaterniond(AngleAxisToRotation(
          rotation_estimated_.segment(image_id_to_idx_[image_id], 3)));
    }
    // Restore the prior position (t = -Rc)
    image.cam_from_world.translation = image.cam_from_world.rotation * -center;
  }

    // Directly use the relative pose for estimation rotation
    auto center = images[curr].Center();
    const ImagePair& image_pair = view_graph.image_pairs.at(
        ImagePair::ImagePairToPairId(curr, parents[curr]));
    if (image_pair.image_id1 == curr) {
      // 1_R_w = 2_R_1^T * 2_R_w
      images[curr].cam_from_world.rotation =
          (Inverse(image_pair.cam2_from_cam1) *
           images[parents[curr]].cam_from_world)
              .rotation;
    } else {
      // 2_R_w = 2_R_1 * 1_R_w
      images[curr].cam_from_world.rotation =
          (image_pair.cam2_from_cam1 * images[parents[curr]].cam_from_world)
              .rotation;
    }
    images[curr].cam_from_world.translation =
        images[curr].cam_from_world.rotation * -center;
  }

[StonerLing]

That's a kind of solution, since I need to use pose prior in bundle adjustment, so it may be better to save this value as a member variable in Image class. Also the restoration of translation may not be done repeatedly, so write and call such a function in GlobalMapper may be better.

[StonerLing]

In my attempts, it seems that the global positioning step in glomap could not do well with pose position prior.

• If use the random position of image and 3d points generated in global positioning, in order to add GPS position cost function in BA, I use EstimateSim3dRobust to allign the reconstruction data with the pose prior data, but this alignment results in significant errors.
• If use fixed position of image in global positioning(set to GPS position), no matter generate the 3d points randomly or through triangulation(the rotation estimation's result could not support estimate a good triangulation in this step, the camera's orientation has a significant offset), the final result indicates that it could not get a normal model.

Has anyone made similar attempts?