Modify poses

[adthoms]

@nickcharron Ideally I would have made a bunch of small PRs for each of the following items but of course there isn't much time to coordinate that. Any questions we can have a quick meeting.

This pull request does the following:

• reduce code duplication for load/write functionality.
• refactor load/write interface using format_type flags. This flag indicates a particular formatting as it applies to a certain extension. Documentation has been updated to indicate the expected per-line formatting for each value of format_type, which is an enum class
• refactor existing PosesTest. In a future pull request, tests will be made for Load/Write from/to bag and PCD.
• add functionality for containerizing sensor and scan data. This feature is useful for 3d_map_builder when performing manual calibration
• optionally save ouput when building map. This feature is necessary for the manual calibration tool I made in 3d_map_builder, as we simply want to visualize the built map in rviz and not have it (and generated poses) written to disk.
• add functionality to read high-rate poses from an odometry message
• fix a bug in LoadLoopClosedPathsInterpolated that now interpolates a high-rate pose at time of loop-closed pose to better estimate the corrected transform at time of loop-closed pose.
• create a simple interface for converting between RPY Euler angles (in Degrees and Radians) to Quaternions. I chose to fix the order of RPY to keep things simple.

[nickcharron]

this needs to be more clear. Use explicit names that we can understand:

enum pose_format_type {TXT, PLY, JSON};

[nickcharron]

oh I just realized this is:

• Type1 Format: timestamp T_WORLD_SENSOR, where T_WORLD_SENSOR is in row major format
• Type2 Format: timestamp tx ty tz qx qy qz qw

What's type 3?

you could use: pose_format_type{SE3, QUAT} but also add this description here, not in the functions below (or both)

[adthoms]

No file extension currently has a type 3, I will remove this constant for now. The trouble with coming up with descriptive names for the enum class is that each file extension has a unique type of formatting, so I just went with integer values as this generalizes well.

[nickcharron]

right but most file formats have a single format right? The only difference is the one that you can specify SE3 or Quaternion right?

[adthoms]

currently PLY and TXT have two formatting types. PCD may also benefit from two formatting types.

[nickcharron]

add description here. What is the string? Sensor frame id?

[nickcharron]

or make this a struct to be more clear (as shown above)

[adthoms]

added description for all data structures. Will improve format of data structures in future pull request.

[nickcharron]

this was my poor choice in design, and it might be too much work for you to change so you can ignore this if you'd like. But ideally, we should change the way we store timestamps and poses. It shouldn't be two separate vectors, it should be a map from timestamp to its associated pose.

The reason I hadn't changed this is because changing that would break a lot of code if we just change from two vectors to one map. However, a way to make this change without breaking anything would be to store in internally as a map, but keep the functions to output each vector, and when those vectors are requested we just compute them at the spot.

This would essentially stop the possibility of having poses and stamps be mis-aligned. But I'll leave it up to you if you want to change that or not. It's okayt he way it is

[adthoms]

Agreed. This should go into a future pull request. Unfortunately I don't have time now with me having to get a paper out in 6 weeks lol

[nickcharron]

no worries

[nickcharron]

this seems like a bad choice in data structure. You're just grouping two vectors including one of poses and one of pointclouds. I assume element i of the poses should correspond to element i of the scans. If that's the case then this isn't a good data structure choice.

better options IMO would be:

struct ScanPose {
Eigen::Matrix4d T_world_baselink;
PointCloud::Ptr cloud_in_baselink (is that correct?)
}
using scan_data_type = std::set<ScanPose>

2. using scan_data_type =
   std::set<std::pair<Eigen::Matrix4d, PointCloud::Ptr>>;

This is a bit of a primitive obsession code smell though. Hides a lot of the details just so that we can use a standard struct

[adthoms]

I like this

struct ScanPose {
Eigen::Matrix4d T_WORLD_LIDAR;
PointCloud::Ptr cloud_in_LIDAR
}
using scan_data_type = std::set<ScanPose>

I'll go with it

[nickcharron]

We actually already have a ScanPose struct in beam_slam: https://github.com/BEAMRobotics/beam_slam/blob/main/bs_models/include/bs_models/lidar/scan_pose.h

But it's a large class. Not sure if it makes sense to port that over to here, or keep this more simple. Only issue I see is double naming might cause confusion. Maybe call this ScanAndPose just so that the name is different? Actually I might liek that better because it's more clean what it is

[adthoms]

The reason for the current choice of data structure follows from the current design of the code. As you mentioned, the current way of storing timestamps, poses, and scans lead me to this design choice: I will create a seperate issue where this is improved on.

[adthoms]

renamed scan_data_type to pose_and_scan_data_type. Naming convention follows your style of naming in beam_mapping. I will flag 'fix naming conventions' as an item in a future pull request.

[adthoms]

@nickcharron I would like to merge what is currently in this PR and make improvements in a future PR as marked by this issue. I have addressed all comments with the exception of improving the current formatting/use of data structures. If/when the IEEE paper is accepted somewhere than this would be a good time to finalize the MapBuilder and supporting code in libbeam.