Slam merge requestshttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests2024-03-27T13:32:31-04:00https://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/405[PV][feat] Initiate a toolbar with a preset manager dialog2024-03-27T13:32:31-04:00Timothée Coubletimothee.couble@kitware.com[PV][feat] Initiate a toolbar with a preset manager dialogThis MR adds some basic UI capabilities for the SLAM.
It adds a toolbar with icons: ![image](/uploads/be60cf235f520b8bd50fab04c9bddcc0/image.png) \
The list of new buttons:
- Create the SLAM Filter on the current source (usually on a L...This MR adds some basic UI capabilities for the SLAM.
It adds a toolbar with icons: ![image](/uploads/be60cf235f520b8bd50fab04c9bddcc0/image.png) \
The list of new buttons:
- Create the SLAM Filter on the current source (usually on a LidarReader)
- Open a Preset dialog:
* This dialog allow the user to fill the SLAM with standard preset for Environment and LiDAR models (e.g the user might want to use a `Velodyne` in a `City`).
* The buttons to right can be use to share custom preset (load / save / delete). Note the preset are saved in local config folder (`~/.config/Kitware/SlamPreset` on linux).
* It is important to note that User Custom preset are overriding **all** properties. (compared to environment and model presets) \
![image](/uploads/171794c774e9b3e905f3a6e4a02b0f41/image.png){width="390" height="278"}
- The next three button are respectively: `Initialization`, `Add External Sensor` and `Optimize Graph` dialogs. They are just re-grouping some properties in dialog. A nice next improvement would be to add more logic to the dialog to help user choosing / modify the right option. \
![image](/uploads/e5aef2d704063b82820af45f3a02c91e/image.png){width="380" height="268"} ![image](/uploads/6c0109c73dab270437e7d2a44200cab1/image.png){width="390" height="343"} ![image](/uploads/97dafa074661d131f002a9bf342b4efd/image.png){width="386" height="278"}
- The last button can be use to reset all SLAM maps
This MR also adds a dialog from menu `Help` to show SLAM version and all associated libraries version: ![image](/uploads/9831bfe8364f2821dc72320a394c31ab/image.png){width="329" height="349"}Timothée Coubletimothee.couble@kitware.comTimothée Coubletimothee.couble@kitware.comhttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/401Draft: [ROS2][feat] Add 2D features in DSSKE and filter edges2024-02-08T06:29:28-05:00Jeanne FaureDraft: [ROS2][feat] Add 2D features in DSSKE and filter edges# Goal
This MR follows the implementation of !324 : we added a keypoint extractor for dense LiDAR, in order to use their dense scan lines to extract 2D features and compare points with their top and bottom neighbors.
There are 2 major ...# Goal
This MR follows the implementation of !324 : we added a keypoint extractor for dense LiDAR, in order to use their dense scan lines to extract 2D features and compare points with their top and bottom neighbors.
There are 2 major axis of work :
1. **Extracting 2D features**, looking at what we call a "kernel", a 2D neighborhood. Here, we fitted a plane on the kernel of a point and tested two features.
2. **Filtering keypoints** after feature computing but before extraction, looking at the keypoints candidates in their neighborhood.
# Changes
### DenseSpinningSensorKeypointExtractor
We'll split our explanations between the 2 axis of work explained above.
#### 1) Extracting 2D features
* We first tried two types of kernel. The first commit leaves a trace of what could be a "square" kernel, but we didn't keep it because extracting a square kernel of more than 1 neighbor per direction was quite tricky. It would also take a lot of computation time.
\--\> So we kept a "cross" kernel, literally taking left, right, top and bottom neighbors under a kernel radius.
* We computed the normal of the plan formed by the kernel using a PCA.
* We tried two features to replace angles
* (a) Mean angle between the normal and the vectors formed by the central point and a point of the kernel.
* (b) Mean distance between the plane and the kernel points.
* Note : we avoid computing an angle or a distance for points too close for the central point, because we consider them as noise. For that, we defined a DistToNeighThreshold
* We filter the features using the values of the left neighborhood to keep only the most meaningful, only for edges.
Let's dig into (a) and (b) principles :
(a) : We define two new PtFeat : one **CosNormal** and one **SinNormal**. They stock the cos and sin values of the mean angle, and are respectively used for Plane and Edge extraction. We consider that the central point belongs to a plane if the cosine of the mean angle tends to 0, and it belongs to an edge if the sine of the mean angle tends to 0. We had to define two thresholds as we are locally filtering the minimal value for future edges extraction.
(b) : We define two features **PlaneDist** and **EdgeDist**, that contain the same value (but the EdgeDist will be locally filtered to only keep the maximal value for future edge extraction). We consider that if a mean distance to the kernel plane is small, the central point belongs to a plane. If it is big, the central point belongs to an edge.
#### 2) Filtering keypoints
* We used the cross kernel concept, but adapted it to only extract top and bottom neighborhoods as they were the ones we were wanting to look at to filter edges. But we could easily adapt to look at left and right also.
* We added a **hasEdge** feature in each part of the kernel to save a boolean indicating if the vector of PtFeat contains an edge candidate.
* We also look at top and bottom neighborhoods of direct left and right neighbors of the central point. So we check if there is any edge in the 3 columns around the central point (using the hasEdge feature), and if there is not, we remove it from keypoints candidates by adding a test in the `isPtValid()` check.
# Results
## Reproduce
### Data
I have used data of an Ouster Lidar with 128 lasers in an enclosed warehouse with what seems to be an office space.
### Build
`colcon build --base-paths slam/ros2_wrapping --cmake-args -DCMAKE_BUILD_TYPE=Release`
### Parameters
* Change the parameters :
* To set the kernel radius (similar to neighborhood radius):
_slam: ke: kernel_radius:_ \[m\], minimum length of a neighborhood.
* For commit with angles between normals and vectors formed by central point and a kernel point :
_slam: ke: dist_to_neighbor_threshold:_ \[m\] minimal distance between central point and a kernel point to consider that this point does not belong to the noi
_slam: ke: edge_sin_normal_threshold:_ \[m\] maximal sin value for the mean of the angles between the normal and a vector formed by central point and a kernel point to caracterize an edge
_slam: ke: plane_cos_normal_threshold:_ \[m\] maximal cos value for the mean of the angles between the normal and a vector formed by central point and a kernel point to caracterize an plane
* For commit with dist to plane :
_slam: ke: plane_pt_dist_thresh:_ \[m\], maximal distance between the plane fitted on the kernel and the mean distance of the kernel points to the plane for a central point to be considered a plane
_slam: ke: edge_pt_dist_thresh:_ \[m\], minimal distance between the plane fitted on the kernel and the mean distance of the kernel points to the plane for a central point to be considered an edge
### Run
`ros2 launch lidar_slam slam_ouster.py outdoor:=false replay:=true use_sim_time:=true`
`ros2 bag play --clock path/to/this_bag.bag --clock`
You can add the `-p --start-offset 10.5` to the bag play to control the arriving frame.
## Output
## Performances
* We wanted to fit a plane using the mean cross product of all vectors from the kernel, but this was costlier than using a PCA.
* As we optimized FitLineAndCheckConsistency in !324, it is going to be hard having better performances with the normals feature. However, we should determine if the results with normals have interest compared to 1D angles.
# Checklist
- [ ] Camel case everywhere except for ROS variables/parameters
- [ ] Lower case for local variables and lambda functions
- [ ] Upper case for members, methods and tool functions (in Utils)
- [ ] Precise namespace when calling a function (or this-\>X or classe.X)
- [ ] Align code (for multiline if and while, "&&" or "||" go in upper line to ensure alignement)
- [ ] Check your spaces
- [ ] between if, while, for and parenthesis
- [ ] between operators and variables: e.g. a + b
- [ ] after ","
- [ ] Mind your commit titles/desc (plurals, he/she + "s", correct tags, title should begin by a verb...)
- [ ] Function names should start with a verb, variable names should start with a name
- [ ] Macros should be between {}
- [ ] Do not use negative boolean (i.e. noJoe)
- [ ] Check minimal size of the types (double -\> float -\> int -\> uint)
- [ ] Check const and ref in functions arguments
- [ ] References should be written "type& name", not "type &name"
- [ ] Update documentation
- [ ] Add MR labels \[ROS\]/\[ROS2\]/\[PV\]
- [ ] If ros/ros2, update task table [here](https://gitlab.kitware.com/keu-computervision/slam/-/issues/55 "ROS2 parallel work")
- [ ] Add a comment over each non trivial function in header files
- [ ] Add a header to each new file
# What remains to solve
* [ ] Check which feature between CosNormal/SinNormal and PlaneDist/EdgeDist is the most relevant
* [ ] Decide which form of kernel is the best (cross, square)
* [ ] Improve keypoints filtering --\> add a filter for planes ? use left & right neighborhoods for edges ?
* [ ] Improve computation time
* [ ] Reflect on new 2D features to extractJeanne FaureJeanne Faurehttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/214[WIP] Integrate ScanContext for automatic loop detection2023-03-17T10:15:24-04:00Tong Fu[WIP] Integrate ScanContext for automatic loop detectionIntegrate [scan context code](https://github.com/irapkaist/scancontext) in SLAM library.
Two API functions in scan context code are used: `makeAndSaveScancontextAndKeys` and `detectLoopClosureID`.Integrate [scan context code](https://github.com/irapkaist/scancontext) in SLAM library.
Two API functions in scan context code are used: `makeAndSaveScancontextAndKeys` and `detectLoopClosureID`.Tong FuTong Fuhttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/211[feat] Add bundle adjustment in SLAM2023-12-15T09:11:10-05:00Tong Fu[feat] Add bundle adjustment in SLAM- This merge request is rebased on merge request #!218 (automatic detection of loop closure)
Bundle adjustment is a postprocess to improve the slam result. It considers two close frames have the similar feature so that they can form a l...- This merge request is rebased on merge request #!218 (automatic detection of loop closure)
Bundle adjustment is a postprocess to improve the slam result. It considers two close frames have the similar feature so that they can form a loop closure. The bundle adjustment is applied after slam and add loop closure constraints between two close frames on a defined frequency.
The changes are made for bundle adjustment:
- Refact **LoopClosureRegistration** function in SLAM
- Add BUNDLE_ADJUSTMENT into PGOConstraint
- If enable bundle adjustment of PGOConstraint, build kdtree of target submaps in bounding box of source submaps. Generally, bundle adjustment is between two close frames which the matching keypoints are not so far.
- Add bundle adjustment process in OptimizaGraph in SLAM
- Parameters for bundle adjustment
- **BAFrequency** to set the bundle adjustment frequency. e.g. BAFrequency = 4 means to launch a LoopClosureRegistration each 4 frames.
- **BAInterval** to set the interval between a query frame and a revisited frame.
- **BAStartFrameIdx** and **BAEndFrameIdx** to set the range where to apply bundle adjustment
- **BAParams** is a LoopClosure::Parameters type, which to tune loop closure parameters for bundle adjustment process.
- Add interface in PV wrapping
- Each interface correspond to a variable of bundle adjustment
- Use bundle adjustment constraint -> UsePGOConstraints[BUNDLE_ADJUSTMENT]
- Bundle adjustment frequency -> BAFrequency
- Interval size -> BAInterval
- Start frame index -> BAStartFrameIdx
- End frame index -> BAEndFrameIdx
- Query map start range -> BAParams.QueryMapStartRange
- Query map end range -> BAParams.QueryMapEndRange
- Revisited map start range -> BAParams.RevisitedMapStartRange
- Revisited map end range -> BAParams.RevisitedMapEndRangeTong FuTong Fuhttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/110[WIP] Multithreading2021-08-19T04:51:10-04:00Julia Sanchez[WIP] Multithreading* Goals:
* Be able to receive (to process?) external sensors data while applying Slam algorithm
* Be able to add a global optimization process (e.g. GPS or any other global pose info)
* Speed up process parallelizing publishing
*...* Goals:
* Be able to receive (to process?) external sensors data while applying Slam algorithm
* Be able to add a global optimization process (e.g. GPS or any other global pose info)
* Speed up process parallelizing publishing
* Changes:
* Trelative, Tprevious and Log** members are removed
* LogStates and PublishableList shared list members are added (homemade containers)
* All accesses to these lists are atomized
* LogCurrentFrameState extracts and gathers all info to publish and to log in shared lists
* All processes needing previous pose info refer now to log list
* Adapt wrappings to manage threads concurrencyhttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/104[WIP] Build and use a vertex map for keypoints extraction2024-01-31T08:20:39-05:00Nicolas Cadart[WIP] Build and use a vertex map for keypoints extractionCurrently, the keypoints extraction is based on the analysis of the laser neighborhood of each point. The scan lines are filled with points shot by the same laser, without any consideration of azimuth or the absence of firing return. Thi...Currently, the keypoints extraction is based on the analysis of the laser neighborhood of each point. The scan lines are filled with points shot by the same laser, without any consideration of azimuth or the absence of firing return. This lead to erroneous neighborhoods analysis.
With this MR, a vertex map (~ 2D range image) is built to store the points indices in a coherent manner between different scan lines or along azimuth value. This allows to define new rules or mechanisms to invalidate or select keypoints:
- The points which have no neighbor (left or right) are considered to lay on a depth gap boundary. This could be really helpful in very poor features environments with only a few laser returns, by detecting the edges of the scanned objects.
- The points which miss some neighbors are excluded from keypoints extraction.
NOTE: The vertex map filling and the automatic computation of angular resolution do not rely anymore on the prior that all points from the input pointcloud come in sorted order (increasing azimuth) in each scan line.
There are still some points to dicuss/improve:
- [ ] if spinning velocity is not perfectly constant or if the measurements are imprecise, we won't have a perfect 1 vertex = 1 point match, because two points will lie in the same voxel, and the next one will be empty. This is even more true when dealing with dual return.
- [ ] too many depth gaps are now detected, especially far isolated points. Maybe we could limit this by checking if a depth gap exists in the previous/next scanline at this azimuth value, or disable this criterion for too far points. I'm not sure how to properly deal with this, but this MR cannot be merged as it is now.
- [ ] should we keep the (maybe too costly) PCA computation and replace it by average line only : apparently, this extracts much more keypoints than necessary. Therefore, even if kp extraction is faster, matching and optimization are much slower.
@julia.sanchez @nick.laurensonNicolas CadartNicolas Cadarthttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/78Add radius search to KDTreePCLAdaptor object2021-03-05T03:23:43-05:00Julia SanchezAdd radius search to KDTreePCLAdaptor object* For now all searches in a kdtree are performed with knn number of neighbors (KdTreePCLAdaptor object).
* This MR aims at adding a function to do the search with a radius parameter.
* It does not change the actual SLAM process in any w...* For now all searches in a kdtree are performed with knn number of neighbors (KdTreePCLAdaptor object).
* This MR aims at adding a function to do the search with a radius parameter.
* It does not change the actual SLAM process in any way and does not add any option to the user.
* It is needed when using KdTreePCLAdaptor.h outside slam process.
@nicolas.cadart PTAL