introlab
Loop closures accepted depiste not enough inliers
Hi,
I noticed a big angle error on my graph after detecting more loops, and tried to look for some potential bad loops. I realized that several loops had been accepted despited the number of inliers for PnP being way less than the MinInliers parameters I have set.
The following is a sreenshot of a loop which has been accepted during a "Detect More Loop Closures" round, and that I cannot refine as the number of inliers is not high enough:
Note that I have not been playing with the MinInliers parameters for that database. If this is not a bug, I would love to know what could lead to this kind of situation. Also, I would love to know if there is a way of automatically getting rid of those loops that don't meet the Inliers>MinInliers condition, while keeping the existing good loops (asking because my database is >230gb and hundreds of thousands of good loops have been registered on it).
Thanks a lot Pierre
For the actual warning you have, it is because PnPRansac doesn't give deterministic results. When the number of inliers is low, it could happen that one "refine" is accepted while the next one is not. Not also that if Core Parameters -> Vis -> CorGuessWinSize is not null, local feature matching won't give the same matches every time you click on Refine (if accepted and the transform changed slightly).
If you used rtabmap-databaseViewer to add more loop closures, there is an option here (checked by default) that will use current optimized graph as guess to compute new loop closures.
If Core Parameters -> Vis -> CorGuessWinSize is not 0, it will only do local feature matching between the two images. It means that if the guess is wrong, it could generate bad matches, thus bad loop closures.
To avoid the issue you have, uncheck "Use optimized graph as guess" before detecting more loop closures. This will force a global feature matching between the corresponding images, resulting in more accurate matches but less matches. You can also increase the number of minimum inliers to accept a loop closure (Core Parameters -> Vis -> MinInliers), and/or decrease max optimization error (Core Parameters -> RGBD-> OptimizeMaxError) to reject bad loop closures if the graph deforms too much.
We don't save statistics about each loop closure added, so we cannot know which loop closure has been added with more or less inliers. Unfortunately, you would need to remove all "User" loop closures from the database and redo the "Detect More Loop Closures" with tuned options above. I guess you could use the stricter parameters to start with, setting high Vis/MinInliers will improve the process speed.
To remove "User" from the existing database, to avoid reprocessing or duplicating it, you can do:
# Show User links:
sqlite3 rtabmap.db "select from_id, to_id, type from Link where type = 4"
# Delete User links:
sqlite3 rtabmap.db "Delete from Link where type = 4"
Hi Mathieu,
Thank you for the information, I will untick that box from now on. But I was using Vis/CorGuessWinSize=0 anyway so I'm not sure this will change anything -- let's see.
For anyone that might find it useful, below is a python script which attempts to filter links that do not have enough inliers. It simply uses cv2 and there is no variance filtering like RTABMap does. It also doesn't recompute matchings, uses those that are stored instead. In practice it removes more than 90% of the links (on a ~10db database on which I detected more loops many times), I can't really explain that high number.
I'm taking this opportunity to ask a question: the concept of link refinement implies that links can be added without matching all the keypoints of the two pictures. Even with Vis/CorGuessWinSize=0; many links benefit a lot from refinements. So, how are those links created in the first place ? Some user links barely have any registered matches, so they must have been added without using the matcher somehow. Note that for example, many links that are kept by my script because they have enough potential inliers for PnPRANSAC, actually look quite "bad" in the databaseviewer as there are only a handful of those lightblue inlier matches. Refining them allows for a rematching and then a good transform estimation with enough inliers. Is there a way to impose that links are always computed from the full macthing set ? I am guessing that the "drifting geometry" of my scan is due to all those links which have been calculated from propagating good ones without recomputing them, somehow propagating errors ?
import cv2
import numpy as np
import shutil
import os
import struct
from tqdm import tqdm
MIN_INLIERS = 25
def parse_calibration(calib_blob):
header = struct.unpack_from('<11i', calib_blob, 0)
if header[3] != 0:
raise ValueError("Unsupported camera type, expected mono (0)")
if header[6] != 9:
raise ValueError("Unexpected K size, expected 9")
K_values = struct.unpack_from('<9d', calib_blob, 44)
return np.array(K_values, dtype=np.float64).reshape((3,3))
def get_features(cursor, node_id):
cursor.execute("SELECT word_id, pos_x, pos_y, depth_x, depth_y, depth_z FROM Feature WHERE node_id=?", (node_id,))
features = cursor.fetchall()
keypoints = {}
points3D = {}
for word_id, x, y, dx, dy, dz in features:
keypoints[word_id] = (x, y)
if dx != 0.0 or dy != 0.0 or dz != 0.0:
points3D[word_id] = (dx, dy, dz)
return keypoints, points3D
def get_camera_intrinsics(cursor, node_id):
cursor.execute("SELECT calibration FROM Data WHERE id=?", (node_id,))
row = cursor.fetchone()
if not row or not row[0]:
return None
return parse_calibration(row[0])
def run_pnp_ransac(points3D, points2D, K):
if len(points3D) < 4:
return 0
obj_pts = np.array(points3D, dtype=np.float32)
img_pts = np.array(points2D, dtype=np.float32)
success, _, _, inliers = cv2.solvePnPRansac(
obj_pts, img_pts, K, None,
flags=cv2.SOLVEPNP_ITERATIVE,
reprojectionError=2.0,
iterationsCount=100,
confidence=0.99
)
return len(inliers) if inliers is not None else 0
def main(input_db_path, output_db_path):
if os.path.exists(output_db_path):
os.remove(output_db_path)
shutil.copyfile(input_db_path, output_db_path)
conn = sqlite3.connect(output_db_path)
cursor = conn.cursor()
cursor.execute("SELECT from_id, to_id FROM Link WHERE type=4")
user_links = cursor.fetchall()
to_remove = []
for from_id, to_id in tqdm(user_links, desc="Filtering user links"):
kp_from, pts3D = get_features(cursor, from_id)
kp_to, _ = get_features(cursor, to_id)
common_words = set(kp_from.keys()) & set(kp_to.keys())
if len(common_words) < 4:
to_remove.append((from_id, to_id))
continue
pts3D_list = []
pts2D_list = []
for wid in common_words:
if wid in pts3D and wid in kp_to and all(a is not None for a in pts3D[wid]):
pts3D_list.append(pts3D[wid])
pts2D_list.append(kp_to[wid])
if len(pts3D_list) < 4:
continue
K = get_camera_intrinsics(cursor, to_id)
if K is None:
continue
inliers = run_pnp_ransac(pts3D_list, pts2D_list, K)
if inliers < MIN_INLIERS:
to_remove.append((from_id, to_id))
print(f"{len(to_remove)} links will be removed (inliers < {MIN_INLIERS})")
for from_id, to_id in to_remove:
cursor.execute("DELETE FROM Link WHERE from_id=? AND to_id=? AND type=4", (from_id, to_id))
conn.commit()
conn.close()
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("input_db", help="Path to input RTAB-Map database")
parser.add_argument("output_db", help="Path to output filtered database")
args = parser.parse_args()
main(args.input_db, args.output_db)```
The word_id refers to ID used in the visual dictionary. Without re-matching features (with or without Vis/CorGuessWinSize), you won't get a lot of inliers. We always rematch features before estimating a transformation between two frames, to get more inliers. Note that the only time we don't recompute the visual correspondences is when we are using Reg/Strategy=1 (Lidar mode), where we use the already computed IDs from the dictionary (e.g., word_ids) to find the correspondences between two frames (in this mode we don't expect many visual inliers or a very accurate visual transformation, we just need a "good enough" guess for ICP).
In particular with binary features, using only word_ids for matching, I am not surprised that 90% of the user loop closures didn't past the test.
"Refine" option always re-computes the correspondences between the frames. With Vis/CorGuessWinSize=0, it does global feature matching between the frames. With Vis/CorGuessWinSize>0, it does local feature matching, starting from the current loop closure transform. Note that in pure visual SLAM, you would not generally get better or worst transformations by "refining" in post-processing. It is still using RANSAC, so the transformations would be similar if the same correspondences are selected by RANSAC.
Note also that in rtabmap-databaseViewer, don't be fooled by the matches shown when changing the main sliders, these correspondences are shown using word_ids only. When you hit Refine or Add constraint, it will show the actual re-matching of the features I am talking about (there should be a lot more blue lines).
When you call rtabmap-detectMoreLoopClosures tool, you can add parameter --RGBD/ProximityBySpace false to always force global feature matching between two candidates, then keep Vis/CorGuessWinSize>0 to re-do a local feature matching to refine the one from global matching (this generally give better transforms).
Thank you for your in depth answer. I realise that comparing words was a bit dumb. I gradually understand more and more of what is going on, so I guess that's good :)
Sadly all that doesn't explain why some loops get added, although clicking ten times on "refine" doesn't work because the number of inliers never gets even close to my Vis/minInliers (so I would be surprised if it was just a probabilistic effect of RANSAC, but maybe it just is).
Because the database I'm working on is so close to ground truth after a simple rtabmap-reprocess, but gets gradually very bad the more I detect more loops, I'm wondering if it wouldn't help in that case to increase the variance of user loops closures, in order to still benefit from loops in places where there are plenty of them. I don't think there is a "loop variance multiplier" parameter is there ? It would be interesting to have a dynamic field in the db viewer: rather than a button "Graph Optimization/Ignore user loop closures", a slider enabling to chose a variance multiplier. Tweaking that parameter scene per scene could be helpful for the environments I'm scanning I feel.
Best Pierre
Because the database I'm working on is so close to ground truth after a simple rtabmap-reprocess, but gets gradually very bad the more I detect more loops, I'm wondering if it wouldn't help in that case to increase the variance of user loops closures, in order to still benefit from loops in places where there are plenty of them.
Normally, odometry links should be more accurate than loop closure / user links, because VIO had more info (IMU, wide angle camera, better pose prediction -> better feature tracking/matching) to be more accurate. Unless VIO makes a very bad jump for some reason, you may want to keep odometry links with very low variance and maybe increase the variance on the loop closure / user links so that they don't bend too much odometry links (which we assume are pretty close to reality, locally at least).
We don't have a variance multiplier option, but we can change variance of loop closure links with "Edit-> Update all loop closure covariances..."
It should update user, loop closure and proximity links all at the same time: https://github.com/introlab/rtabmap/blob/e5655b0b961df83a94b4fac423584ed68dcd3d18/guilib/src/DatabaseViewer.cpp#L4410-L4425