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
