def __init__(self): # Initializing the classifier models, one per class self.ClsModelCore = [libNewFactorFake_SP.ClsModelFake1(), libNewFactorFake_SP.ClsModelFake2()] # Symbol initialization self.X = lambda i: int(gtsam.Symbol('x', i)) self.XO = lambda j: int(gtsam.Symbol('o', j))
def __init__(self, model_noise, adding_constant=0): # The instances differ from each other by model noise self.model_noise = model_noise self.adding_constant = adding_constant # Symbol initialization self.X = lambda i: int(gtsam.Symbol('x', i)) self.XO = lambda j: int(gtsam.Symbol('o', j))
def __init__(self, addresses): # Initializing the classifier models, one per class libNewFactor_LG.initialize_python_objects(addresses[0], addresses[1]) libNewFactor_LG_rand2.initialize_python_objects( addresses[2], addresses[3]) libNewFactor_LG_rand3.initialize_python_objects( addresses[4], addresses[5]) self.ClsModelCore = [ libNewFactor_LG, libNewFactor_LG_rand2, libNewFactor_LG_rand3 ] # Symbol initialization self.X = lambda i: int(gtsam.Symbol('x', i)) self.XO = lambda j: int(gtsam.Symbol('o', j))
def __init__(self, class_probability_prior, geo_model, da_model, cls_model, prior_mean, prior_noise, cls_enable=True): # Symbol initialization self.X = lambda i: int(gtsam.Symbol('x', i)) self.XO = lambda j: int(gtsam.Symbol('o', j)) # Camera pose prior self.graph = gtsam.NonlinearFactorGraph() self.graph.add( gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise)) # Class realization and prior probabilities. if one of the class probabilities is 0 it will # set the logWeight to -inf self.cls_enable = cls_enable # Setting initial values self.initial = gtsam.Values() self.initial.insert(self.X(0), prior_mean) self.prev_step_camera_pose = prior_mean self.daRealization = list() # self.initial.print() # Setting up models self.geoModel = geo_model self.daModel = da_model self.clsModel = cls_model # self.classifierModel = classifierModel # Setting up ISAM2 TODO: make ISAM2 work. As of now, it doesn't take the initial values at optimize_isam2 method params = gtsam.ISAM2Params() # params.relinearization_threshold = 0.01 params.relinearize_skip = 1 self.isam = gtsam.ISAM2(params) # Setting up weight memory self.weight_memory = list() self.normalized_weight = 0 # Setting up class realization self.classRealization = dict() self.classProbabilityPrior = class_probability_prior
A script validating the ImuFactor inference. """ from __future__ import print_function import math import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.mplot3d import Axes3D import gtsam from gtsam_utils import plotPose3 from PreintegrationExample import PreintegrationExample, POSES_FIG # shorthand symbols: BIAS_KEY = int(gtsam.Symbol('b', 0)) V = lambda j: int(gtsam.Symbol('v', j)) X = lambda i: int(gtsam.Symbol('x', i)) np.set_printoptions(precision=3, suppress=True) class ImuFactorExample(PreintegrationExample): def __init__(self): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # Choose one of these twists to change scenario: zero_twist = (np.zeros(3), np.zeros(3)) forward_twist = (np.zeros(3), self.velocity)
def state_key(self, i): return gtsam.Symbol('x', i).key()
from __future__ import print_function import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import numpy as np import time # for sleep() import gtsam from gtsam_examples import SFMdata import gtsam_utils # shorthand symbols: X = lambda i: int(gtsam.Symbol('x', i)) L = lambda j: int(gtsam.Symbol('l', j)) def visual_ISAM2_plot(poses, points, result): # VisualISAMPlot plots current state of ISAM2 object # Author: Ellon Paiva # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert # Declare an id for the figure fignum = 0 fig = plt.figure(fignum) ax = fig.gca(projection='3d') plt.cla() # Plot points # Can't use data because current frame might not see all points # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow