def locally_optimize(self,
local_window=parameters.kLocalWindow,
verbose=False,
rounds=10):
frames, points, frames_ref = self.update_local_window_map(local_window)
print('local optimization window: ', [f.id for f in frames])
print(' refs: ', [f.id for f in frames_ref])
#print(' points: ', sorted([p.id for p in points]))
#err = optimizer_g2o.optimize(frames, points, None, False, verbose, rounds)
err, ratio_bad_observations = optimizer_g2o.localOptimization(
frames, points, frames_ref, False, verbose, rounds)
Printer.green('local optimization - perc bad observations: %.2f %% ' %
(ratio_bad_observations * 100))
#self.cull_map_points(points) # already performed in optimizer_g2o.localOptimization()
return err
def refresh(self):
elapsed = self.elapsed()
self.moving_average.getAverage(elapsed)
self.start()
if self._is_verbose is True:
dT = self.moving_average.getAverage()
name = self._name
if self._is_paused:
name += ' [paused]'
message = 'Timer::' + name + ' - fps: ' + str(
1. / dT) + ', T: ' + str(dT)
if kPrintGreen is True:
Printer.green(message)
else:
print(message)
def elapsed(self):
if self._is_paused:
self._elapsed = self._accumulated
else:
now = cv2.getTickCount()
self._elapsed = self._accumulated + (
now - self._start_time) / cv2.getTickFrequency()
if self._is_verbose is True:
name = self._name
if self._is_paused:
name += ' [paused]'
message = 'Timer::' + name + ' - elapsed: ' + str(self._elapsed)
if kPrintGreen is True:
Printer.green(message)
else:
print(message)
return self._elapsed
def __init__(self,
min_num_features=kMinNumFeatureDefault,
num_levels=3,
detector_type=FeatureDetectorTypes.FAST,
descriptor_type=FeatureDescriptorTypes.NONE,
tracker_type=TrackerTypes.LK):
if num_levels < 3:
Printer.green('LkFeatureTracker: forcing at least 3 levels')
num_levels = 3
super().__init__(min_num_features, num_levels, detector_type,
descriptor_type, tracker_type)
self.detector = feature_detector_factory(min_num_features, num_levels,
detector_type,
descriptor_type)
# we use LK pyr optic flow for matching
self.lk_params = dict(winSize=(21, 21),
maxLevel=self.num_levels,
criteria=(cv2.TERM_CRITERIA_EPS
| cv2.TERM_CRITERIA_COUNT, 30, 0.01))
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()
data_test = pd.read_csv("test.csv")
train_labels = data['Survived']
test_ids = data_test['PassengerId']
def fix_frame(frame): # Do some cleaning
frame['Age'] = frame['Age'].fillna(20)
frame['Pclass'] = frame['Pclass'].astype('category') # By default pandas doesn't encode numbers - but Pclass is probably better as a category - we want to one hot encode it
return frame
features = ["Pclass","Sex","Age","SibSp","Embarked","Cabin"]
train = fix_frame(data[features])
test = fix_frame(data_test[features])
pipe = skpipe.Pipeline([
('printer1',Printer()), # You can delete the printers - they are there to help you
('to_numbers',PandasEncoder()), # One hot encodes a pandas frame - everything except numbers
('printer2',Printer()),
('random_forest',RandomForestClassifier(n_estimators=100))
])
score = np.mean(cv.cross_val_score(pipe,train,train_labels,cv=5,n_jobs=1)) # set n_jobs to -1 to run each test on a different processor - much faster
pipe.fit(train,train_labels)
predictions = pipe.predict(test)
pd.DataFrame(predictions,index=test_ids,columns=["Survived"]).to_csv("submission.csv")
print("Estimated score over 5 tests:",score)
print("Done writing to file")
def poseOptimization(frame, verbose=False, rounds=10):
is_ok = True
# create g2o optimizer
opt = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
opt.set_algorithm(solver)
#robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991)) # chi-square 2 DOFs
thHuberMono = math.sqrt(5.991)
# chi-square 2 DOFS
point_edge_pairs = {}
num_point_edges = 0
se3 = g2o.SE3Quat(frame.pose[0:3, 0:3], frame.pose[0:3, 3])
v_se3 = g2o.VertexSE3Expmap()
v_se3.set_estimate(se3)
v_se3.set_id(0)
v_se3.set_fixed(False)
opt.add_vertex(v_se3)
# add point vertices to graph
for idx, p in enumerate(frame.points):
if p is None: # do not use p.is_bad here since a single point observation is ok for pose optimization
continue
frame.outliers[idx] = False
# add edge
#print('adding edge between point ', p.id,' and frame ', frame.id)
edge = g2o.EdgeSE3ProjectXYZOnlyPose()
edge.set_vertex(0, opt.vertex(0))
edge.set_measurement(frame.kpsu[idx])
invSigma2 = Frame.detector.inv_level_sigmas2[frame.octaves[idx]]
edge.set_information(np.eye(2) * invSigma2)
edge.set_robust_kernel(g2o.RobustKernelHuber(thHuberMono))
edge.fx = frame.fx
edge.fy = frame.fy
edge.cx = frame.cx
edge.cy = frame.cy
edge.Xw = p.pt[0:3]
opt.add_edge(edge)
point_edge_pairs[p] = (edge, idx) # one edge per point
num_point_edges += 1
if num_point_edges < 3:
Printer.red('poseOptimization: not enough correspondences!')
is_ok = False
return 0, is_ok, 0
if verbose:
opt.set_verbose(True)
# We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
# At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
chi2Mono = 5.991 # chi-square 2 DOFs
num_bad_points = 0
for it in range(4):
opt.initialize_optimization()
opt.optimize(rounds)
num_bad_points = 0
for p, edge_pair in point_edge_pairs.items():
if frame.outliers[edge_pair[1]] is True:
edge_pair[0].compute_error()
chi2 = edge_pair[0].chi2()
if chi2 > chi2Mono:
frame.outliers[edge_pair[1]] = True
edge_pair[0].set_level(1)
num_bad_points += 1
else:
frame.outliers[edge_pair[1]] = False
edge_pair[0].set_level(0)
if it == 2:
edge_pair[0].set_robust_kernel(None)
if len(opt.edges()) < 10:
Printer.red('poseOptimization: stopped - not enough edges!')
is_ok = False
break
print('pose optimization: initial ', num_point_edges, ' points, found ',
num_bad_points, ' bad points')
if num_point_edges == num_bad_points:
Printer.red(
'poseOptimization: all the initial correspondences are bad!')
is_ok = False
# update pose estimation
if is_ok is True:
est = v_se3.estimate()
R = est.rotation().matrix()
t = est.translation()
frame.pose = poseRt(R, t)
num_valid_points = num_point_edges - num_bad_points
return opt.active_chi2(), is_ok, num_valid_points
def search_frame_for_triangulation(f1, f2, img2, img1 = None):
idxs2_out = []
idxs1_out = []
lines_out = []
num_found_matches = 0
img2_epi = None
if __debug__:
timer = Timer()
timer.start()
f1.update_camera_pose()
f2.update_camera_pose()
O1w = f1.Ow
O2w = f2.Ow
# compute epipoles
e1 = f1.project_point(O2w).ravel() # in first frame
e2 = f2.project_point(O1w).ravel() # in second frame
#print('e1: ', e1)
#print('e2: ', e2)
# if the translation is too small we cannot triangulate
if np.linalg.norm(O1w-O2w) < parameters.kMinTraslation: # we assume the Inializer has been used for building the first map
Printer.red("search for triangulation: impossible with zero translation!")
return idxs1_out, idxs2_out, num_found_matches, img2_epi # EXIT
# compute the fundamental matrix between the two frames
F12, H21 = computeF12(f1, f2)
idxs1 = []
for i, p in enumerate(f1.points):
if p is None: # we consider just unmatched keypoints
kp = f1.kpsu[i]
scale_factor = Frame.detector.scale_factors[f1.octaves[i]]
# discard points which are too close to the epipole
if np.linalg.norm(kp-e1) < parameters.kMinDistanceFromEpipole * scale_factor:
continue
idxs1.append(i)
kpsu1 = f1.kpsu[idxs1]
# compute epipolar lines in second image
lines2 = cv2.computeCorrespondEpilines(kpsu1.reshape(-1,1,2), 2, F12)
lines2 = lines2.reshape(-1,3)
xs2_inf = np.dot(H21, add_ones(kpsu1).T).T # x2inf = H21 * x1 where x2inf is corresponding point according to infinite homography [Hartley Zisserman pag 339]
xs2_inf = xs2_inf[:, 0:2] / xs2_inf[:, 2:]
line_edges = [ epiline_to_end_points(line, e2, x2_inf, f2.W) for line, x2_inf in zip(lines2, xs2_inf) ]
#print("line_edges: ", line_edges)
if __debug__:
print('search_frame_for_triangulation - timer1: ', timer.elapsed())
assert(len(line_edges) == len(idxs1))
timer.start()
len_des2 = len(f2.des)
flag_match = np.full(len_des2, False, dtype=bool)
dist_match = np.zeros(len_des2)
index_match = np.full(len_des2, 0, dtype=int)
for i, idx in enumerate(idxs1): # N.B.: a point in f1 can be matched to more than one point in f2, we avoid this by caching matches with f2 points
f2_idx, dist = find_matches_along_line(f2, e2, line_edges[i], f1.des[idx])
if f2_idx > -1:
if not flag_match[f2_idx]: # new match
flag_match[f2_idx] = True
dist_match[f2_idx] = dist
idxs2_out.append(f2_idx)
idxs1_out.append(idx)
index_match[f2_idx] = len(idxs2_out)-1
assert(f2.points[f2_idx] is None)
assert(f1.points[idx] is None)
if __debug__:
lines_out.append(line_edges[i])
else: # already matched
if dist < dist_match[f2_idx]: # update in case of a smaller distance
dist_match[f2_idx] = dist
index = index_match[f2_idx]
idxs1_out[index] = idx
if __debug__:
lines_out[index] = line_edges[i]
num_found_matches = len(idxs1_out)
assert(len(idxs1_out) == len(idxs2_out))
if __debug__:
#print("num found matches: ", num_found_matches)
if True:
kpsu2 = f2.kpsu[idxs2_out]
img2_epi = draw_lines(img2.copy(), lines_out, kpsu2)
#cv2.imshow("epipolar lines",img2_epi)
#cv2.waitKey(0)
if __debug__:
print('search_frame_for_triangulation - timer2: ', timer.elapsed())
return idxs1_out, idxs2_out, num_found_matches, img2_epi
def initialize(self, f_cur, img_cur):
# prepare the output
out = InitializerOutput()
is_ok = False
# if too many frames have passed, move the current id_ref forward
if (len(self.frames) - 1) - self.id_ref >= kMaxIdDistBetweenFrames:
self.id_ref = len(self.frames) - 1 # take last frame in the array
self.f_ref = self.frames[self.id_ref]
f_ref = self.f_ref
# append current frame
self.frames.append(f_cur)
# if the current frames do no have enough features exit
if len(f_ref.kps) < kNumMinFeatures or len(
f_cur.kps) < kNumMinFeatures:
Printer.red('Inializer: not enough features!')
return out, is_ok
# find image point matches
idx_cur, idx_ref = match_frames(f_cur, f_ref)
print('├────────')
print('initializing frames ', f_cur.id, ', ', f_ref.id)
Mrc = self.estimatePose(f_ref.kpsn[idx_ref], f_cur.kpsn[idx_cur])
f_cur.pose = np.linalg.inv(poseRt(
Mrc[:3, :3], Mrc[:3, 3])) # [Rcr, tcr] w.r.t. ref frame
# remove outliers
mask_index = [i for i, v in enumerate(self.mask_match) if v > 0]
print('num inliers: ', len(mask_index))
idx_cur_inliers = idx_cur[mask_index]
idx_ref_inliers = idx_ref[mask_index]
# create a temp map for initializing
map = Map()
map.add_frame(f_ref)
map.add_frame(f_cur)
points4d = self.triangulatePoints(f_cur.pose, f_ref.pose,
f_cur.kpsn[idx_cur_inliers],
f_ref.kpsn[idx_ref_inliers])
#pts4d = triangulate(f_cur.pose, f_ref.pose, f_cur.kpsn[idx_cur], f_ref.kpsn[idx_ref])
new_pts_count, mask_points = map.add_points(points4d,
None,
f_cur,
f_ref,
idx_cur_inliers,
idx_ref_inliers,
img_cur,
check_parallax=True)
print("triangulated: %d new points from %d matches" %
(new_pts_count, len(idx_cur)))
err = map.optimize(verbose=False)
print("pose opt err: %f" % err)
#reset points in frames
f_cur.reset_points()
f_ref.reset_points()
num_map_points = len(map.points)
#print("# map points: %d" % num_map_points)
is_ok = num_map_points > kNumMinTriangulatedPoints
out.points4d = points4d[mask_points]
out.f_cur = f_cur
out.idx_cur = idx_cur_inliers[mask_points]
out.f_ref = f_ref
out.idx_ref = idx_ref_inliers[mask_points]
# set median depth to 'desired_median_depth'
desired_median_depth = parameters.kInitializerDesiredMedianDepth
median_depth = f_cur.compute_points_median_depth(out.points4d)
depth_scale = desired_median_depth / median_depth
print('median depth: ', median_depth)
out.points4d = out.points4d * depth_scale # scale points
f_cur.pose[:3,
3] = f_cur.pose[:3,
3] * depth_scale # scale initial baseline
print('├────────')
Printer.green('Inializer: ok!')
return out, is_ok