Slam merge requestshttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests2021-08-19T04:51:10-04:00https://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/137Scale covariance with geometric meaningful weight2021-10-27T11:25:14-04:00Julia SanchezScale covariance with geometric meaningful weight**Recall**:
* The final covariance is C = (J.t S^(-1) J)^(−1) where J is the Jacobian of the cost function and S is the covariance of all residuals (see http://ceres-solver.org/nnls_covariance.html for more info). So, the covariance ref...**Recall**:
* The final covariance is C = (J.t S^(-1) J)^(−1) where J is the Jacobian of the cost function and S is the covariance of all residuals (see http://ceres-solver.org/nnls_covariance.html for more info). So, the covariance reflects the models (planes/lines) uncertainties and the final fitting error of the ICP like algorithm.
**Changes** :
* For now, the residuals are weighted with a custom weight fitQualityWeight = 1 - RMSE/UserParam
* This leads to a biased covariance scale
* To make the covariance output more reliable in this MR, the residuals are weighted with the target models' variances information extracted from neighborhoods
* The MaxLineDistance and MaxPlaneDistance parameters are changed (from 0.2 to 0.1) to obtain as good results as before
**Results** :
* The resulting covariance is much smaller.
- *Explication on covariances* : the covariances of the models and the final result covariance are directly linked (C = (J.t S^(-1) J)^(−1) : if the first one decrease, the final one decreases).
- *Explication on weights* : the weight induces by the covariance on one residual is bigger than the actual weight of current master custom version. Bigger weight -> bigger Jacobian -> bigger J^T*J -> smaller cov. This behavior makes sense if the convergence took place : the less gradient there is, the less localized the system seems to be. However this covariance does not reflect the final quality of the residuals relatively to their model : if the models are very accurate (small covariance) and the points are very far from them at convergence, the covariance does not capture it.
* The new weights lead to more heterogeneity in residuals relatively to model accuracy.
* It improves some parts and worsens some others.
* The processing time and the keypoint matches number are kept approximately the same as master's.https://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/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/276WIP : [ROS][refact] Switch from msg command to services2023-07-25T13:21:57-04:00Julia SanchezWIP : [ROS][refact] Switch from msg command to services* Messages/topics should be more used for sensor data
* Services are more adapted to punctual actions like commands
- [ ] Reset
- [ ] Mapping mode
- [ ] Save trajectory
- [x] Save maps
- [ ] Load map
- [ ] Optimize graph
- [ ] Switch on...* Messages/topics should be more used for sensor data
* Services are more adapted to punctual actions like commands
- [ ] Reset
- [ ] Mapping mode
- [ ] Save trajectory
- [x] Save maps
- [ ] Load map
- [ ] Optimize graph
- [ ] Switch on/off + switch on/off sensors
- [ ] Calibrationhttps://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/405[PV][feat] Initiate a toolbar with a preset manager dialog2024-04-05T07:15:35-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/0001c1147d51c7da5c7d2a463b5020b8/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.com