There is code in relative_pose_transaction_base.h that is specific to imu, and we would like to move this to a derived class.

Who: Alex
What: We would like to refactor the ExtrinsicsLookup class to a singleton design pattern. This will reduce code duplication as we are currently passing the same ExtrinsicsLookup class to multiple sensor models as a shared pointer in the following files:

1. global_mapper (.cpp)
2. global_map (.h and .cpp)
3. sub_map (.h and .cpp)

Once refactored, the ExtrinsicsLookup class should also include:

// get extrinsics params from global namespace ros::param::get("~imu_frame", imu_frame); ros::param::get("~lidar_frame", lidar_frame); ros::param::get("~camera_frame", camera_frame); ros::param::get("~static_extrinsics", static_extrinsics); ros::param::get("~intrinsics_path", intrinsics_path);

which is currently in global_mapper_params.h . By moving these global parameters into the ExtrinsicsLookup class, we may load these parameters for any sensor model to lookup as all sensor models will require the instance of the singleton ExtrinsicsLookup class.

Implement a method of matching the current keyframe to the active submap rather than directly triangulating points, these constraints should be constant

The submap message will publish the visual word for the landmark, but the local mapper doesnt have access to the vocabulary so it will have to match based on distance alone

Add the ability for lidar odometry to perform without a frame initializer.

Proposal: initial pose estimate will be from a constant velocity assumption using the last two scanposes

An onGraphUpdate() function must be implemented in @jakemclaughlin6 VIO class to update orientation, position, and velocity when the biases change. This should make use of SlamTool's preintegrators class, which is currently passed as a shared pointer via the RelativeImuState3DStampedConstraint constraints.

Matching against a submap that has been provided by the global mapper

1. When attempting to triangulate a feature in the tracker, first check if there is a good match in the submap and use its position instead as a constant landmark
2. To do this, project all submap points into the image using its estimated pose (imu + pose refinement) to create a new vector of keypoints and descriptors, then directly match this against the current images keypoints and descriptors with a bruteforce matcher
3. The match criteria should be very strict (include keypoint distance to reject outliers)

Inputs:

• beam_cv::Tracker tracker
• std::vector<Vector3d> map_points
• std::vector<cv::Mat> map_point_descriptors
• std::vector<uint64_t> current_frame_landmarks (contains descriptor and pixel through the tracker)
• Matrix4d T_WORLD_CURRENTFRAME
• CameraModel cam_model

Ouput:

• std::vector<opt<Vector3d>> matched_map_points (same size as current_frame_landmarks, no match = no optional value)

We need to implement a way to send data from the global mapper to local mapper. It'll probably just be through a custom ROS message (or messages) which the sensor models will listen to.

Currently, the local mapper doesn't even use global map data, so we need also need to implement that.

We want to add constraints between camera keypoints and lidar maps.

We can most likely use the singleton LidarMap class in beam_slam_common, but we will likely need a way to pass the keypoints from the VIO to this sensor model. We can simply making a topic that the VIO publishes to, then this sensor model will just subscribe to that topic and also get an instance of the lidar map. We could investigate other ways to do this without having to publish the keypoints, (such as using the graph) but it also likely won't be much overhead and the lidar maps lag behind the vision anyways so it's not bad if there's a bit of a delay.

This was my idea for doing the correspondence search:

• For each keyframe, do:
  • For each scan in LidarMap, do:
    • For each point in scan, do:
      • Project point into camera frame
      • Store point if it lands within FOV, and its associated timestamp
  • For each keypoint, do:
    • Find nearest 3 neighbors in lidar map
    • Consistency check: check that there's no occlusion issues (see LVI SAM or SuperOdometry - can't remember which one)
    • Take closest point as correspondence
    • Take 3 points as corresponding plane
    • Add point to plane constraint, or optionally point to point, or point to line

There is a bug in calculating the covariance within the preintegrator class. For some reason, nan numbers begin to populate the covariance matrix when process noise is concidered for imu measurements. First attempt at a fix will be to swap out the calculation of the covariance with orbslam3.

Currently, matching against the current sliding window of features doesn't result in many matches.

This could be because we are using the SSC detector or because we aren't detecting enough features. The fix to this would be:

detect and track more features
This could also just be because the sliding window isn't big enough to match against. The solution to this would be:

store marginalized landmarks in a seperate map and match against these (same as existing logic and we would store the fuse landmark variables directly)
would need to add constraints for fixed landmark values
every once in a while, the map would need to be culled so that it isn't ever growing (i.e. remove landmarks that are older than X)

The local mapper will be receiving global map data to help with slam. There are two cases that both need some reloc requests to be sent to the global mapper.

1. Currently mapping in existing submap. Then we need to keep sending reloc requests to know when we have moved to a new submap
2. Currently mapping in unexplored areas. Then we need to keep sending reloc requests to know when we have entered a submap

These should have the exact same implementation from the local mapper side of things, but may need to be separate when the global mapper gets this. Possibly consider implementing some state (e.g., in existing map, or in unexplored area) to know how to deal with the reloc requests. But all reloc request should trigger a reloc.

We currently have two imu sensor models:
imu_3d requires a frame initializer and is mainly a test-case model. Inertial_odometry requires initialization like vio.

The goal of this issue is to merge these into one sensor model, where a frame initializer can be used if it is supplied, otherwise it will attempt to initialize off a slam initializer

Implement a frame initializer similar to odometry frame initializer, but with manual adding of poses

1. Add a flag to position3d determining if it should be held constant or not.
2. Implement a ConstVisualConstraint which adds a constraint to a landmark but does not pass the variable in itself, just its constant values (Eigen::Vector3d), this keeps the landmark still while just optimizing the pose

Problem Description

Currently, we build a lidar map (as a singleton) for two purposes:

1. To perform scan-to-map registration where I incrementally build a map from registration results and use that map to register each new scan.
2. For finding lidar-camera correspondences for the tight coupling.

It is generated by either ScanToMapScanRegistration, or ScanToScanRegistration (where it averages each new poses with all measurements to previous scans). Currently we do not update the map with pose updates.

Implementation

This needs to be implemented in two spots:

1. The LoInitializer builds a map (which is poor quality because it's built with no motion estimation so the registration doesn't converge as well). It currently just resets the map after LoInitialization. Since we later refine the trajectory in the VIO initializer, we should keep that map and update the poses with the VIO initializer results. We will have to interpolate for the new lidar poses though since they are not yet aligned with the camera poses.
2. ScanMatcher3D and CameraLidarCoupling both use the lidar map throughout the regular slam. Either of these could update the map from the onGraphUpdate function. I'd suggest doing it inside ScanMatcher3D since that will always be running, whereas CameraLidarCoupling may not.

Time analysis

The LidarMap is currently implemented in a way that all points are transformed to the world frame as soon as they are added to the map. This makes it fast for getting the full map for registration or camera keypoint correspondence search. We expect the same scan in a map to be used in the world frame many times so it makes sense to transform it right away and not when queried. However, if we are constantly updating the scan poses from the graph, it may be more computationally expensive to do it this way. The alternative is to keep all points in their respective lidar frames, then only transforming them to the world frame when needed, so it would be fast to update scan poses. We should do some analysis to test this.

Our current map implementatiion is not efficient for doing 3D searches. Since we have to constantly add and remove points from our map, we'd have to also rebuild a kd tree or octree for searching (e.g., for scan matching).

To speed things up, we can implement something similar to SuperOdometry where they have a hashed voxel map and each voxel contains its own search treer (octree in that case). This way, anytime you add or remove from the map, you only have to rebuild the search tree for the voxels that it changes.

To make full use of this, we'd also have to rewrite all the scan matching algorithms in beam_matching because they currently use pcl. This would take time. We could potentially implement a derived correspondence search in pcl then reimplement their matchers (icp, ndt, gicp, ...) with our new correspondence search method.

• Reset callback implemented
• SLAM initialization can reset properly
• Update lidar path init to have a clear function
• VO can reset + detects when to reset
• IO can reset + detects when to reset
• LO can reset + detects when to reset (@nickcharron)

Reprojection constraints can skew the optimization by a lot. The goal of this is to normalize each images reproejction constraints to be in line with what would be its odometry constraint (relative pose)

1. compute shannon entropy (H) from the pose covariance
2. Divide H by # of features in the image to get H per reprojection constraint

This is a minor issue to resolve the enforcement of noise parameters passed to the ImuPreintegration class. For common data sets such as Euroc and TUM, intrinsic properties of the imu have been calibrated and should be passed to the ImuPreintegration class. These paramets should be passed as type double in the YAML file.

The current visual path init produces a path in a bad frame and the gravity estimate is wrong as a result

Right now, the lidar odometry stores all active scans in a list, and then passes them as they come in to the scan matcher class. For the case of the scan to map, this won't have any duplicate data because the registration class doesn't need to store to scan pose. For the case of the scan to scan registration class, it actually copies the scan poses and saves them in a list of reference scans which means this data is duplicated (and it can be a lot of data, depending on our lag duration and number of neighbours).

If we store shared pointers to the scan poses and pass those to the scan registration (and it stores the pointers and not the data) then it will eliminate data duplication.

CAUTION: the lidar odometry will be updating the pose of the ScanPoses which will affect the scan registration. We need to make sure this doesn't cause any adverse effects.

Online calibration causes odometry to fail pretty frequently. Steps to debug this:

1. log the calibrations on graph updates, identify if the calibration gets skewed a lot when odom gets out of wack
2. print how many constraints are connected to the calibrations
3. maybe increase the priors on the calibrations, or we need to add more priors to them

We would like to test the functionality of RegisterNewImuPreintegratedFactor() when optional inputs R_WORLD_IMU and t_WORLD_IMU are passed. A unit test will be made where 1) ground truth poses are perturbed at times when imu preintegrated factors are registered 2) perturbed poses are passed to RegisterNewImuPreintegratedFactor() 3) a graph containing associated imu states is optimized 4) optimized results are checked against expected results. We should expect that, when poses are perturbed, the optimized pose will be averaged between the perturbed pose and the pose obtained from imu preintegration.

We will eventually need to implement a method for saving and loading map data to and from the global mapper.

There's two reasons for this:

1. Maps might get too large and not fit in RAM
2. If we want to use a previously built map for SLAM/Reloc, then we need to load previous maps into the global mapper so that it can send that data to the local mapper

Currently, only the ScanToMapLoam registration populates the LidarMap singleton.

This LidarMap singleton will be used by the Camera-Lidar constraint sensor model. If we want the option of using MultiScanRegistration instead of ScanToMapRegistration with the Camera-Lidar constraints, then we'll need to make sure MultiScanRegistration also populates the map, even if it's not using it for registration.

The global mapper was implemented to receive SlamChunkMsg ROS messages (https://github.com/BEAMRobotics/beam_slam/tree/main/global_mapping/msg) and add them to the global map.

The local mapper still does not send its data to the global mapper using those ROS messages. This should occur once the local mapper factors out that data (or some other chosen time, e.g., maybe when the covriance stops changing).

Currently, matching against the current sliding window of features doesn't result in many matches.

This could be because we are using the SSC detector or because we aren't detecting enough features. The fix to this would be:

• detect and track more features

This could also just be because the sliding window isn't big enough to match against. The solution to this would be:

• store marginalized landmarks in a seperate map and match against these (same as existing logic and we would store the fuse landmark variables directly)
• would need to add constraints for fixed landmark values
• every once in a while, the map would need to be culled so that it isn't ever growing (i.e. remove landmarks that are older than X)

Create a seperate sensor model that adds tightly coupled visual-lidar constraints

In this issue, we would like to:

1. make the PoseLookup class a singleton
2. include the ExtrinsicLookup singleton class in the PoseLookup class.
3. clean-up code duplication

Use this: https://github.com/dzunigan/imu_initialization
To estimate accel bias