keys.push_back(2) print 'size: ', keys.size() print keys.at(0) print keys.at(1) noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0) noise.print_('noise:') print 'noise print:', noise f = gtsam.JacobianFactor(7, np.ones([2, 2]), model=noise, b=np.ones(2)) print 'JacobianFactor(7):\n', f print "A = ", f.getA() print "b = ", f.getb() f = gtsam.JacobianFactor(np.ones(2)) f.print_('jacoboian b_in:') print "JacobianFactor initalized with b_in:", f diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 2., 3.])) fv = gtsam.PriorFactorVector(1, np.array([4., 5., 6.]), diag) print "priorfactorvector: ", fv print "base noise: ", fv.get_noiseModel() print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base( fv.get_noiseModel()) X = gtsam.symbol(65, 19) print X print gtsam.symbolChr(X) print gtsam.symbolIndex(X)
def mhjcbb(sim, num_tracks=10, prob=0.95, posterior_pose_md_threshold=1.5, prune2_skip=10, max_observed_diff=3): slams = [[gtsam.ISAM2(), set()]] prune2_count = 1 observed = set() for x, (odom, obs) in enumerate(sim.step()): for isam2, observed in slams: graph = gtsam.NonlinearFactorGraph() values = gtsam.Values() if x == 0: prior_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model) graph.add(prior_factor) values.insert(X(0), odom) else: odom_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom, odom_model) graph.add(odom_factor) pose0 = isam2.calculateEstimatePose2(X(x - 1)) values.insert(X(x), pose0.compose(odom)) isam2.update(graph, values) ################################################################################ mhjcbb = gtsam.da_MHJCBB2(num_tracks, prob, posterior_pose_md_threshold) for isam2, observed, in slams: mhjcbb.initialize(isam2) for l, br in obs.items(): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) mhjcbb.add(gtsam.Rot2(br[0]), br[1], br_model) mhjcbb.match() ################################################################################ new_slams = [] for i in range(mhjcbb.size()): track, keys = mhjcbb.get(i) keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())] isam2 = gtsam.ISAM2() isam2.update(slams[track][0].getFactorsUnsafe(), slams[track][0].calculateEstimate()) graph = gtsam.NonlinearFactorGraph() values = gtsam.Values() observed = set(slams[track][1]) for (l_true, br), l in zip(obs.items(), keys): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l), pose1.transform_from(point)) observed.add(l) isam2.update(graph, values) new_slams.append([isam2, observed]) slams = new_slams slams = prune1(slams, x, posterior_pose_md_threshold) if len(slams[0][1]) > prune2_count * prune2_skip: slams = prune2(slams, max_observed_diff) prune2_count += 1 result = [] for isam2, observed in slams: traj_est = [ isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj)) ] traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est]) landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed] landmark_est = np.array([(p.x(), p.y()) for p in landmark_est]) result.append((traj_est, landmark_est)) return result
def from_values(values): estimate_keys = [values.keys().at(i) for i in range(values.keys().size())] quadric_keys = [k for k in estimate_keys if chr(gtsam.symbolChr(k)) == 'q'] quadrics = {int(gtsam.symbolIndex(k)): quadricslam.ConstrainedDualQuadric.getFromValues(values, k) for k in quadric_keys} return Quadrics(quadrics)
def slam(sim, da='jcbb', prob=0.95): isam2 = gtsam.ISAM2() graph = gtsam.NonlinearFactorGraph() values = gtsam.Values() observed = set() for x, (odom, obs) in enumerate(sim.step()): if x == 0: prior_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model) graph.add(prior_factor) values.insert(X(0), odom) else: odom_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom, odom_model) graph.add(odom_factor) pose0 = isam2.calculateEstimatePose2(X(x - 1)) values.insert(X(x), pose0.compose(odom)) isam2.update(graph, values) graph.resize(0) values.clear() estimate = isam2.calculateEstimate() if da == 'dr': for l_true, br in obs.items(): l = len(observed) br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l), pose1.transform_from(point)) observed.add(l) elif da == 'perfect': for l_true, br in obs.items(): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l_true), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l_true not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l_true), pose1.transform_from(point)) observed.add(l_true) elif da == 'jcbb': ################################################################################ jcbb = gtsam.da_JCBB2(isam2, prob) for l, br in obs.items(): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) jcbb.add(gtsam.Rot2(br[0]), br[1], br_model) keys = jcbb.match() ################################################################################ keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())] for (l_true, br), l in zip(obs.items(), keys): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l), pose1.transform_from(point)) observed.add(l) isam2.update(graph, values) graph.resize(0) values.clear() traj_est = [ isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj)) ] traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est]) landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed] landmark_est = np.array([(p.x(), p.y()) for p in landmark_est]) return [[traj_est, landmark_est]]
def plot_system(self, graph, estimate): """ Plots the trajectory and quadrics Color represents the sum of boundingbox factor errors at each pose """ # extract variables and factors trajectory = Trajectory.from_values(estimate) quadrics = Quadrics.from_values(estimate) # collect nonlinear factors from graph box_factors = [ quadricslam.BoundingBoxFactor.getFromGraph(graph, i) for i in range(graph.size()) if graph.at(i).keys().size() == 2 and chr(gtsam.symbolChr(graph.at(i).keys().at(1))) == 'q' ] # odom_factors = [graph.at(i) for i in range(graph.size()) if graph.at(i).keys().size() == 2 and chr(gtsam.symbolChr(graph.at(i).keys().at(1))) == 'x'] # extract x-y positions xy = np.array([[pose.x(), pose.y()] for pose in trajectory.data()]) # extract error at each pose errors = [] for pose_key in trajectory.keys(): # get factors for current pose bbfs = [ f for f in box_factors if gtsam.symbolIndex(f.keys().at(0)) == pose_key ] # get sum of each bbf error at pose error = np.sum([ np.square(bbf.unwhitenedError(estimate)).sum() for bbf in bbfs ]) errors.append(error) # open and clear figure figure = plt.figure(self._figname) figure.clf() # plot trajectory x-y plt.plot(xy[:, 0], xy[:, 1], linestyle='-', c='c', zorder=1) plt.scatter(xy[:, 0], xy[:, 1], s=25, c=errors, zorder=9) plt.colorbar() # plot quadrics for quadric in quadrics.data(): plt.plot(quadric.pose().x(), quadric.pose().y(), marker='o', markersize=5, c='m', fillstyle='none') plt.plot(quadric.pose().x(), quadric.pose().y(), marker='o', markersize=5, c='m', alpha=0.5) # show plot plt.show()