Official RTAB-Map Forum

Re: Manually adding/modifying node constraints

Posted by pakr on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Manually-adding-modifying-node-constraints-tp6983p7016.html

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
Free forum by Nabble Edit this page