def test_landmark_2d(self): # create graph graph = mrob.FGraph() #initial point at [0,0,0] with some noise n1 = graph.add_node_pose_2d(np.random.randn(3) * 0.05) # non-initialized landmarks, but they could be initialiazed l1 = graph.add_node_landmark_2d(np.array([0, 0])) l2 = graph.add_node_landmark_2d(np.zeros(2)) print('node pose id = ', n1, ' , node landmark 1 id = ', l1, ' , node landmark 2 id = ', l2) # anchor factor W_0 = np.identity(3) graph.add_factor_1pose_2d(np.zeros(3), n1, 1e6 * W_0) # landmark factors obs = np.array([1, 0]) W = np.identity(2) graph.add_factor_1pose_1landmark_2d(obs, n1, l1, W) obs = np.array([1, np.pi / 2]) graph.add_factor_1pose_1landmark_2d(obs, n1, l2, W) graph.print(True) print('\n\n\n Solving Fgraph:\n') #graph.solve(mrob.GN) #1 iteration of Gauss-Newton graph.solve( mrob.LM ) #as many iterations until convergence for Levenberg Marquardt graph.print(True) print( 'Current chi2 = ', graph.chi2() ) # re-evaluates the error, on print it is only the error on evalation before update
def __init__(self, initial_state, alphas, state_dim=3, obs_dim=2, landmark_dim=2, action_dim=3, *args, **kwargs): super(Sam, self).__init__(*args, **kwargs) self.state_dim = state_dim self.landmark_dim = landmark_dim self.obs_dim = obs_dim self.action_dim = action_dim self.alphas = alphas self.graph = mrob.FGraph()
def test_2d(self): # example similar to ./FGrpah/examples/example_FGraph_solve.cpp # create graph, reserving for 10 nodes and 20 factors if indicated graph = mrob.FGraph() # TODO give more nodes, such as a rectangle. x = np.random.randn(3) n1 = graph.add_node_pose_2d(x) x = 1 + np.random.randn(3) * 1e-1 n2 = graph.add_node_pose_2d(x) print('node 1 id = ', n1, ' , node 2 id = ', n2) invCov = np.identity(3) graph.add_factor_1pose_2d(np.zeros(3), n1, 1e6 * invCov) graph.add_factor_2poses_2d(np.ones(3), n1, n2, invCov) graph.solve(mrob.GN) graph.print(True)
def __init__(self, initial_state, alphas, state_dim=3, obs_dim=2, landmark_dim=2, action_dim=3, *args, **kwargs): super(Sam, self).__init__(*args, **kwargs) self.state_dim = state_dim self.landmark_dim = landmark_dim self.obs_dim = obs_dim self.action_dim = action_dim self.alphas = alphas self.graph = mrob.FGraph() self.cur_state = initial_state.mu self.Sigma = initial_state.Sigma self.N_zero = self.graph.add_node_pose_2d(self.cur_state) W = self.Sigma self.graph.add_factor_1pose_2d(self.cur_state, self.N_zero, inv(W)) self.ci2 = [] self.LEHRBUCH = {}
def test_landmark_3d(self): # example translated from FGrpah/examples/example_solver_3d_landmarks.cpp graph = mrob.FGraph() n1 = graph.add_node_pose_3d( mrob.geometry.SE3(np.random.randn(6) * 0.05)) # non-initialized landmarks, nut they could be initialiazed l1 = graph.add_node_landmark_3d(np.zeros(3)) l2 = graph.add_node_landmark_3d(np.zeros(3)) l3 = graph.add_node_landmark_3d(np.zeros(3)) print('node pose id = ', n1, ' , node landmark 1 id = ', l1, ' , node landmark 2 id = ', l2, ' , node landmark 3 id = ', l3) # anchor factor W_0 = np.identity(6) graph.add_factor_1pose_3d(mrob.geometry.SE3(), n1, 1e6 * W_0) # landmark factors obs = np.array([1, 0, 0]) W = np.identity(3) # XXX there is a flag (not implemented) for autoinitializaing landmarks (with inverse function). Please tell me if it is necessary/useful graph.add_factor_1pose_1landmark_3d(obs, n1, l1, W) obs = np.array([1, 1, 0]) graph.add_factor_1pose_1landmark_3d(obs, n1, l2, W) obs = np.array([1, 1, 1]) graph.add_factor_1pose_1landmark_3d(obs, n1, l3, W) graph.print(True) print('\n\n\n Solving Fgraph:\n') #graph.solve(mrob.fgraph.GN) #1 iteration of Gauss-Newton graph.solve( mrob.LM ) #as many iterations until convergence for Levenberg Marquardt graph.print(True)
# import mrob import numpy as np # example similar to ./FGrpah/examples/example_FGraph_solve.cpp # create graph, reserving for 10 nodes and 20 factors if indicated graph = mrob.FGraph() # TODO give more nodes, such as a rectangle. x = np.random.randn(3) n1 = graph.add_node_pose_2d(x) x = 1 + np.random.randn(3) * 1e-1 n2 = graph.add_node_pose_2d(x) print('node 1 id = ', n1, ' , node 2 id = ', n2) invCov = np.identity(3) graph.add_factor_1pose_2d(np.zeros(3), n1, 1e6 * invCov) graph.add_factor_2poses_2d(np.ones(3), n1, n2, invCov) graph.solve(mrob.GN) graph.print(True)
def test_sphere(self): # Initialize data structures vertex_ini = {} factors = {} factor_inf = {} factors_dictionary = {} N = 2500 # load file, .g2o format from https://github.com/RainerKuemmerle/g2o/wiki/File-Format #file_path = '../../benchmarks/sphere_gt.g2o' file_path = '../../benchmarks/sphere.g2o' with open(path.join(path.dirname(__file__), file_path), 'r') as file: for line in file: d = line.split() # read edges and vertex if d[0] == 'EDGE_SE3:QUAT': # EDGE_SE33:QUAT id_origin[1] id_target[2] x[3],y[4],z[5],qx[6],qy[7],qz[8],qw[9] # L11[10], L12[11], L13, L14, L15, L16 # L22[16], L23, L24, L25, L26 # L33[21], L34, L35, L36 # L44[25], L45 L46 # L55[28] L56 # L66[30] # transforming quaterntion to SE3 quat = np.array([d[6], d[7], d[8], d[9]], dtype='float64') T = np.eye(4, dtype='float64') T[:3, :3] = mrob.geometry.quat_to_so3(quat) T[0, 3] = d[3] T[1, 3] = d[4] T[2, 3] = d[5] factors[int(d[1]), int(d[2])] = mrob.geometry.SE3(T) # matrix information. g2o convention W = np.array([[d[10], d[11], d[12], d[13], d[14], d[15]], [d[11], d[16], d[17], d[18], d[19], d[20]], [d[12], d[17], d[21], d[22], d[23], d[24]], [d[13], d[18], d[22], d[25], d[26], d[27]], [d[14], d[19], d[23], d[26], d[28], d[29]], [d[15], d[20], d[24], d[27], d[29], d[30]]], dtype='float64') # mrob convetion is however xi = [w, v], so we need to permute these values P = np.zeros((6, 6)) P[:3, 3:] = np.eye(3) P[3:, :3] = np.eye(3) factor_inf[int(d[1]), int(d[2])] = P @ W @ P.transpose() #print(W) #print(factor_inf[int(d[1]), int(d[2])]) factors_dictionary[int(d[2])].append(int(d[1])) else: # VERTEX_SE3:QUAT id[1], x[2],y[3],z[4],qx[5],qy[6],qz[7],qw[8] # these are the initial guesses for node states # transform to a RBT # transforming quaterntion to SE3 quat = np.array([d[5], d[6], d[7], d[8]], dtype='float64') T = np.eye(4, dtype='float64') T[:3, :3] = mrob.geometry.quat_to_so3(quat) T[0, 3] = d[2] T[1, 3] = d[3] T[2, 3] = d[4] #print('ds: ', d[1], d[2], d[3],d[4],d[5],d[6],d[7],d[8]) #print(T) vertex_ini[int(d[1])] = mrob.geometry.SE3(T) # create an empty list of pairs of nodes (factor) connected to each node factors_dictionary[int(d[1])] = [] #print(factors_dictionary) # Initialize FG graph = mrob.FGraph() x = vertex_ini[0] print(x.T()) n = graph.add_node_pose_3d(x) W = np.eye(6) graph.add_factor_1pose_3d(x, n, 1e5 * W) processing_time = [] # start events, we solve for each node, adding it and it corresponding factors # in total takes 0.3s to read all datastructure for t in range(1, N): x = vertex_ini[t] n = graph.add_node_pose_3d(x) assert t == n, 'index on node is different from counter' # find factors to add. there must be 1 odom and other observations connecting_nodes = factors_dictionary[n] for nodeOrigin in factors_dictionary[n]: # inputs: obs, idOrigin, idTarget, invCov obs = factors[nodeOrigin, t] #covInv = factor_inf[nodeOrigin, t] covInv = np.eye(6) #covInv[3:,3:] = np.eye(3)*100 graph.add_factor_2poses_3d(obs, nodeOrigin, t, covInv) # for end. no more loop inside the factors # solve the problem incrementally #graph.solve(mrob.GN) #graph.solve(mrob.LM, 3) # Solves the batch problem print('Current state of the graph: chi2 = ', graph.chi2()) start = time.time() graph.solve(mrob.LM, 8) end = time.time() print(', chi2 = ', graph.chi2(), ', time on calculation [s] = ', 1e0 * (end - start))
def test_m3500(self): # Initialize data structures vertex_ini = {} factors = {} factors_dictionary = {} N = 3500 # load file with open( path.join(path.dirname(__file__), '../../benchmarks/M3500.txt'), 'r') as file: for line in file: d = line.split() # read edges and vertex, in TORO format if d[0] == 'EDGE2': # EDGE2 id_origin id_target dx dy dth I11 I12 I22 I33 I13 I23 factors[int(d[1]), int(d[2])] = np.array([ d[3], d[4], d[5], d[6], d[7], d[8], d[9], d[10], d[11] ], dtype='float64') factors_dictionary[int(d[2])].append(int(d[1])) else: # VERTEX2 id x y theta # these are the initial guesses for node states vertex_ini[int(d[1])] = np.array([d[2], d[3], d[4]], dtype='float64') # create an empty list of pairs of nodes (factor) connected to each node factors_dictionary[int(d[1])] = [] # Initialize FG graph = mrob.FGraph() x = np.zeros(3) n = graph.add_node_pose_2d(x) print('node 0 id = ', n) # id starts at 1 graph.add_factor_1pose_2d(x, n, 1e9 * np.identity(3)) processing_time = [] # start events, we solve for each node, adding it and it corresponding factors # in total takes 0.3s to read all datastructure for t in range(1, N): x = vertex_ini[t] n = graph.add_node_pose_2d(x) assert t == n, 'index on node is different from counter' # find factors to add. there must be 1 odom and other observations connecting_nodes = factors_dictionary[n] for nodeOrigin in factors_dictionary[n]: # inputs: obs, idOrigin, idTarget, invCov obs = factors[nodeOrigin, t][:3] covInv = np.zeros((3, 3)) # on M3500 always diagonal information matrices covInv[0, 0] = factors[nodeOrigin, t][3] covInv[1, 1] = factors[nodeOrigin, t][5] covInv[2, 2] = factors[nodeOrigin, t][6] graph.add_factor_2poses_2d(obs, nodeOrigin, t, covInv) # for end. no more loop inside the factors # solve the problem iteratively 2500nodes #graph.solve(mrob.GN) print('current initial chi2 = ', graph.chi2()) start = time.time() graph.solve(mrob.LM, 50) end = time.time() print('\nLM chi2 = ', graph.chi2(), ', total time on calculation [s] = ', 1e0 * (end - start))
# # add path to local library mrob on bashr_rc: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/build/mrobpy" import mrob import numpy as np # example translated from FGrpah/examples/example_solver_3d_landmarks.cpp # Different ways to initialize robust factors #graph = mrob.FGraph(mrob.QUADRATIC) #graph = mrob.FGraph(mrob.HUBER) #graph = mrob.FGraph(mrob.MCCLURE) graph = mrob.FGraph(mrob.CAUCHY) xi = np.array( [-0.0282668, -0.05882867, 0.0945925, -0.02430618, 0.01794402, -0.06549129]) n1 = graph.add_node_pose_3d(mrob.geometry.SE3(xi)) # non-initialized landmarks, nut they could be initialiazed l1 = graph.add_node_landmark_3d(np.zeros(3)) l2 = graph.add_node_landmark_3d(np.zeros(3)) l3 = graph.add_node_landmark_3d(np.zeros(3)) print('node pose id = ', n1, ' , node landmark 1 id = ', l1, ' , node landmark 2 id = ', l2, ' , node landmark 3 id = ', l3) # anchor factor W_0 = np.identity(6) graph.add_factor_1pose_3d(mrob.geometry.SE3(), n1, 1e6 * W_0) # landmark factors obs = np.array([1, 0, 0]) W = np.identity(3) graph.add_factor_1pose_1landmark_3d(obs, n1, l1, W)