Georeferencing GLIM maps

[VRichardJP]

I would like to georeference GLIM maps. There is already a gnss_global extension module which deals with GNSS data, but it does not fully satisfy my needs. In particular:

• The module uses Pose messages instead of NavSatFix, so GNSS sensor data must first be processed (e.g. with navsat_transform_node?). This is a bit inconvenient.
• The module computes the transformation T_world_utm only once, after the mapping device has travelled a few meter. So travelling longer distance won't improve the transformation (while new pose constraints are still added to the graph.)
• When the map size is not big (e.g. <1km) or when the path contains many loops, using poor GNSS data may be detrimental to the map. In other words, there are cases where I would rather not add any GNSS-based constraint to the graph (yet I would still like to georefence the map).

To address these issues, I am thinking about the following:

• Add support for NavSatFix messages. NavSatFix could be converted to UTM or MGRS with geographiclib. NavSatFix messages could be filtered based on fix status or position covariance.
• Use the first valid NavSatFix message to compute a GNSS origin and compute all submap_coords relatively to this origin point (thus avoiding errors on large floating point values).
• Add a enable_gnss_factor-like option. If enabled, use current logic to add pose factors to the graph (but use the GNSS origin instead of T_world_utm)
• Add a on_update_submaps callback, which receives submaps optimized by the main global mapping module and computes T_world_utm (using exactly the same logic than the current T_world_utm initialization). This T_world_utm transform is what is needed to georeference the map. There is no save callback yet, but I think it could be nice to dump the transformation data on save so that exporting a georeferenced map can be automated. Otherwise, the user could use tool like pdal to apply the transformation on the exported map manually.

[koide3]

Yeah, the gnss_global module is just a simple PoC implementation and may not cover a wide range of applications. It's better to create another new module rather than extending the existing gnss_global.

I think it would be beneficial if the new module has the following features (mostly based on your proposal).

• Accepts both NavSatFix (global WGS84 coord) and PoseStamped (local reference coord).
• Continually optimizes T_world_utm along with sensor states.
• Has some mechanism to validate GNSS data correctness.

There is no save callback yet, but I think it could be nice to dump the transformation data on save so that exporting a georeferenced map can be automated.

There is a callback for this purpose, and each extension module can save some information in the dump directory.

So, I think it is generally feasible to implement a module with the required functionalities. I wouldn't be able to work on this next moths because my bandwidth has been limited due to some assignments, but I can help you if you are willing to implement the new GNSS module.

[VRichardJP]

Thanks for the feedback.

I have started to implement a "georef_global" module, which filters NavSatFix messages, converts them to UTM frame, and adds interpolated pose/covariance as gtsam::PoseTranslationPrior<gtsam::Pose3> for each X(submap->id). This pretty much works the same as in gnss_global except that T_world_utm is now a simple translation (I want to enforce world = utm + xyz_offset with the GNSS data).

This kind of approach should work if this georef_global is the only module to add priors on the absolute position. However, global_mapping modules does add 2 constraints on the initial position:

• gtsam::PriorFactor<gtsam::Pose3>(X(0), gtsam::Pose3::Identity(), gtsam::noiseModel::Isotropic::Precision(6, 1e6)
• gtsam_points::LinearDampingFactor(X(0), 6, params.init_pose_damping_scale)

The first one sets the initial pose to 0. One simple trick I have used in the past is to use a high variance on initial x,y,z and yaw. Something like this:

Eigen::VectorXd init_noise(6);
//                 roll,     pitch,       yaw,       x,       y,         z
init_noise << 0.10*0.10, 0.10*0.10, M_PI*M_PI, 10.*10., 10.*10., 100.*100.;
model = gtsam::noiseModel::Diagonal::Variances(init_noise);
gtsam::PriorFactor<gtsam::Pose3>(X(0), gtsam::Pose3::Identity(), model)

However, I am not sure what gtsam_points::LinearDampingFactor is doing. What is its impact on the graph optimization?

There is a callback for this purpose, and each extension module can save some information in the dump directory.

Ah yes, I think it should do the job.

[VRichardJP]

I just realized there is a way easier method to implement that: instead of hacking my way around so GTSAM optimizes world = shifted UTM/MGRS, it is possible to add simply a new variable T(0) representing T_geo_world, and use GTSAM's expressions to constrains p_geo = T_geo_world * p_world.

Besides the NavSatFix message conversion, my code is basically gnss_global with the following changes:

  void on_smoother_update(gtsam_points::ISAM2Ext& isam2, gtsam::NonlinearFactorGraph& new_factors, gtsam::Values& new_values) {
    if (!isam2.valueExists(T(0))) {
      // insert T_geo_world variable
      new_values.insert(T(0), gtsam::Pose3::Identity());
    }

    static bool first_factors = true;
    if (first_factors) {
      if (output_factors_.size() < 10) {
        // wait until we have enough factors to prevent underconstrained system
        return;
      }
      first_factors = false;
    }

    const auto factors = output_factors_.get_all_and_clear();
    if (!factors.empty()) {
      logger_->debug("insert {} georeferencing prior factors", factors.size());
      new_factors.add(factors);
    }
  }

  void on_smoother_update_result(gtsam_points::ISAM2Ext& isam2) {
    T_geo_world_ = Eigen::Isometry3d(isam2.calculateEstimate<gtsam::Pose3>(T(0)).matrix());
    logger_->info("estimated T_geo_world={}", convert_to_string(T_geo_world_));
  }

  void backend_task() {
        // [...]

        // Measured position in geo frame
        const double tl = left->stamp;
        const double tr = right->stamp;
        const double p = (stamp - tl) / (tr - tl);
        const GNSSData interpolated = {
          .stamp = stamp,
          .pos = (1.0 - p) * left->pos + p * right->pos,
          .cov = (1.0 - p) * left->cov + p * right->cov,
        };

        // Estimated position in geo frame
        gtsam::Expression<gtsam::Pose3> T_geo_world(T(0));
        gtsam::Expression<gtsam::Pose3> world_pose(X(submap->id));
        auto geo_p = gtsam::translation(T_geo_world * world_pose);

        // Add GNSS measurement factor
        logger_->info("Add Georeferencing factor between submap={} and gnss={}", convert_to_string(submap->T_world_origin.translation().eval()), convert_to_string(interpolated.pos));

        auto noise_model = gtsam::noiseModel::Diagonal::Variances(interpolated.cov);
        gtsam::NonlinearFactor::shared_ptr factor (new
          gtsam::ExpressionFactor<gtsam::Point3>(
          noise_model, interpolated.pos, geo_p)
        );
        output_factors_.push_back(factor);

        // [...]
  }

It seems to work just fine:

[2025-02-28 02:25:19.233] [glim] [info] playback speed: 1.722x
[2025-02-28 02:25:20.889] [georef_global] [info] estimated T_geo_world=se3(75685.263253,31099.370525,6.779151,-0.007136,-0.007180,0.667517,0.744526)
[2025-02-28 02:25:21.280] [georef_global] [info] Add Georeferencing factor between submap=vec(-26.232009,-30.459571,-0.594140) and gnss=vec(75711.512453,31070.809436,6.703938)
[2025-02-28 02:25:22.180] [georef_global] [info] estimated T_geo_world=se3(75685.219081,31099.439874,6.773955,-0.007021,-0.007171,0.668136,0.743971)

I think these changes could be added to gnss_global and the module could be set to accept either Pose or NavSatFix messages (I don't have enough free time to propose a PR though).

Note that with this implementation, bad GNSS data (e.g. with underestimated error) may distord the map. One simple trick is to scale the GNSS measurement covariance. In such case georeferencing should still work the same, but submaps won't be shifted because of the GNSS data.

[koide3]

Thanks a lot for sharing your trial. I'll update the gnss_global module or create another one with the suggested ideas later.

[VRichardJP]

There are a few caveats with the implementation above. Notably, gtsam::Expression does not appear to be serializable (or rather I don't know how to do it), so the offline_viewer will complain about not being able to restore the graph.

So I have replaced the inline gtsam::Expression by a new gtsam_points::GeoRefFactor, which does exactly the same (but is serializable). Implementation is rather straight-forward:

  virtual gtsam::Vector evaluateError(
    const gtsam::Pose3& T_geo_world,
    const gtsam::Pose3& pose_world,
    boost::optional<gtsam::Matrix&> H1 = boost::none,
    boost::optional<gtsam::Matrix&> H2 = boost::none) const override {

    // pose_geo = T_geo_world * pose_world
    gtsam::Matrix66 H_transform, H_pose;
    gtsam::Pose3 pose_geo = T_geo_world.compose(pose_world,
      H1 ? &H_transform : nullptr,
      H2 ? &H_pose : nullptr);

    // extract translation part
    gtsam::Matrix36 H_translation;
    gtsam::Point3 p_geo = pose_geo.translation(
      H1 || H2 ? &H_translation : nullptr);

    if (H1) {
      *H1 = H_translation * H_transform;
    }

    if (H2) {
      *H2 = H_translation * H_pose;
    }

    gtsam::Vector3 error = p_geo - p_geo_meas_;

    return error;
  }

Used like so:

        auto noise_model = gtsam::noiseModel::Diagonal::Variances(interpolated.cov);
        // Use ad-hoc GeoRefFactor to constrain `T_geo_world * p_world = p_geo`
        gtsam::NonlinearFactor::shared_ptr factor (new
          gtsam_points::GeoRefFactor(
          T(0), X(submap->id), interpolated.pos, noise_model)
        );
        output_factors_.push_back(factor);

There are a couple other modifications I had to make on GLIM side, such adding T key remapping, and adding a few export_geo functions to add the possibility to export georeferenced map from the offline_viewer.

One last problem I have yet to solve is to find what to do when multiple georeferenced maps are loaded: each has its own local world frame and own T_geo_world, but after the two maps are aligned I am not sure if it is still possible to use any of the T_geo_world to georeference the map ("moving" the local world frame basically invalidate the transform).

[Nasekoma]

@VRichardJP, could you please publish your changes to your public forks of the mentioned repositories?

[laurence-diack-pk]

@VRichardJP, could you please publish your changes to your public forks of the mentioned repositories?

Here's one I've been working on based on the above comments: https://github.com/laurence-diack-pk/glim_ext/tree/_LD_georef

Still haven't figured out the merging with multiple independently referenced maps though..

[VRichardJP]

Merging multiple georeferenced maps can be done by transforming all point clouds into the same geo frame.

What GLIM currently does is:

• Load each map in its own local frame (world_1...world_N)
• Merging is done by aligning world_i and world_j.

Instead, you can do the following:

• Use each session T(0) (renamed T(0)...T(N)) to transform the map points into the global geo frame (to increase precision, calculate a global offset on first geo referenced map and use shifted coordinates)
• Now all maps are already in the same geo frame, so the maps should be already almost aligned

[gugelhop]

@laurence-diack-pk @VRichardJP I tried your modification and it seems to be working. In the log, I could see that the georef constraints were added. However, I could not figure out how to export a georeferenced map to use with my vector map. Could you please guide me through this step?

[Nasekoma]

@laurence-diack-pk I've been testing your extension and have found a few bugs in it:

1. Georeference uses keys based on values made with uppercase letters, but glim uses gtsam::symbol_shorthand functions, which use lowercase letters. So, in function Georeference::on_smoother_update all factors are always skipped because of the submap pose key

2. In the same function Georeference::on_smoother_update new factors can be based on new values that will be added in ISAM2 only after this function call. So, checking if all keys exist in the optimizer should also check if keys exist in the new_values

        ...
        for (const auto& key : factor->keys()) {
            if (!isam2.valueExists(key) && !new_values.exists(key)) {
        ...

3. To export GeoPositionFactor serialize function needs to be written and exported. I've used pattern from the flat_earther extension and RotateVector3Factor from gtsam_points

// in GeoPositionFactor
...
private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/)
  {
    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
    ar & boost::serialization::make_nvp("NoiseModelFactor2",
            boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(measurement_);
  }
...
// exporting in cpp file
BOOST_CLASS_EXPORT_GUID(glim::GeoPositionFactor, "glim::GeoPositionFactor");

4. I suspect using ECEF makes T_geo_world too unstable because of big values -- gtsam::IndeterminantLinearSystemException is thrown almost immediately. I changed ECEF to local Cartesian and added LinearDampingFactor for T_geo_world. This helped in my case, but the resulting map is much worse than without GNSS data (probably needs better GNSS data filtration and other parameters).

[bespectacledengineer]

@laurence-diack-pk I've been testing your extension and have found a few bugs in it:

1. Georeference uses keys based on values made with uppercase letters, but glim uses gtsam::symbol_shorthand functions, which use lowercase letters. So, in function Georeference::on_smoother_update all factors are always skipped because of the submap pose key

2. In the same function Georeference::on_smoother_update new factors can be based on new values that will be added in ISAM2 only after this function call. So, checking if all keys exist in the optimizer should also check if keys exist in the new_values

    ...
    for (const auto& key : factor->keys()) {
        if (!isam2.valueExists(key) && !new_values.exists(key)) {
    ...
   

3. To export GeoPositionFactor serialize function needs to be written and exported. I've used pattern from the flat_earther extension and RotateVector3Factor from gtsam_points

// in GeoPositionFactor ... private: friend class boost::serialization::access; template<class ARCHIVE> void serialize(ARCHIVE & ar, const unsigned int /version/) { // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object<Base>(*this)); ar & BOOST_SERIALIZATION_NVP(measurement_); } ... // exporting in cpp file BOOST_CLASS_EXPORT_GUID(glim::GeoPositionFactor, "glim::GeoPositionFactor"); 4. I suspect using ECEF makes T_geo_world too unstable because of big values -- gtsam::IndeterminantLinearSystemException is thrown almost immediately. I changed ECEF to local Cartesian and added LinearDampingFactor for T_geo_world. This helped in my case, but the resulting map is much worse than without GNSS data (probably needs better GNSS data filtration and other parameters).

I implemented all of your bug fixes, and although the code runs, the output is still in the local slam frame. What is worse is that even with the damping factor, the optimizer runs into a undeterminant linear system error later in the mapping process.

@Nasekoma did you get @laurence-diack-pk's implementation to work?

I personally found another bug which prevented the initialize_transform function from being called. Essentially there is a condition that initializes the transformation between T_world_local when the submap id is greater than equal to 2. The variable being used to keep count of the submap id is not being updated in the code, and the if-block is never entered.

I would really appreciate if someone shares a working implementation with the community. It would really help.