def run_simulation(self):
num_client = int(self.num_client.text())
num_severs = int(self.num_server.text())
iterations = int(self.num_iterations.text())
ml_algo_index = self.selectMLAlgorithm.currentIndex()
method = 'log_reg' # default
if ml_algo_index == 0:
method = 'log_reg'
elif ml_algo_index == 1:
method = 'perceptron'
else:
method = 'mlp'
random.seed(0)
np.random.seed(0)
initializer = Initializer(
num_clients=num_client,
iterations=iterations,
num_servers=num_severs,
method=method,
simulation_output_view=self.simulation_output)
# can use any amount of iterations less than config.ITERATIONS but the
# initializer has only given each client config.ITERATIONS datasets for training.
a = datetime.datetime.now()
initializer.run_simulation(iterations,
self.simulation_output,
server_agent_name='server_agent0')
b = datetime.datetime.now()
def reset(self):
self.initializer = Initializer("braak1", self.numNodes, self.params)
self.concentration = self.initializer.get()
self.concentrationHistory = np.copy(self.concentration)
self.producer = Producer(self.types)
self.diffusor = Diffusor("euclidean",
self.params,
EuclideanAdjacency=self.euclideanAdjacency)
def __init__(self):
Initializer.__init__(self)
self.check_init_first_time()
# posts posted more than 15 days ago will be ignored
self.ignore_before = self.utc.utcnow() - timedelta(days=ConfReader.MAX_DAYS_SPENT_TO_CRAWL)
print "** [!] RESTRICTION: Ignore posts before: " + str(self.ignore_before) + " **"
self.recalculate_visits()
def __init__(self, camera, tracker, grountruth=None):
self.cam = camera
self.map = Map()
Frame.set_tracker(tracker) # set the static field of the class
# camera info
self.W, self.H = camera.width, camera.height
self.K = camera.K
self.Kinv = camera.Kinv
self.D = camera.D # distortion coefficients [k1, k2, p1, p2, k3]
self.stage = SLAMStage.NO_IMAGES_YET
self.intializer = Initializer()
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched inliers
self.num_vo_map_points = None # current number of valid VO map points (matched and found valid in current pose optimization)
self.trueX, self.trueY, self.trueZ = None, None, None
self.grountruth = grountruth
self.mask_match = None
self.velocity = None
self.init_history = True
self.poses = [] # history of poses
self.t0_est = None # history of estimated translations
self.t0_gt = None # history of ground truth translations (if available)
self.traj3d_est = [
] # history of estimated translations centered w.r.t. first one
self.traj3d_gt = [
] # history of estimated ground truth translations centered w.r.t. first one
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_main_track = TimerFps('Track',
is_verbose=self.timer_verbose)
self.timer_pose_opt = TimerFps('Pose optimization',
is_verbose=self.timer_verbose)
self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('Pose estimation',
is_verbose=self.timer_verbose)
self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose)
self.timer_seach_map = TimerFps('Search Map',
is_verbose=self.timer_verbose)
self.timer_triangulation = TimerFps('Triangulation',
is_verbose=self.timer_verbose)
self.time_local_opt = TimerFps('Local optimization',
is_verbose=self.timer_verbose)
def initialize_repo(self):
# 初始化应用配置
support_languages = []
for k,v in self.support_languages.items():
if v.get():
for language in appConfig.languages[k]:
support_languages.append(language)
# 初始化项目仓库
init = Initializer()
init.initialize(self.android_resources_dir.get(), self.ios_resources_dir.get(), support_languages)
# 初始化完毕
result = askokcancel(title = '初始化完成', message='已完成项目仓库初始化，请重启程序')
if result:
self.root.quit()
def instance(cls):
if cls._instance is None:
init = Initializer()
init.load_config()
cls._instance = Simulator(init.get("signal-time"),
init.get("script-name"),
init.get("error-detection"))
return cls._instance
def __config_logging():
'''配置日志'''
LOG_FORMAT = "%(asctime)s - %(levelname)s - %(message)s"
DATE_FORMAT = "%m/%d/%Y %H:%M:%S %p"
logging.basicConfig(filename='app.log',
filemode='a',
level=logging.DEBUG,
format=LOG_FORMAT,
datefmt=DATE_FORMAT)
logging.FileHandler(filename='app.log', encoding='utf-8')
if __name__ == "__main__":
__config_logging()
# 加载多语言资源
factory = LanguageFactory()
factory.load_languages()
# Tk
root = Tk()
root.title(factory.get_entry("app_title"))
# 进行项目初始化
initializer = Initializer()
if not initializer.is_repo_initialized():
# 进入项目初始化页面
RepoInitDialog(root).pack()
else:
# 进入正常编辑页面
MainDialog(root).pack()
root.mainloop()
# make sure we leave no references behind
AddonSettings.clear_cached_addon_settings_object()
# close the log to prevent locking on next call
Logger.instance().close_log()
log_file = None
except:
if log_file:
log_file.critical("Error running plugin", exc_info=True)
log_file.close_log()
raise
# setup the paths in Python
from initializer import Initializer # nopep8
Initializer.set_unicode()
currentPath = Initializer.setup_python_paths()
# ANY OF THESE SETTINGS SHOULD ONLY BE ENABLED FOR DEBUGGING PURPOSES
# from debug import remotedebugger
# debugger = remotedebugger.RemoteDebugger()
# Debugging with profiler
# import profile as cProfile
# import cProfile
# from debug import profilelinebyline as cProfile
# Path for PC
# statsPath = os.path.abspath(os.path.join(currentPath, "../DEV/retrospect.pc.leia.pstat"))
# Path for ATV
# statsPath = os.path.abspath("/private/var/mobile/retrospect.atv.pstat")
train_data_length = int(data_length * 0.8)
print("train_label_length:", train_data_length)
data_sample_train, data_sample_test = data_sample[:
train_data_length], data_sample[
train_data_length:]
data_label_train, data_label_test = data_label[:
train_data_length], data_label[
train_data_length:]
num_iterations = 1000
lr = 0.001
weight_decay = 0.01
train_batch_size = 16
test_batch_size = 100
data_handler = DataHander(16)
opt = Optimizer(lr=lr, momentum=0.9, iteration=0, gamma=0.0005, power=0.75)
initializer = Initializer()
data_handler.get_data(sample=data_sample_train, label=data_label_train)
data_handler.shuffle()
dnn = DNNNet(optimizer=opt.batch_gradient_descent_anneling,
initializer=initializer.xavier,
batch_size=train_batch_size,
weights_decay=weight_decay)
dnn.initial()
train_error = []
max_loss = math.inf
early_stopping_iter = 15
early_stopping_mark = 0
for i in range(num_iterations):
print('第', i, '次迭代')
opt.update_iteration(i)
data_handler.pull_data()
def init():
"""交互式初始化Graia项目"""
initializer = Initializer()
initializer.run()
logging.debug("Send request to kudago: {}".format(url))
events = requests.get(url).json()
next_url = events["next"]
self._on_http_load(events, depth)
self._deep_load(next_url, depth + 1)
def _on_http_load(self, events, page):
name = "test/kudago{}.json".format(str(page))
logging.debug("Save events to {}".format(name))
with open(name, "w") as fd:
json.dump(events, fd)
if __name__ == "__main__":
logging.basicConfig(level=logging.DEBUG)
context = Initializer.get_context()
se = context.get_se()
kudago_cfg = context.get_kudago_cfg()
es_cfg = context.get_es_cfg()
loader = EventLoader(se, kudago_cfg)
# update from kudago
loader.http_load()
# load files to index
se.delete_index(es_cfg["index"])
se.create_index(es_cfg)
loader.load()
class SLAM(object):
def __init__(self, camera, tracker, grountruth=None):
self.cam = camera
self.map = Map()
Frame.set_tracker(tracker) # set the static field of the class
# camera info
self.W, self.H = camera.width, camera.height
self.K = camera.K
self.Kinv = camera.Kinv
self.D = camera.D # distortion coefficients [k1, k2, p1, p2, k3]
self.stage = SLAMStage.NO_IMAGES_YET
self.intializer = Initializer()
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched inliers
self.num_vo_map_points = None # current number of valid VO map points (matched and found valid in current pose optimization)
self.trueX, self.trueY, self.trueZ = None, None, None
self.grountruth = grountruth
self.mask_match = None
self.velocity = None
self.init_history = True
self.poses = [] # history of poses
self.t0_est = None # history of estimated translations
self.t0_gt = None # history of ground truth translations (if available)
self.traj3d_est = [
] # history of estimated translations centered w.r.t. first one
self.traj3d_gt = [
] # history of estimated ground truth translations centered w.r.t. first one
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_main_track = TimerFps('Track',
is_verbose=self.timer_verbose)
self.timer_pose_opt = TimerFps('Pose optimization',
is_verbose=self.timer_verbose)
self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('Pose estimation',
is_verbose=self.timer_verbose)
self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose)
self.timer_seach_map = TimerFps('Search Map',
is_verbose=self.timer_verbose)
self.timer_triangulation = TimerFps('Triangulation',
is_verbose=self.timer_verbose)
self.time_local_opt = TimerFps('Local optimization',
is_verbose=self.timer_verbose)
# fit essential matrix E with RANSAC such that: p2.T * E * p1 = 0 where E = [t21]x * R21
# out: [Rrc, trc] (with respect to 'ref' frame)
# N.B.1: trc is estimated up to scale (i.e. the algorithm always returns ||trc||=1, we need a scale in order to recover a translation which is coherent with previous estimated poses)
# N.B.2: this function has problems in the following cases: [see Hartley/Zisserman Book]
# - 'geometrical degenerate correspondences', e.g. all the observed features lie on a plane (the correct model for the correspondences is an homography) or lie a ruled quadric
# - degenerate motions such a pure rotation (a sufficient parallax is required) or an infinitesimal viewpoint change (where the translation is almost zero)
# N.B.3: the five-point algorithm (used for estimating the Essential Matrix) seems to work well in the degenerate planar cases [Five-Point Motion Estimation Made Easy, Hartley]
# N.B.4: as reported above, in case of pure rotation, this algorithm will compute a useless fundamental matrix which cannot be decomposed to return the rotation
def estimate_pose_ess_mat(self, kpn_ref, kpn_cur):
E, self.mask_match = cv2.findEssentialMat(
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.),
method=cv2.RANSAC,
prob=kRansacProb,
threshold=kRansacThresholdNormalized)
_, R, t, mask = cv2.recoverPose(E,
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.))
return poseRt(R, t.T) # Rrc,trc (cur with respect to 'ref' frame)
def track(self, img, frame_id, pose=None, verts=None):
# check image size is coherent with camera params
print("img.shape ", img.shape)
print("cam ", self.H, " x ", self.W)
assert img.shape[0:2] == (self.H, self.W)
self.timer_main_track.start()
# build current frame
self.timer_frame.start()
f_cur = Frame(self.map, img, self.K, self.Kinv, self.D, des=verts)
self.timer_frame.refresh()
if self.stage == SLAMStage.NO_IMAGES_YET:
# push first frame in the inizializer
self.intializer.init(f_cur)
self.stage = SLAMStage.NOT_INITIALIZED
return # EXIT (jump to second frame)
if self.stage == SLAMStage.NOT_INITIALIZED:
# try to inizialize
initializer_output, is_ok = self.intializer.initialize(f_cur, img)
if is_ok:
f_ref = self.intializer.f_ref
# add the two initialized frames in the map
self.map.add_frame(
f_ref) # add first frame in map and update its id
self.map.add_frame(
f_cur) # add second frame in map and update its id
# add points in map
new_pts_count, _ = self.map.add_points(
initializer_output.points4d, None, f_cur, f_ref,
initializer_output.idx_cur, initializer_output.idx_ref,
img)
Printer.green("map: initialized %d new points" %
(new_pts_count))
self.stage = SLAMStage.OK
return # EXIT (jump to next frame)
f_ref = self.map.frames[-1] # get previous frame in map
self.map.add_frame(f_cur) # add f_cur to map
# udpdate (velocity) motion model (kinematic without damping)
self.velocity = np.dot(f_ref.pose,
np.linalg.inv(self.map.frames[-2].pose))
predicted_pose = np.dot(self.velocity, f_ref.pose)
if kUseMotionModel is True:
print('using motion model')
# set intial guess for current pose optimization
f_cur.pose = predicted_pose.copy()
#f_cur.pose = f_ref.pose.copy() # get the last pose as an initial guess for optimization
if kUseSearchFrameByProjection:
# search frame by projection: match map points observed in f_ref with keypoints of f_cur
print('search frame by projection...')
idx_ref, idx_cur, num_found_map_pts = search_frame_by_projection(
f_ref, f_cur)
print("# found map points in prev frame: %d " %
num_found_map_pts)
else:
self.timer_match.start()
# find keypoint matches between f_cur and f_ref
idx_cur, idx_ref = match_frames(f_cur, f_ref)
self.num_matched_kps = idx_cur.shape[0]
print('# keypoint matches: ', self.num_matched_kps)
self.timer_match.refresh()
else:
print('estimating pose by fitting essential mat')
self.timer_match.start()
# find keypoint matches between f_cur and f_ref
idx_cur, idx_ref = match_frames(f_cur, f_ref)
self.num_matched_kps = idx_cur.shape[0]
print('# keypoint matches: ', self.num_matched_kps)
self.timer_match.refresh()
# N.B.: please, in order to understand the limitations of fitting an essential mat, read the comments of the method self.estimate_pose_ess_mat()
self.timer_pose_est.start()
# estimate inter frame camera motion by using found keypoint matches
Mrc = self.estimate_pose_ess_mat(f_ref.kpsn[idx_ref],
f_cur.kpsn[idx_cur])
Mcr = np.linalg.inv(poseRt(Mrc[:3, :3], Mrc[:3, 3]))
f_cur.pose = np.dot(Mcr, f_ref.pose)
self.timer_pose_est.refresh()
# remove outliers from keypoint matches by using the mask computed with inter frame pose estimation
mask_index = (self.mask_match.ravel() == 1)
self.num_inliers = sum(mask_index)
print('# inliers: ', self.num_inliers)
idx_ref = idx_ref[mask_index]
idx_cur = idx_cur[mask_index]
# if too many outliers reset estimated pose
if self.num_inliers < kNumMinInliersEssentialMat:
f_cur.pose = f_ref.pose.copy(
) # reset estimated pose to previous frame
Printer.red('Essential mat: not enough inliers!')
# set intial guess for current pose optimization:
# keep the estimated rotation and override translation with ref frame translation (we do not have a proper scale for the translation)
f_cur.pose[:, 3] = f_ref.pose[:, 3].copy()
#f_cur.pose[:,3] = predicted_pose[:,3].copy() # or use motion model for translation
# populate f_cur with map points by propagating map point matches of f_ref:
# we use map points observed in f_ref and keypoint matches between f_ref and f_cur
num_found_map_pts_inter_frame = 0
if not kUseSearchFrameByProjection:
for i, idx in enumerate(idx_ref): # iterate over keypoint matches
if f_ref.points[
idx] is not None: # if we have a map point P for i-th matched keypoint in f_ref
f_ref.points[idx].add_observation(
f_cur, idx_cur[i]
) # then P is automatically matched to the i-th matched keypoint in f_cur
num_found_map_pts_inter_frame += 1
print("# matched map points in prev frame: %d " %
num_found_map_pts_inter_frame)
# f_cur pose optimization 1 (here we use first available information coming from first guess of f_cur pose and map points of f_ref matched keypoints )
self.timer_pose_opt.start()
pose_opt_error, pose_is_ok, self.num_vo_map_points = optimizer_g2o.poseOptimization(
f_cur, verbose=False)
print("pose opt err1: %f, ok: %d" % (pose_opt_error, int(pose_is_ok)))
# discard outliers detected in pose optimization (in current frame)
#f_cur.reset_outlier_map_points()
if pose_is_ok is True:
# discard outliers detected in f_cur pose optimization (in current frame)
f_cur.reset_outlier_map_points()
else:
# if current pose optimization failed, reset f_cur pose to f_ref pose
f_cur.pose = f_ref.pose.copy()
self.timer_pose_opt.pause()
# TODO: add recover in case of f_cur pose optimization failure
# now, having a better estimate of f_cur pose, we can find more map point matches:
# find matches between {local map points} (points in the built local map) and {unmatched keypoints of f_cur}
if pose_is_ok is True and not self.map.local_map.is_empty():
self.timer_seach_map.start()
#num_found_map_pts = search_local_frames_by_projection(self.map, f_cur)
num_found_map_pts = search_map_by_projection(
self.map.local_map.points, f_cur) # use the built local map
print("# matched map points in local map: %d " % num_found_map_pts)
self.timer_seach_map.refresh()
# f_cur pose optimization 2 with all the matched map points
self.timer_pose_opt.resume()
pose_opt_error, pose_is_ok, self.num_vo_map_points = optimizer_g2o.poseOptimization(
f_cur, verbose=False)
print("pose opt err2: %f, ok: %d" %
(pose_opt_error, int(pose_is_ok)))
print("# valid matched map points: %d " % self.num_vo_map_points)
# discard outliers detected in pose optimization (in current frame)
if pose_is_ok is True:
f_cur.reset_outlier_map_points()
self.timer_pose_opt.refresh()
if kUseSearchFrameByProjection:
print("search for triangulation with epipolar lines...")
idx_ref, idx_cur, self.num_matched_kps, _ = search_frame_for_triangulation(
f_ref, f_cur, img)
print("# matched keypoints in prev frame: %d " %
self.num_matched_kps)
# if pose is ok, then we try to triangulate the matched keypoints (between f_cur and f_ref) that do not have a corresponding map point
if pose_is_ok is True and len(idx_ref) > 0:
self.timer_triangulation.start()
# TODO: this triangulation should be done from keyframes!
good_pts4d = np.array([
f_cur.points[i] is None for i in idx_cur
]) # matched keypoints of f_cur without a corresponding map point
# do triangulation in global frame
pts4d = triangulate_points(f_cur.pose, f_ref.pose,
f_cur.kpsn[idx_cur],
f_ref.kpsn[idx_ref], good_pts4d)
good_pts4d &= np.abs(pts4d[:, 3]) != 0
#pts4d /= pts4d[:, 3:]
pts4d[good_pts4d] /= pts4d[good_pts4d,
3:] # get homogeneous 3-D coords
new_pts_count, _ = self.map.add_points(pts4d,
good_pts4d,
f_cur,
f_ref,
idx_cur,
idx_ref,
img,
check_parallax=True)
print("# added map points: %d " % (new_pts_count))
self.timer_triangulation.refresh()
# local optimization
self.time_local_opt.start()
err = self.map.locally_optimize(local_window=kLocalWindow)
print("local optimization error: %f" % err)
self.time_local_opt.refresh()
# large window optimization of the map
# TODO: move this in a seperate thread with local mapping
if kUseLargeWindowBA is True and f_cur.id >= parameters.kEveryNumFramesLargeWindowBA and f_cur.id % parameters.kEveryNumFramesLargeWindowBA == 0:
err = self.map.optimize(
local_window=parameters.kLargeWindow) # verbose=True)
Printer.blue("large window optimization error: %f" % err)
print("map: %d points, %d frames" %
(len(self.map.points), len(self.map.frames)))
#self.updateHistory()
self.timer_main_track.refresh()
def updateHistory(self):
f_cur = self.map.frames[-1]
self.cur_R = f_cur.pose[:3, :3].T
self.cur_t = np.dot(-self.cur_R, f_cur.pose[:3, 3])
if (self.init_history is True) and (self.trueX is not None):
self.t0_est = np.array(
[self.cur_t[0], self.cur_t[1],
self.cur_t[2]]) # starting translation
self.t0_gt = np.array([self.trueX, self.trueY,
self.trueZ]) # starting translation
if (self.t0_est is not None) and (self.t0_gt is not None):
p = [
self.cur_t[0] - self.t0_est[0], self.cur_t[1] - self.t0_est[1],
self.cur_t[2] - self.t0_est[2]
] # the estimated traj starts at 0
self.traj3d_est.append(p)
self.traj3d_gt.append([
self.trueX - self.t0_gt[0], self.trueY - self.t0_gt[1],
self.trueZ - self.t0_gt[2]
])
self.poses.append(poseRt(self.cur_R, p))
# get current translation scale from ground-truth if this is set
def getAbsoluteScale(self, frame_id):
if self.grountruth is not None and kUseGroundTruthScale:
self.trueX, self.trueY, self.trueZ, scale = self.grountruth.getPoseAndAbsoluteScale(
frame_id)
return scale
else:
self.trueX = 0
self.trueY = 0
self.trueZ = 0
return 1
from initializer import Initializer populationSize = 50 dataLength = 10 numberOfIterations = 100 crossoverRate = 0.7 mutationRate = 0.1 algorithm = Initializer.initializeAlgorithm(populationSize, dataLength, numberOfIterations, mutationRate, crossoverRate) print algorithm.execute()
class Tracking(object):
def __init__(self, system):
if kDebugDrawMatches:
Frame.is_store_imgs = True
self.system = system
self.camera = system.camera
self.map = system.map
self.local_mapping = system.local_mapping
self.intializer = Initializer()
self.motion_model = MotionModel(
) # motion model for current frame pose prediction without damping
#self.motion_model = MotionModelDamping() # motion model for current frame pose prediction with damping
self.dyn_config = SLAMDynamicConfig()
self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance
self.reproj_err_frame_map_sigma = Parameters.kMaxReprojectionDistanceMap
self.max_frames_between_kfs = int(system.camera.fps)
self.min_frames_between_kfs = 0
self.state = SlamState.NO_IMAGES_YET
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched points
self.num_matched_map_points = None # current number of matched map points (matched and found valid in current pose optimization)
self.num_kf_ref_tracked_points = None # number of tracked points in k_ref (considering a minimum number of observations)
self.mask_match = None
self.pose_is_ok = False
self.predicted_pose = None
self.velocity = None
self.f_cur = None
self.idxs_cur = None
self.f_ref = None
self.idxs_ref = None
self.kf_ref = None # reference keyframe (in general, different from last keyframe depending on the used approach)
self.kf_last = None # last keyframe
self.kid_last_BA = -1 # last keyframe id when performed BA
self.local_keyframes = [] # local keyframes
self.local_points = [] # local points
self.tracking_history = TrackingHistory()
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_main_track = TimerFps('Track',
is_verbose=self.timer_verbose)
self.timer_pose_opt = TimerFps('Pose optimization',
is_verbose=self.timer_verbose)
self.timer_seach_frame_proj = TimerFps('Search frame by proj',
is_verbose=self.timer_verbose)
self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('Ess mat pose estimation',
is_verbose=self.timer_verbose)
self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose)
self.timer_seach_map = TimerFps('Search map',
is_verbose=self.timer_verbose)
self.init_history = True
self.poses = [] # history of poses
self.t0_est = None # history of estimated translations
self.t0_gt = None # history of ground truth translations (if available)
self.traj3d_est = [
] # history of estimated translations centered w.r.t. first one
self.traj3d_gt = [
] # history of estimated ground truth translations centered w.r.t. first one
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.trueX, self.trueY, self.trueZ = None, None, None
self.groundtruth = system.groundtruth # not actually used here; could be used for evaluating performances
if kLogKFinfoToFile:
self.kf_info_logger = Logging.setup_file_logger(
'kf_info_logger',
'kf_info.log',
formatter=Logging.simple_log_formatter)
# estimate a pose from a fitted essential mat;
# since we do not have an interframe translation scale, this fitting can be used to detect outliers, estimate interframe orientation and translation direction
# N.B. read the NBs of the method estimate_pose_ess_mat(), where the limitations of this method are explained
def estimate_pose_by_fitting_ess_mat(self, f_ref, f_cur, idxs_ref,
idxs_cur):
# N.B.: in order to understand the limitations of fitting an essential mat, read the comments of the method self.estimate_pose_ess_mat()
self.timer_pose_est.start()
# estimate inter frame camera motion by using found keypoint matches
# output of the following function is: Trc = [Rrc, trc] with ||trc||=1 where c=cur, r=ref and pr = Trc * pc
Mrc, self.mask_match = estimate_pose_ess_mat(
f_ref.kpsn[idxs_ref],
f_cur.kpsn[idxs_cur],
method=cv2.RANSAC,
prob=kRansacProb,
threshold=kRansacThresholdNormalized)
#Mcr = np.linalg.inv(poseRt(Mrc[:3, :3], Mrc[:3, 3]))
Mcr = inv_T(Mrc)
estimated_Tcw = np.dot(Mcr, f_ref.pose)
self.timer_pose_est.refresh()
# remove outliers from keypoint matches by using the mask computed with inter frame pose estimation
mask_idxs = (self.mask_match.ravel() == 1)
self.num_inliers = sum(mask_idxs)
print('# inliers: ', self.num_inliers)
idxs_ref = idxs_ref[mask_idxs]
idxs_cur = idxs_cur[mask_idxs]
# if there are not enough inliers do not use the estimated pose
if self.num_inliers < kNumMinInliersEssentialMat:
#f_cur.update_pose(f_ref.pose) # reset estimated pose to previous frame
Printer.red('Essential mat: not enough inliers!')
else:
# use the estimated pose as an initial guess for the subsequent pose optimization
# set only the estimated rotation (essential mat computation does not provide a scale for the translation, see above)
#f_cur.pose[:3,:3] = estimated_Tcw[:3,:3] # copy only the rotation
#f_cur.pose[:,3] = f_ref.pose[:,3].copy() # override translation with ref frame translation
Rcw = estimated_Tcw[:3, :3] # copy only the rotation
tcw = f_ref.pose[:3,
3] # override translation with ref frame translation
f_cur.update_rotation_and_translation(Rcw, tcw)
return idxs_ref, idxs_cur
def pose_optimization(self, f_cur, name=''):
print('pose opt %s ' % (name))
pose_before = f_cur.pose.copy()
# f_cur pose optimization 1 (here we use f_cur pose as first guess and exploit the matched map points of f_ref )
self.timer_pose_opt.start()
pose_opt_error, self.pose_is_ok, self.num_matched_map_points = optimizer_g2o.pose_optimization(
f_cur, verbose=False)
self.timer_pose_opt.pause()
print(' error^2: %f, ok: %d' %
(pose_opt_error, int(self.pose_is_ok)))
if not self.pose_is_ok:
# if current pose optimization failed, reset f_cur pose
f_cur.update_pose(pose_before)
return self.pose_is_ok
# track camera motion of f_cur w.r.t. f_ref
def track_previous_frame(self, f_ref, f_cur):
print('>>>> tracking previous frame ...')
is_search_frame_by_projection_failure = False
use_search_frame_by_projection = self.motion_model.is_ok and kUseSearchFrameByProjection and kUseMotionModel
if use_search_frame_by_projection:
# search frame by projection: match map points observed in f_ref with keypoints of f_cur
print('search frame by projection')
search_radius = Parameters.kMaxReprojectionDistanceFrame
f_cur.reset_points()
self.timer_seach_frame_proj.start()
idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection(
f_ref,
f_cur,
max_reproj_distance=search_radius,
max_descriptor_distance=self.descriptor_distance_sigma)
self.timer_seach_frame_proj.refresh()
self.num_matched_kps = len(idxs_cur)
print("# matched map points in prev frame: %d " %
self.num_matched_kps)
# if not enough map point matches consider a larger search radius
if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection:
f_cur.remove_frame_views(idxs_cur)
f_cur.reset_points()
idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection(
f_ref,
f_cur,
max_reproj_distance=2 * search_radius,
max_descriptor_distance=0.5 *
self.descriptor_distance_sigma)
self.num_matched_kps = len(idxs_cur)
Printer.orange(
"# matched map points in prev frame (wider search): %d " %
self.num_matched_kps)
if kDebugDrawMatches and True:
img_matches = draw_feature_matches(f_ref.img,
f_cur.img,
f_ref.kps[idxs_ref],
f_cur.kps[idxs_cur],
f_ref.sizes[idxs_ref],
f_cur.sizes[idxs_cur],
horizontal=False)
cv2.imshow('tracking frame by projection - matches',
img_matches)
cv2.waitKey(1)
if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection:
f_cur.remove_frame_views(idxs_cur)
f_cur.reset_points()
is_search_frame_by_projection_failure = True
Printer.red(
'Not enough matches in search frame by projection: ',
self.num_matched_kps)
else:
# search frame by projection was successful
if kUseDynamicDesDistanceTh:
self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat(
f_ref, f_cur, idxs_ref, idxs_cur)
# store tracking info (for possible reuse)
self.idxs_ref = idxs_ref
self.idxs_cur = idxs_cur
# f_cur pose optimization 1:
# here, we use f_cur pose as first guess and exploit the matched map point of f_ref
self.pose_optimization(f_cur, 'proj-frame-frame')
# update matched map points; discard outliers detected in last pose optimization
num_matched_points = f_cur.clean_outlier_map_points()
print(' # num_matched_map_points: %d' %
(self.num_matched_map_points))
#print(' # matched points: %d' % (num_matched_points) )
if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame:
Printer.red(
'failure in tracking previous frame, # matched map points: ',
self.num_matched_map_points)
self.pose_is_ok = False
if not use_search_frame_by_projection or is_search_frame_by_projection_failure:
self.track_reference_frame(f_ref, f_cur, 'match-frame-frame')
# track camera motion of f_cur w.r.t. f_ref
# estimate motion by matching keypoint descriptors
def track_reference_frame(self, f_ref, f_cur, name=''):
print('>>>> tracking reference %d ...' % (f_ref.id))
if f_ref is None:
return
# find keypoint matches between f_cur and kf_ref
print('matching keypoints with ', Frame.feature_matcher.type.name)
self.timer_match.start()
idxs_cur, idxs_ref = match_frames(f_cur, f_ref)
self.timer_match.refresh()
self.num_matched_kps = idxs_cur.shape[0]
print("# keypoints matched: %d " % self.num_matched_kps)
if kUseEssentialMatrixFitting:
# estimate camera orientation and inlier matches by fitting and essential matrix (see the limitations above)
idxs_ref, idxs_cur = self.estimate_pose_by_fitting_ess_mat(
f_ref, f_cur, idxs_ref, idxs_cur)
if kUseDynamicDesDistanceTh:
self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat(
f_ref, f_cur, idxs_ref, idxs_cur)
# propagate map point matches from kf_ref to f_cur (do not override idxs_ref, idxs_cur)
num_found_map_pts_inter_frame, idx_ref_prop, idx_cur_prop = propagate_map_point_matches(
f_ref,
f_cur,
idxs_ref,
idxs_cur,
max_descriptor_distance=self.descriptor_distance_sigma)
print("# matched map points in prev frame: %d " %
num_found_map_pts_inter_frame)
if kDebugDrawMatches and True:
img_matches = draw_feature_matches(f_ref.img,
f_cur.img,
f_ref.kps[idx_ref_prop],
f_cur.kps[idx_cur_prop],
f_ref.sizes[idx_ref_prop],
f_cur.sizes[idx_cur_prop],
horizontal=False)
cv2.imshow('tracking frame (no projection) - matches', img_matches)
cv2.waitKey(1)
# store tracking info (for possible reuse)
self.idxs_ref = idxs_ref
self.idxs_cur = idxs_cur
# f_cur pose optimization using last matches with kf_ref:
# here, we use first guess of f_cur pose and propated map point matches from f_ref (matched keypoints)
self.pose_optimization(f_cur, name)
# update matched map points; discard outliers detected in last pose optimization
num_matched_points = f_cur.clean_outlier_map_points()
print(' # num_matched_map_points: %d' %
(self.num_matched_map_points))
#print(' # matched points: %d' % (num_matched_points) )
if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame:
f_cur.remove_frame_views(idxs_cur)
f_cur.reset_points()
Printer.red(
'failure in tracking reference %d, # matched map points: %d' %
(f_ref.id, self.num_matched_map_points))
self.pose_is_ok = False
# track camera motion of f_cur w.r.t. given keyframe
# estimate motion by matching keypoint descriptors
def track_keyframe(self, keyframe, f_cur, name='match-frame-keyframe'):
f_cur.update_pose(self.f_ref.pose.copy()
) # start pose optimization from last frame pose
self.track_reference_frame(keyframe, f_cur, name)
def update_local_map(self):
self.f_cur.clean_bad_map_points()
#self.local_points = self.map.local_map.get_points()
self.kf_ref, self.local_keyframes, self.local_points = self.map.local_map.get_frame_covisibles(
self.f_cur)
self.f_cur.kf_ref = self.kf_ref
# track camera motion of f_cur w.r.t. the built local map
# find matches between {local map points} (points in the built local map) and {unmatched keypoints of f_cur}
def track_local_map(self, f_cur):
if self.map.local_map.is_empty():
return
print('>>>> tracking local map...')
self.timer_seach_map.start()
self.update_local_map()
num_found_map_pts, reproj_err_frame_map_sigma, matched_points_frame_idxs = search_map_by_projection(
self.local_points,
f_cur,
max_reproj_distance=self.
reproj_err_frame_map_sigma, #Parameters.kMaxReprojectionDistanceMap,
max_descriptor_distance=self.descriptor_distance_sigma,
ratio_test=Parameters.kMatchRatioTestMap
) # use the updated local map
self.timer_seach_map.refresh()
#print('reproj_err_sigma: ', reproj_err_frame_map_sigma, ' used: ', self.reproj_err_frame_map_sigma)
print("# new matched map points in local map: %d " % num_found_map_pts)
print("# local map points ", self.map.local_map.num_points())
if kDebugDrawMatches and True:
img_matched_trails = f_cur.draw_feature_trails(
f_cur.img.copy(),
matched_points_frame_idxs,
trail_max_length=3)
cv2.imshow('tracking local map - matched trails',
img_matched_trails)
cv2.waitKey(1)
# f_cur pose optimization 2 with all the matched local map points
self.pose_optimization(f_cur, 'proj-map-frame')
f_cur.update_map_points_statistics(
) # here we do not reset outliers; we let them reach the keyframe generation
# and then bundle adjustment will possible decide if remove them or not;
# only after keyframe generation the outliers are cleaned!
print(' # num_matched_map_points: %d' %
(self.num_matched_map_points))
if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackLocalMap:
Printer.red(
'failure in tracking local map, # matched map points: ',
self.num_matched_map_points)
self.pose_is_ok = False
#if kUseDynamicDesDistanceTh:
# self.reproj_err_frame_map_sigma = self.dyn_config.update_reproj_err_map_stat(reproj_err_frame_map_sigma)
# store frame history in order to retrieve the complete camera trajectory
def update_tracking_history(self):
if self.state == SlamState.OK:
isometry3d_Tcr = self.f_cur.isometry3d * self.f_cur.kf_ref.isometry3d.inverse(
) # pose of current frame w.r.t. current reference keyframe kf_ref
self.tracking_history.relative_frame_poses.append(isometry3d_Tcr)
self.tracking_history.kf_references.append(self.kf_ref)
self.tracking_history.timestamps.append(self.f_cur.timestamp)
else:
self.tracking_history.relative_frame_poses.append(
self.tracking_history.relative_frame_poses[-1])
self.tracking_history.kf_references.append(
self.tracking_history.kf_references[-1])
self.tracking_history.timestamps.append(
self.tracking_history.timestamps[-1])
self.tracking_history.slam_states.append(self.state)
def need_new_keyframe(self, f_cur):
num_keyframes = self.map.num_keyframes()
nMinObs = kNumMinObsForKeyFrameDefault
if num_keyframes <= 2:
nMinObs = 2 # if just two keyframes then we can have just two observations
num_kf_ref_tracked_points = self.kf_ref.num_tracked_points(
nMinObs) # number of tracked points in k_ref
num_f_cur_tracked_points = f_cur.num_matched_inlier_map_points(
) # number of inliers in f_cur
Printer.purple('F(%d) #points: %d, KF(%d) #points: %d ' %
(f_cur.id, num_f_cur_tracked_points, self.kf_ref.id,
num_kf_ref_tracked_points))
if kLogKFinfoToFile:
self.kf_info_logger.info(
'F(%d) #points: %d, KF(%d) #points: %d ' %
(f_cur.id, num_f_cur_tracked_points, self.kf_ref.id,
num_kf_ref_tracked_points))
self.num_kf_ref_tracked_points = num_kf_ref_tracked_points
is_local_mapping_idle = self.local_mapping.is_idle()
local_mapping_queue_size = self.local_mapping.queue_size()
print('is_local_mapping_idle: ', is_local_mapping_idle,
', local_mapping_queue_size: ', local_mapping_queue_size)
# condition 1: more than "max_frames_between_kfs" have passed from last keyframe insertion
cond1 = f_cur.id >= (self.kf_last.id + self.max_frames_between_kfs)
# condition 2: more than "min_frames_between_kfs" have passed and local mapping is idle
cond2 = (f_cur.id >=
(self.kf_last.id +
self.min_frames_between_kfs)) & is_local_mapping_idle
#cond2 = (f_cur.id >= (self.kf_last.id + self.min_frames_between_kfs))
# condition 3: few tracked features compared to reference keyframe
cond3 = (num_f_cur_tracked_points <
num_kf_ref_tracked_points * Parameters.kThNewKfRefRatio) and (
num_f_cur_tracked_points >
Parameters.kNumMinPointsForNewKf)
#print('KF conditions: %d %d %d' % (cond1, cond2, cond3) )
ret = (cond1 or cond2) and cond3
if ret:
if is_local_mapping_idle:
return True
else:
self.local_mapping.interrupt_optimization()
if True:
if local_mapping_queue_size <= 3:
return True
else:
return False
else:
return False
else:
return False
# @ main track method @
def track(self, img, frame_id, timestamp=None):
Printer.cyan('@tracking')
time_start = time.time()
# check image size is coherent with camera params
print("img.shape: ", img.shape)
print("camera ", self.camera.height, " x ", self.camera.width)
assert img.shape[0:2] == (self.camera.height, self.camera.width)
if timestamp is not None:
print('timestamp: ', timestamp)
self.timer_main_track.start()
# build current frame
self.timer_frame.start()
f_cur = Frame(img, self.camera, timestamp=timestamp)
self.f_cur = f_cur
print("frame: ", f_cur.id)
self.timer_frame.refresh()
# reset indexes of matches
self.idxs_ref = []
self.idxs_cur = []
if self.state == SlamState.NO_IMAGES_YET:
# push first frame in the inizializer
self.intializer.init(f_cur)
self.state = SlamState.NOT_INITIALIZED
return # EXIT (jump to second frame)
if self.state == SlamState.NOT_INITIALIZED:
# try to inizialize
initializer_output, intializer_is_ok = self.intializer.initialize(
f_cur, img)
if intializer_is_ok:
kf_ref = initializer_output.kf_ref
kf_cur = initializer_output.kf_cur
# add the two initialized frames in the map
self.map.add_frame(
kf_ref) # add first frame in map and update its frame id
self.map.add_frame(
kf_cur) # add second frame in map and update its frame id
# add the two initialized frames as keyframes in the map
self.map.add_keyframe(
kf_ref) # add first keyframe in map and update its kid
self.map.add_keyframe(
kf_cur) # add second keyframe in map and update its kid
kf_ref.init_observations()
kf_cur.init_observations()
# add points in map
new_pts_count, _, _ = self.map.add_points(
initializer_output.pts,
None,
kf_cur,
kf_ref,
initializer_output.idxs_cur,
initializer_output.idxs_ref,
img,
do_check=False)
Printer.green("map: initialized %d new points" %
(new_pts_count))
# update covisibility graph connections
kf_ref.update_connections()
kf_cur.update_connections()
# update tracking info
self.f_cur = kf_cur
self.f_cur.kf_ref = kf_ref
self.kf_ref = kf_cur # set reference keyframe
self.kf_last = kf_cur # set last added keyframe
self.map.local_map.update(self.kf_ref)
self.state = SlamState.OK
self.update_tracking_history()
self.motion_model.update_pose(kf_cur.timestamp,
kf_cur.position,
kf_cur.quaternion)
self.motion_model.is_ok = False # after initialization you cannot use motion model for next frame pose prediction (time ids of initialized poses may not be consecutive)
self.intializer.reset()
if kUseDynamicDesDistanceTh:
self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat(
kf_ref, kf_cur, initializer_output.idxs_ref,
initializer_output.idxs_cur)
return # EXIT (jump to next frame)
# get previous frame in map as reference
f_ref = self.map.get_frame(-1)
#f_ref_2 = self.map.get_frame(-2)
self.f_ref = f_ref
# add current frame f_cur to map
self.map.add_frame(f_cur)
self.f_cur.kf_ref = self.kf_ref
# reset pose state flag
self.pose_is_ok = False
with self.map.update_lock:
# check for map point replacements in previous frame f_ref (some points might have been replaced by local mapping during point fusion)
self.f_ref.check_replaced_map_points()
if kUseDynamicDesDistanceTh:
print('descriptor_distance_sigma: ',
self.descriptor_distance_sigma)
self.local_mapping.descriptor_distance_sigma = self.descriptor_distance_sigma
# udpdate (velocity) old motion model # c1=ref_ref, c2=ref, c3=cur; c=cur, r=ref
#self.velocity = np.dot(f_ref.pose, inv_T(f_ref_2.pose)) # Tc2c1 = Tc2w * Twc1 (predicted Tcr)
#self.predicted_pose = g2o.Isometry3d(np.dot(self.velocity, f_ref.pose)) # Tc3w = Tc2c1 * Tc2w (predicted Tcw)
# set intial guess for current pose optimization
if kUseMotionModel and self.motion_model.is_ok:
print('using motion model for next pose prediction')
# update f_ref pose according to its reference keyframe (the pose of the reference keyframe could be updated by local mapping)
self.f_ref.update_pose(
self.tracking_history.relative_frame_poses[-1] *
self.f_ref.kf_ref.isometry3d)
# predict pose by using motion model
self.predicted_pose, _ = self.motion_model.predict_pose(
timestamp, self.f_ref.position, self.f_ref.orientation)
f_cur.update_pose(self.predicted_pose)
else:
print('setting f_cur.pose <-- f_ref.pose')
# use reference frame pose as initial guess
f_cur.update_pose(f_ref.pose)
# track camera motion from f_ref to f_cur
self.track_previous_frame(f_ref, f_cur)
if not self.pose_is_ok:
# if previous track didn't go well then track the camera motion from kf_ref to f_cur
self.track_keyframe(self.kf_ref, f_cur)
# now, having a better estimate of f_cur pose, we can find more map point matches:
# find matches between {local map points} (points in the local map) and {unmatched keypoints of f_cur}
if self.pose_is_ok:
self.track_local_map(f_cur)
# end block {with self.map.update_lock:}
# TODO: add relocalization
# HACK: since local mapping is not fast enough in python (and tracking is not in real-time) => give local mapping more time to process stuff
self.wait_for_local_mapping(
) # N.B.: this must be outside the `with self.map.update_lock:` block
with self.map.update_lock:
# update slam state
if self.pose_is_ok:
self.state = SlamState.OK
else:
self.state = SlamState.LOST
Printer.red('tracking failure')
# update motion model state
self.motion_model.is_ok = self.pose_is_ok
if self.pose_is_ok: # if tracking was successful
# update motion model
self.motion_model.update_pose(timestamp, f_cur.position,
f_cur.quaternion)
f_cur.clean_vo_map_points()
# do we need a new KeyFrame?
need_new_kf = self.need_new_keyframe(f_cur)
if need_new_kf:
Printer.green('adding new KF with frame id % d: ' %
(f_cur.id))
if kLogKFinfoToFile:
self.kf_info_logger.info(
'adding new KF with frame id % d: ' % (f_cur.id))
kf_new = KeyFrame(f_cur, img)
self.kf_last = kf_new
self.kf_ref = kf_new
f_cur.kf_ref = kf_new
self.local_mapping.push_keyframe(kf_new)
if not kLocalMappingOnSeparateThread:
self.local_mapping.do_local_mapping()
else:
Printer.yellow('NOT KF')
# From ORBSLAM2:
# Clean outliers once keyframe generation has been managed:
# we allow points with high innovation (considered outliers by the Huber Function)
# pass to the new keyframe, so that bundle adjustment will finally decide
# if they are outliers or not. We don't want next frame to estimate its position
# with those points so we discard them in the frame.
f_cur.clean_outlier_map_points()
if self.f_cur.kf_ref is None:
self.f_cur.kf_ref = self.kf_ref
self.update_tracking_history(
) # must stay after having updated slam state (self.state)
Printer.green("map: %d points, %d keyframes" %
(self.map.num_points(), self.map.num_keyframes()))
#self.update_history()
self.timer_main_track.refresh()
duration = time.time() - time_start
print('tracking duration: ', duration)
# Since we do not have real-time performances, we can slow-down things and make tracking wait till local mapping gets idle
# N.B.: this function must be called outside 'with self.map.update_lock' blocks,
# since both self.track() and the local-mapping optimization use the RLock 'map.update_lock'
# => they cannot wait for each other once map.update_lock is locked (deadlock)
def wait_for_local_mapping(self):
if kTrackingWaitForLocalMappingToGetIdle:
#while not self.local_mapping.is_idle() or self.local_mapping.queue_size()>0:
if not self.local_mapping.is_idle():
print('>>>> waiting for local mapping...')
self.local_mapping.wait_idle()
else:
if not self.local_mapping.is_idle(
) and kTrackingWaitForLocalMappingSleepTime > 0:
print('>>>> sleeping for local mapping...')
time.sleep(kTrackingWaitForLocalMappingSleepTime)
# close the log to prevent locking on next call
Logger.Instance().CloseLog()
logFile = None
# make sure we leave no references behind
AddonSettings.ClearCachedAddonSettingsObject()
except:
if logFile:
logFile.Critical("Error running plugin", exc_info=True)
raise
# setup the paths in Python
from initializer import Initializer # nopep8
Initializer.SetUnicode()
currentPath = Initializer.SetupPythonPaths()
# ANY OF THESE SETTINGS SHOULD ONLY BE ENABLED FOR DEBUGGING PURPOSES
# from debug import remotedebugger
# debugger = remotedebugger.RemoteDebugger()
# import profile as cProfile
# import cProfile
# from debug import profilelinebyline as cProfile
# Path for PC
# statsPath = os.path.abspath(os.path.join(currentPath, "../../DEV/retrospect.pc.pstat"))
# Path for ATV
# statsPath = os.path.abspath("/private/var/mobile/retrospect.atv.pstat")
# Path for rPi
class BrainsphereModel:
def __init__(self, functional_connectivity, patient_data, **kwargs):
self.types = [
'ConcentrationLinear', 'Constant', 'ConcentrationSigmoid',
'WeightedDegreeLinear', 'WeightedDegreeSigmoid'
]
self.producer = Producer(self.types)
self.params = self.producer.params
for key, value in kwargs.items():
if key == "nodeCoordinates":
self.nodeCoordinates = value
elif key == "optimizer":
self.optimizer = value
elif key == "loss":
self.loss = value
elif key == "euclideanAdjacency":
self.euclideanAdjacency = value
elif key == "producer":
self.producer = value
elif key == "diffuser":
self.diffuser = value
elif key == "params":
self.params.update(value)
else:
raise TypeError("Illegal Keyword '" + str(key) + "'")
self.functionalConnectivity = functional_connectivity
self.patientData = patient_data
self.numNodes, _ = np.shape(functional_connectivity)
self.loss = Loss("mse", self.patientData)
self.lastloss = 0
self.reset()
def reset(self):
self.initializer = Initializer("braak1", self.numNodes, self.params)
self.concentration = self.initializer.get()
self.concentrationHistory = np.copy(self.concentration)
self.producer = Producer(self.types)
self.diffusor = Diffusor("euclidean",
self.params,
EuclideanAdjacency=self.euclideanAdjacency)
def run(self):
stop_concentration = 1100
timesteps = 2500
self.reset()
deltaT = 0.0001
self.concentration += deltaT * (
self.producer.produce(params=self.params,
concentration=self.concentration,
connectivity=self.functionalConnectivity) +
self.diffusor.diffuse(self.concentration))
self.concentrationHistory = np.append(self.concentrationHistory,
self.concentration,
axis=0)
deltaConc = np.sum(self.concentrationHistory[1, :]) - np.sum(
self.concentrationHistory[0, :])
if deltaConc <= 0.0:
return 9999999
else:
deltaT *= stop_concentration / timesteps / deltaConc
while (np.sum(self.concentration) <
stop_concentration) and (np.sum(deltaConc) > 0):
deltaConc = deltaT * (self.producer.produce(
params=self.params,
concentration=self.concentration,
connectivity=self.functionalConnectivity) +
self.diffusor.diffuse(self.concentration))
self.concentration += deltaConc
self.concentrationHistory = np.append(self.concentrationHistory,
self.concentration,
axis=0)
# print(self.loss.get(self.concentrationHistory))
self.lastloss = self.loss.get(self.concentrationHistory)
return self.lastloss
def gradient(self, loss=None):
if loss is None:
loss = self.run()
params_new = {}
params_old = self.params.copy()
deltaX = {}
for key, value in params_old.items():
deltaX[key] = np.sign(np.random.randn()) * 0.01
params_new[key] = value + deltaX[key]
self.params = params_new
new_loss = self.run()
grad = {}
self.params = params_old
for key, value in params_old.items():
grad[key] = (new_loss - loss) / (deltaX[key])
return grad
def gradient4(self):
loss = self.run()
gradients = Parallel(n_jobs=4)(delayed(self.gradient)(loss)
for i in range(4))
grad = {}
for key in self.params:
gradsum = 0
count = 0
for g in gradients:
gradsum += g.get(key)
count += 1.0
grad[key] = gradsum / count
return grad
import random
import warnings
import datetime
import config
import numpy as np
from initializer import Initializer
if __name__ == '__main__':
random.seed(0)
np.random.seed(0)
initializer = Initializer(num_clients=config.NUM_CLIENTS,
iterations=config.ITERATIONS,
num_servers=config.NUM_SERVERS)
# can use any amount of iterations less than config.ITERATIONS but the
# initializer has only given each client config.ITERATIONS datasets for training.
a = datetime.datetime.now()
initializer.run_simulation(config.ITERATIONS,
server_agent_name='server_agent0')
b = datetime.datetime.now()
events = requests.get(url).json()
next_url = events["next"]
self._on_http_load(events, depth)
self._deep_load(next_url, depth+1)
def _on_http_load(self, events, page):
name = "test/kudago{}.json".format(str(page))
logging.debug("Save events to {}".format(name))
with open(name, "w") as fd:
json.dump(events, fd)
if __name__ == "__main__":
logging.basicConfig(level=logging.DEBUG)
context = Initializer.get_context()
se = context.get_se()
kudago_cfg = context.get_kudago_cfg()
es_cfg = context.get_es_cfg()
loader = EventLoader(se, kudago_cfg)
# update from kudago
loader.http_load()
# load files to index
se.delete_index(es_cfg["index"])
se.create_index(es_cfg)
loader.load()
#===============================================================================
import os
import sys
import xbmc
import xbmcgui
# we need to import the initializer
addOnPath = os.path.abspath(os.path.join(os.path.dirname(__file__), "..",
".."))
sys.path.append(addOnPath)
# setup some initial stuff
from initializer import Initializer
Initializer.set_unicode()
Initializer.setup_python_paths()
sys.path.remove(addOnPath)
from retroconfig import Config
from logger import Logger
Logger.create_logger(os.path.join(Config.profileDir, Config.logFileNameAddon),
Config.appName,
append=True,
dual_logger=lambda x, y=4: xbmc.log(x, y))
from helpers.htmlentityhelper import HtmlEntityHelper
from addonsettings import AddonSettings, LOCAL
from favourites import Favourites
from paramparser import ParameterParser
def __init__(self, system):
if kDebugDrawMatches:
Frame.is_store_imgs = True
self.system = system
self.camera = system.camera
self.map = system.map
self.local_mapping = system.local_mapping
self.intializer = Initializer()
self.motion_model = MotionModel(
) # motion model for current frame pose prediction without damping
#self.motion_model = MotionModelDamping() # motion model for current frame pose prediction with damping
self.dyn_config = SLAMDynamicConfig()
self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance
self.reproj_err_frame_map_sigma = Parameters.kMaxReprojectionDistanceMap
self.max_frames_between_kfs = int(system.camera.fps)
self.min_frames_between_kfs = 0
self.state = SlamState.NO_IMAGES_YET
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched points
self.num_matched_map_points = None # current number of matched map points (matched and found valid in current pose optimization)
self.num_kf_ref_tracked_points = None # number of tracked points in k_ref (considering a minimum number of observations)
self.mask_match = None
self.pose_is_ok = False
self.predicted_pose = None
self.velocity = None
self.f_cur = None
self.idxs_cur = None
self.f_ref = None
self.idxs_ref = None
self.kf_ref = None # reference keyframe (in general, different from last keyframe depending on the used approach)
self.kf_last = None # last keyframe
self.kid_last_BA = -1 # last keyframe id when performed BA
self.local_keyframes = [] # local keyframes
self.local_points = [] # local points
self.tracking_history = TrackingHistory()
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_main_track = TimerFps('Track',
is_verbose=self.timer_verbose)
self.timer_pose_opt = TimerFps('Pose optimization',
is_verbose=self.timer_verbose)
self.timer_seach_frame_proj = TimerFps('Search frame by proj',
is_verbose=self.timer_verbose)
self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('Ess mat pose estimation',
is_verbose=self.timer_verbose)
self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose)
self.timer_seach_map = TimerFps('Search map',
is_verbose=self.timer_verbose)
self.init_history = True
self.poses = [] # history of poses
self.t0_est = None # history of estimated translations
self.t0_gt = None # history of ground truth translations (if available)
self.traj3d_est = [
] # history of estimated translations centered w.r.t. first one
self.traj3d_gt = [
] # history of estimated ground truth translations centered w.r.t. first one
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.trueX, self.trueY, self.trueZ = None, None, None
self.groundtruth = system.groundtruth # not actually used here; could be used for evaluating performances
if kLogKFinfoToFile:
self.kf_info_logger = Logging.setup_file_logger(
'kf_info_logger',
'kf_info.log',
formatter=Logging.simple_log_formatter)
def __init__(self, handler_class):
self.context = Initializer.get_context()
http_cfg = self.context.get_http_cfg()
HTTPServer.__init__(self, (http_cfg["host"], http_cfg["port"]), handler_class)
