Official RTAB-Map Forum

Manually adding/modifying node constraints

classic Classic list List threaded Threaded
♦
♦
 4 messages Options
pakr
Reply | Threaded
Open this post in threaded view
♦
♦
|

Manually adding/modifying node constraints

pakr
This post was updated on .
Hi Mathieu,

thank you for creating and publishing such a sophisticated SLAM framework.

I would like to merge multiple databases that do not contain overlapping areas but of which I know their relative poses.
Additionally, some of the mapped rooms are quite similar (sometimes only one different furniture item), so that I fear that many wrong loop closures may happen.
Manually removing all wrong loop closures after the first database generation does not appear to be feasible in my case.

a) Is there a way to set fixed known constraints in advance, before automatic loop closure?
I have thought about joining the databases as if they were recorded in a single session and manually adjusting odometry/pose constraints (e.g. adding offsets to a specific set of pose entries in the Node table), maybe also adding user/neighbor links (as mentioned in this topic) and then reprocessing the data set with "rtabmap-reprocess".
Can you give an example/hint regarding where to make the necessary edits?

b) Is there documentation for the database structure and contents?
I have not been able to find much information and there seem to have been changes, e.g. in this version there are entries for "rot_variance" and "trans_variance" but in the current version there is only a BLOB entry called "information_matrix".

Thank you very much in advance!

Cheers,
pakr
matlabbe
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Manually adding/modifying node constraints

matlabbe
Administrator
Well it was not possible before. I took some time to support adding manual constraints in this case in this commit (currently available only on devel branch). Here how it works after opening a database in rtabmap-databaseViewer:
1) Browse two frames of different sessions on which you want to add a constraint
2) In Constraints View panel, the Add button will be enabled if they are valid nodes, then click on Add
3) It will fail to find automatically the transform, you can now click on Yes on the error message to manually set the transform


4) This dialog will open:


5) Here I set approximately 90 degrees  between the frames (the frames on left may switch to have the largest id on left)


6) Here another example showing the new constraint between the two sessions:



For your second question b), your first link is like 4 years ago :P. We keep now the information matrix (6x6 double) for each link. The actual documentation of the database is this schema. In some cases, editing directly the database can be convenient, but in most cases DatabaseViewer is enough to modify general stuff.

cheers,
Mathieu
pakr
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Manually adding/modifying node constraints

pakr
Thank you for your support and adding the software feature to the database viewer!

I built the version from the devel branch and manually setting user links worked just fine.

In addition, it also helped me with understanding the database, so that I was able to create a python script which automatically joins two databases and adds a user link.

Maybe it can be useful for others, so here you go:
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
import os.path
from subprocess import call
import struct
import sqlite3


#Parameters
db0_path = './rtabmap00.db'
db1_path = './rtabmap01.db'
fused_database_path = './fused_map.db'

nodeid0 = 1 #database 0 node id for link between databases
nodeid1 = 1 #database 1 node id for link between databases
xoffs = 5.5 #translational x offset in m
yoffs = 1.5 #translational y offset in m
#rotation not yet implemented


#call rtabmap-reprocess
#--> databases are properly joined with common dictionary
if not os.path.exists(fused_database_path):
    call(["rtabmap-reprocess","%s;%s" % (db0_path,db1_path),
          fused_database_path])

#connect to databases
conn0 = sqlite3.connect(db0_path)
c0 = conn0.cursor()

conn1 = sqlite3.connect(db1_path)
c1 = conn1.cursor()

connf = sqlite3.connect(fused_database_path)
cf = connf.cursor()

print("adjusting link table")

#delete all links in fused map
cf.execute('DELETE FROM Link')

#copy links from db0 to fused db
c0.execute('SELECT * FROM Link')
db0_links = c0.fetchall()
numlinks_db0 = len(db0_links)
for i in range(numlinks_db0):
    cf.execute('INSERT INTO Link (from_id, to_id, type, information_matrix, \
               transform, user_data) VALUES (?, ?, ?, ?, ?, ?)',db0_links[i])

#get node offset
c0.execute('SELECT MAX(id) FROM Node')
nodeoffset = c0.fetchone()[0]

#copy links from db1 to fused db, with respective offset
c1.execute('SELECT * FROM Link')
db1_links = c1.fetchall()
numlinks_db1 = len(db1_links)
for i in range(numlinks_db1):
    from_id_new = db1_links[i][0] + nodeoffset
    to_id_new = db1_links[i][1] + nodeoffset
    new_link = (from_id_new, to_id_new, *db1_links[i][2:])
    cf.execute('INSERT INTO Link (from_id, to_id, type, information_matrix, \
               transform, user_data) VALUES (?, ?, ?, ?, ?, ?)',new_link)

print("adding user link")

#add user link
#user link --> information matrix is 6x6 double identity matrix
infmat = b'\x00\x00\x00\x00\x00\x00\xf0?\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf0?\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf0?\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\xf0?\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\xf0?\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
        \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf0?'
transmat = (1., 0., 0., xoffs, 0., 1., 0., yoffs, 0., 0., 1., 0.)
transmat = struct.pack('%df' % len(transmat), *transmat)
new_link = (nodeid0,nodeoffset + nodeid1,4,infmat,transmat,None)
cf.execute('INSERT INTO Link (from_id, to_id, type, information_matrix, \
           transform, user_data) VALUES (?, ?, ?, ?, ?, ?)',new_link)

transmat = (1., 0., 0., -xoffs, 0., 1., 0., -yoffs, 0., 0., 1., 0.)
transmat = struct.pack('%df' % len(transmat), *transmat)
new_link = (nodeoffset + nodeid1,nodeid0,4,infmat,transmat,None)
cf.execute('INSERT INTO Link (from_id, to_id, type, information_matrix, \
           transform, user_data) VALUES (?, ?, ?, ?, ?, ?)',new_link)

#commit changes and close databases
connf.commit()

connf.close()
conn0.close()
conn1.close()

print("done")


Here is a small example of how the result might look like (databases not optimized):


Cheers, pakr
matlabbe
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Manually adding/modifying node constraints

matlabbe
Administrator
Great, thx for sharing!
Free forum by Nabble Edit this page