def __init__(self):
super().__init__()
# g2o::BlockSlover_6_3(g2o::BlockSolver_6_3::LinearSolverType*)
linear_solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
solver = g2o.OptimizationAlgorithmLevenberg(linear_solver)
super().set_algorithm(solver)
# additional parameters
#
self._map = None
#
self.vertex_seq_generator = AtomicCounter()
self.edge_seq_generator = AtomicCounter()
# Point | Frame | Landmark -> Vertex mapping
# inverse vertex query
self._ivq = {}
# (Vertex, Vetex) -> Edge mapping, a sparse matrix
# inverse edges query
self._ieq = {}
#
self.USE_LANDMARKS = False
class CameraPoint3D(MapPoint):
Seq = AtomicCounter()
def __init__(self, x, y, z):
super().__init__()
self.identity = Identity(CameraPoint3D.Seq())
self.p = Point3D(x, y, z)
self.triangulated = False
self.parent = None
self.px = None
self.world = None
def set_world(self, world_point):
self.world = world_point
return self
def set_px(self, px):
self.px = px
return self
def __str__(self):
return "<CameraPoint3D %d: %.3f, %.3f, %.3f>" % (
self.identity.seq, self.x, self.y, self.z)
class WorldPoint3D(MapPoint):
Seq = AtomicCounter()
def __init__(self, x, y, z):
super().__init__()
self.identity = Identity(WorldPoint3D.Seq())
self.p = Point3D(x, y, z)
# FrameKey = np.uint64
# PixelPos = np.uint32
self.observations = {}
self.parent = None
self.color = None
def set_color(self, color):
self.color = color
return self
def associate_with(self, frame, pixel_pos):
last_pos = self.observations.get(frame.seq, None)
px = frame.pixels.get(pixel_pos, None)
if px is None:
raise Exception("The pixel is not recorded by the frame!")
if last_pos is None:
self.observations[frame.seq] = pixel_pos
else:
if last_pos != pixel_pos:
print("pos %d is different from last_pos %d" %
(pixel_pos, last_pos))
print(
"Pixel(frame=%d, r=%d, c=%d, pixel_pos=%d) mapped points: "
% (frame.seq, px.r, px.c, pixel_pos))
for p in px.cam_pt_array:
w = p.world
print(w)
print("\n")
legacy_px = frame.pixels.get(last_pos)
print(
"Pixel(frame=%d, r=%d, c=%d, last_pos=%d) mapped points: "
% (frame.seq, legacy_px.r, legacy_px.c, last_pos))
for p in legacy_px.cam_pt_array:
w = p.world
print(w)
raise Exception(
"The point %s has already been mapped to a different palce"
% self)
return self
def __str__(self):
return "<WorldPoint3D %d: %.3f, %.3f, %.3f>" % (self.identity.seq,
self.x, self.y, self.z)
def __init__(self):
super().__init__()
# g2o::BlockSlover_6_3(g2o::BlockSolver_6_3::LinearSolverType*)
linear_solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.OptimizationAlgorithmLevenberg(linear_solver)
super().set_algorithm(solver)
# additional parameters
terminate = g2o.SparseOptimizerTerminateAction()
terminate.set_gain_threshold(1e-6)
super().add_post_iteration_action(terminate)
#
self._map = None
#
self.frame = None
#
self.points = None
#
self.vSE3 = None
#
self.measurements = None
#
self.vertex_seq_generator = AtomicCounter()
self.edge_seq_generator = AtomicCounter()
# Point | Frame | Landmark -> Vertex mapping
# inverse vertex query
self._ivq = {}
# (Vertex, Vetex) -> Edge mapping, a sparse matrix
# inverse edges query
self._ieq = {}
#
self.USE_LANDMARKS = False
#
self.edges = []
class Worker(threading.Thread):
logger = LogAdaptor("Worker", _logger)
Seq = AtomicCounter()
def __init__(self, tasks, name="worker"):
# identity
self.seq = self.Seq()
self.tid = None
threading.Thread.__init__(self, name=name)
self.tasks = tasks
self.daemon = True
self._lock = threading.Lock()
# implement event loop interface
# close event
self._stopped = False
# stop event
self._stopping = False
print("[Thread %d], Starting ..." % (self.seq, ))
self.start()
def set_stopping(self):
# might be set from multiple locations
print("[Thread %d], setting to stop." % (self.seq, ))
with self._lock:
self._stopping = True
print("[Thread %d], set to stop." % (self.seq, ))
def isStopping(self):
return self._stopping
def run_forever(self):
self.tid = SelfThread()
while not self.isStopping():
self._run_once()
self._stopped = True
print("[Thread %d] (system tid %d) stopped." % (self.seq, self.tid))
def _run_once(self):
func, args, kw = self.tasks.get()
try:
func(*args, **kw)
except RuntimeErr as e:
self.logger.error(e)
finally:
self.tasks.task_done()
# for asynchronous events implements linux epoll based _run_once and run_untile_complete
def run(self):
self.run_forever()
class Point3D(Vec3):
Seq = AtomicCounter()
def __init__(self, x, y, z=0):
super().__init__(x, y, z)
self.identity = Identity(Point3D.Seq())
@property
def seq(self):
return self.identity.seq
@seq.setter
def seq(self, val):
self.identity.seq = val
return self
def __str__(self):
return "<Point3D %d: %.3f, %.3f, %.3f>" % (self.identity.seq, self.x,
self.y, self.z)
class Observation:
Seq = AtomicCounter()
def __init__(self, label, roi_kps, roi_features):
# label
self.label = label
# score
self.score = roi_features['score']
# roi keypoints
self.roi_kps = roi_kps
# roi feature
self.roi_features = roi_features
# associated frame
self.frame = None
## Updated while tracking ...
#
self.projected_pos = roi_features['box']
#
self.projected_mask = roi_features['mask']
# key points used to reconstruct struction in 3d world
self.points = {}
#
self.bbox = roi_features['box']
## Covisibility Graph Topology
# recording upgraded 3dpoints for each ROI
self.observations = {}
## Identity information
self.seq = self.Seq()
self.id = None
self.uuid = uuid.uuid4()
# computed key used to uniquely identify a key point
self.key = None
# parameters used for tracking
self.kf = None
# self.opticalPredictor = None
self.predicted_states = self.projected_pos # None
# union set, here i uses Uncompressed Union Set to present films of the object
self.parent = None
self.records = []
# rendering attributes
self.color = None
# AABB
self._aabb = None
# OBB
self._Obb = None
self._major = None
self._minor = None
self._W = None
# Centroid
self._centroid = None
def Init(self):
self.kf = BBoxKalmanFilter()
# self.opticalPredictor = OpticalFlowBBoxPredictor()
self.records.append((
None, # Observation
self.frame.seq if hasattr(self, "frame") else -1, # frameId
))
return self
def clone(self):
ob = Observation(self.label, self.roi_kps, self.roi_features)
ob.set_FromFrame(self.frame)
for key, val in self.observations.items():
ob.observations[key] = val
#
for k, p in self.points.items():
ob.points[k] = p
ob.Init()
ob.update(self)
ob.predicted_states = ob.projected_pos
ob.color = self.color
return ob
# remove points based on depth distribution
def culling(self):
points = []
for k, p in self.points.items():
points.append(p)
points = sorted(points, key=lambda p: p.z)
# remove dpeth %5 smallest, and 95% biggest points
l = len(points)
if l == 0:
return
# outliers check
median = points[int(l / 2)].z
#
ustd = np.std(np.array([p.z for p in points]))
#
if l < 10:
if l < 4:
# at least we need 4 points
return
left_idx = 0
while left_idx < l:
p = points[left_idx]
if np.abs(p.z - median) > 1.2 * ustd:
left_idx += 1
else:
break
right_idx = l - 1
while right_idx > left_idx:
p = points[right_idx]
if np.abs(p.z - median) > 1.2 * ustd:
right_idx -= 1
else:
break
# not modified
if left_idx == 0 and right_idx == l - 1:
return
else:
left_idx = int(l * 0.05)
right_idx = int(l * 0.95)
lower_bound = left_idx
left_idx = 0
while left_idx < lower_bound:
p = points[left_idx]
if np.abs(p.z - median) > 1.5 * ustd:
left_idx += 1
else:
break
upper_bound = right_idx
right_idx = l - 1
while right_idx > upper_bound:
p = points[right_idx]
if np.abs(p.z - median) > 1.5 * ustd:
right_idx -= 1
else:
break
pass
print("[%s] culling points [0,%d], [%d,%d)" %
(self, left_idx, right_idx, l))
#
for i in range(left_idx + 1):
p = points[i]
self.remove(p)
for j in range(right_idx, l):
p = points[j]
self.remove(p)
pass
def isIn(self, projection):
y1, x1, y2, x2 = self.bbox
x, y = projection.data
if x >= x1 and x <= x2 and \
y >= y1 and y <= y2:
return True
return False
pass
def merge(self, other):
assert self.label == other.label
for k, p in other.points.items():
self.points[k] = p
return self
def findRoI(self, frame_key):
for record in self.records:
if record[1] == frame_key:
if record[0] is None:
return self
return record[0]
raise Exception("Not Found!")
pass
def check_box_sim(self, roi):
y1, x1, y2, x2 = self.bbox
h1 = y2 - y1
w1 = x2 - x1
alpha1 = h1 / float(w1)
y1, x1, y2, x2 = roi.bbox
h2 = y2 - y1
w2 = x2 - x1
alpha2 = h2 / float(w2)
# print("%s |alpha1 - alpha2|: %f" % (self.label, np.abs(alpha1 - alpha2)))
if np.abs(alpha1 - alpha2) > 0.08:
return False
else:
return True
def map_keypoint_to_box(self, keypoint):
y1, x1, y2, x2 = self.bbox
shp = self.frame.img_grey().shape
h = y2 - y1
w = x2 - x1
x = (keypoint[0] - x1) * shp[1] / w
y = (keypoint[1] - y1) * shp[0] / h
return (x, y)
def map_keypoint_from_box(self, keypoint):
y1, x1, y2, x2 = self.bbox
shp = self.frame.img_grey().shape
h = y2 - y1
w = x2 - x1
x = keypoint[0] * w / shp[1] + x1
y = keypoint[1] * h / shp[0] + y1
return (x, y)
# associate with pixels
def associate_with(self, mappoint, frame, pos):
key = (mappoint.key, frame.seq)
if self.observations.get(key, None) is None:
self.observations[key] = pos
else:
# @todo : TODO check
last_pos = self.observations[key]
if last_pos != pos:
raise Exception(
"The mappoint %s projectd position %d has bee associated with frame %s in a different position: %d"
% (mappoint, pos, frame, last_pos))
return self
def set_FromFrame(self, frame):
self.frame = frame
return self
# @todo : TODO already matched
def is_active(self):
return True
# @todo : TODO observed by camera, i.e, camera is able to capure 3d points of the landmark
# we use projection test upon estimated 3d structures to see whether this is viewable by
# the camera
def viewable(self):
return True
def last_frame(self):
last_observation = self.records[-1][0]
if last_observation != None:
return last_observation.frame
else:
return self.frame
def predict(self, cur_frame=None):
y1, x1, y2, x2 = self.projected_pos
w1, h1 = x2 - x1, y2 - y1
box = np.array([x1, y1, w1, h1])
box0 = self.kf.predict(box)
assert (len(box0.shape) == 1)
self.predicted_states = np.array(
[box0[0], box0[1], box0[0] + box0[2], box0[1] + box0[3]])
if cur_frame is not None:
# use the velocity estimated for the last frame (containing the object)
last_frame = self.last_frame()
box1 = last_frame.predictors['OpticalFlow'].predict(
box, cur_frame.img_grey())
self.predicted_states = np.array(
[box1[0], box1[1], box1[0] + box1[2], box1[1] + box1[3]])
if DEBUG:
logging.info(
"<Landmark %d> , bbox <%d, %d, %d, %d>; predicted bbox by KalmanFilter: <%d, %d, %d, %d>"
% (self.seq, x1, y1, x2, y2, self.predicted_states[0],
self.predicted_states[1], self.predicted_states[2],
self.predicted_states[3]))
logging.info(
"<Landmark %d> , bbox velocity (delta/frame) <%d, %d, %d, %d>, predicted by KalmanFilter"
% (self.seq, self.predicted_states[0] - x1,
self.predicted_states[1] - y1, box0[2] - w1, box0[3] - h1))
if cur_frame is not None:
logging.info(
"<landmark %d> , bbox velocity (delta/frame) <%d, %d, %d, %d>, predicted by OpticalFlowBBoxPredictor"
% (self.seq, box1[0] - x1, box1[1] - y1, box1[2] - w1,
box1[3] - h1))
return self.predicted_states
def update(self, detection):
logging.info(
"<Landmark %d> update projected pose from %s => %s" %
(self.seq, str(self.projected_pos), str(detection.projected_pos)))
self.projected_pos = detection.projected_pos
self.projected_mask = detection.projected_mask
#
y1, x1, y2, x2 = self.projected_pos
box = np.array([x1, y1, x2 - x1, y2 - y1])
self.kf.update(box)
## Compute Cuboids
def computeAABB(self):
points = []
for _, point in self.points.items():
points.append(point.data)
l = len(points)
if l < 10:
raise Exception("Not Enough Points (%d) for this landmark %s!" %
(l, self))
points = np.array(points)
bottomLeftVec = points.min(axis=0)
topRightVec = points.max(axis=0)
bottomLeft = Point3D(bottomLeftVec[0], bottomLeftVec[1],
bottomLeftVec[2])
topRight = Point3D(topRightVec[0], topRightVec[1], topRightVec[2])
# update AABB
self._aabb = AABB(bottomLeft, topRight)
return self._aabb
# compute major direction and minor direction
def computeOBB(self):
if self._aabb is None:
self.computeAABB()
if self._obb is None:
# estimate using PCA algortithm
# @todo : TODO
pass
else:
# do refinement
#
return self._obb
def Centroid(self):
v = np.zeros(3)
l = len(self.points)
for point_key, point in self.points.items():
v += point.data
v /= float(l)
if self._centroid is None:
self._centroid = Point3D(v[0], v[1], v[2])
else:
self._centroid.update(v[0], v[1], v[2])
return self._centroid
## Spanning Tree
def findParent(self):
if self.parent is not None:
parent = self.parent.findParent()
self.parent = parent
return parent
return self
def add(self, mappoint):
if self.points.get(mappoint.key, None) is None:
self.points[mappoint.key] = mappoint
else:
raise Exception(
"The point has already been registered into the RoI")
def remove(self, mappoint):
if self.points.get(mappoint.key, None) is not None:
return self.points.pop(mappoint.key)
else:
logging.warning(
"The mappoint %s with key %s is not registered with the landmark!"
% (mappoint, mappoint.key))
## Just for logging
def __str__(self):
return "<F#%d.Ob#%d(%s:%.2f)>" % (self.frame.seq, self.seq, self.label,
self.score)
def __repr__(self):
return "<F#%d.Ob#%d(%s)>" % (self.frame.seq, self.seq, self.label)
class Pixel2D:
Seq = AtomicCounter()
# implements STL iterators
class VectorIterator(object):
def __init__(self, vec):
self._vec = vec
self.counter = self.__counter__()
def __iter__(self):
return self
def __counter__(self):
l = len(self._vec)
# one dimension index
ind = 0
while True:
yield ind
ind += 1
if ind >= l:
break
def __next__(self):
try:
ind = next(self.counter)
return self._vec[ind]
except StopIteration:
raise StopIteration()
def __str__(self):
return "Vector iterator"
def __init__(self, r, c, val=0):
self.identity = Identity(Pixel2D.Seq())
# y
self.r = r
# x
self.c = c
self.val = val
# cv keypoint reference
self.kp = None
# key point extracted feature
self.feature = None
## Covisibility Graph Topology
# reproj 3d points in camera space
#
self.parent = None
self.distance = None
self.cam_pt_array = []
# a weak frame reference
self.frame = None
# a weak roi reference
self.roi = None
@property
def x(self):
return self.c
@property
def y(self):
return self.r
@property
def sources(self):
return self.cam_pt_array
def __getitem__(self, i):
data = np.array([self.c, self.r])
return data[i]
def __setitem__(self, k, v):
data = np.array([self.c, self.r])
data[k] = v
self.r = data[1]
self.c = data[0]
return self
def set_frame(self, frame):
self.frame = frame
idx = self.pixel_pos
# add strong connection
frame.pixels[idx] = self
return self
@property
def seq(self):
return self.identity.seq
@property
def pixel_pos(self):
frame = self.frame
H, W = frame.img.shape[:2]
idx = int(self.r * W + self.c)
self.identity.key = idx
return idx
def set_roi(self, landmark):
self.roi = landmark
return self
def set_feature(self, feature):
self.feature = feature
return self
def set_kp(self, kp):
self.kp = kp
return self
def add(self, cam_pt3):
# if not present
self.cam_pt_array.append(cam_pt3)
return self
def __iter__(self):
data = np.array([self.c, self.r])
return self.VectorIterator(data)
@property
def data(self):
ret_data = np.array([int(self.c), int(self.r)])
return ret_data
def findObservationOf(self, world_pt3):
if len(self.sources) == 0:
return None
if world_pt3 is None:
return None
best = None
def check_eq(retrived, new):
left = retrived.data
right = new.data
dist = np.linalg.norm(left - right)
if dist < R_PRECISION:
return True
else:
return False
for p in self.cam_pt_array:
# comparing world point location
w = p.world
if w is None:
continue
if check_eq(w, world_pt3):
if best is None:
best = w
else:
dist1 = np.linalg.norm(w - world_pt3)
dist2 = np.linalg.norm(best - world_pt3)
if dist1 < dist2:
best = w
return best
def isObservationOf(self, mappoint):
if mappoint is None:
return False
frame = self.frame
px_pos = mappoint.frames.get(frame.seq, None)
if px_pos is None:
return False
else:
idx = self.pixel_pos
if idx == px_pos:
return True
else:
return False
# Union find with compression
def findParent(self, cmpr=False):
if self.parent is not None:
parent = self.parent.findParent()
if cmpr:
self.parent = parent
return parent
return self
def __repr__(self):
return "<Pixel2D %d: r=%.3f, c=%.3f>" % (self.identity.seq, self.r,
self.c)
def __str__(self):
return "<Pixel2D %d: r=%.3f, c=%.3f>" % (self.identity.seq, self.r,
self.c)
class Frame:
Seq = AtomicCounter()
logger = LoggerAdaptor("Frame", _logger)
def __init__(self):
self.identity = Identity(Frame.Seq())
self.isKeyFrame = False
## Content
# used to align with ground truth
self.timestamp = None
# might be a image path or url read by a asynchronous reader
self._img_src = None
# color constructed from img: cv::Mat or std::vector<byte>
self.img = None
# computed grey img or collected grey img directly from a camera
self._img_grey = None
# a camera instance to performance MVP or other image related computation
self.camera = None
# group of map tiles, where we storage 3d points, measurements and source images
# Note: this should be a weak reference to the original data representation
self.runtimeBlock = None
## Rigid object movements
# later we will move these attributes to Object3D as common practice
# in game development area, i.e, class Frame -> class Frame: public Object3D
# rotation and translation relative to origins
# Rwc. Note the author of ORB_SLAM2 uses the opporsite notation (Rcw) to reprented camera
# camera pose, see discuss #226
self.R0 = np.eye(3)
# twc
self.t0 = np.zeros((3, 1))
# rotation and translation relative to the last frame, updated in each frame
self.R1 = np.eye(3)
self.t1 = np.zeros((3, 1))
# used to very reprojection error in initialization process
self.Rcw = np.eye(3) # inv(R0)
self.tcw = np.zeros((3, 1)) # -inv(R0).dot(t0)
# GT set by tracker when timestamp and ground truth are all available
self.assoc_timestamp = None
self.R_gt = None
self.t_gt = None
## Covisibility Graph Topology
# previous frame
self.pre = None
#
self.pixels = {}
## Features Expression Layer
# extracted features
# opencv key points
self.kps = None
# opencv key point features
self.kps_feats = None
# extracted roi keypoints (defaults to ORB keypoints)
self.roi_kps = None
# extracted roi features
self.roi_feats = None
# meta data
self._detections = {}
# media scene depth
self.medianSceneDepth = -1.
self.extractors = {}
self.is_First = False
### Main Logics Executor ###
#
self.predictors = {
'OpticalFlow': OpticalFlowBBoxPredictor(),
'OpticalFlowKPnt': OpticalFlowKPntPredictor()
}
#
self.matchers = {}
#
self.USE_IMAGE_LEVEL_ORB = False # =>
#
self.SHOW_ROI = False
#
self.sample_size = -1
@property
def seq(self):
return self.identity.seq
@seq.setter
def seq(self, val):
self.identity.seq = val
return self
def set_camera(self, camera):
self.camera = camera
return self
def set_timestamp(self, timestamp):
self.timestamp = timestamp
return self
# @todo : TODO
def set_FromImg(self, img):
self.img = img
self.predictors['OpticalFlow'].set_FromImg(self.img_grey()).Init()
self.predictors['OpticalFlowKPnt'].set_FromImg(self.img_grey()).Init()
return self
# getter of self._grey_img
def img_grey(self):
img_grey = self._img_grey
if img_grey is None:
img_grey = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY)
# suppose the image is undistorted
self._img_grey = img_grey
return img_grey
# @todo : TODO
def extract(self):
orb_kp, orb_desc = (None, None)
if self.USE_IMAGE_LEVEL_ORB:
# @todo : REFACTOR the tasks should be running in pararllel
# self.logger.info("%s, Extracting Image ORB features ..." % self)
print("%s, Extracting Image ORB features ..." % self)
orb_kp, orb_desc = self.ExtractORB()
# @todo : TODO
# add to frame
# self.logger.info("Type of image orb key points : %s, size %d" % (type(orb_kp), len(orb_kp)))
print("Type of image orb key points : %s, size %d" %
(type(orb_kp), len(orb_kp)))
# self.logger.info("Type of image orb descriptors : %s, shape %s" % (type(orb_desc), orb_desc.shape))
print("Type of image orb descriptors : %s, shape %s" %
(type(orb_desc), orb_desc.shape))
# extract deep features or ROI
# self.logger.info("%s, Extracting ROI features ..." % self)
print("%s, Extracting ROI features ..." % self)
roi_kp, roi_features = self.ExtractROI()
kps = []
kps_feats = []
if self.USE_IMAGE_LEVEL_ORB:
kps.extend(orb_kp)
kps_feats.extend(orb_desc)
# catenate orb keypoints and features, see opencv docs for definition
# of returned key points and descriptors
if self.extractors['sfe'].USE_ROI_LEVEL_ORB:
for i, roi_feat_per_box in enumerate(roi_features):
desc_per_box, kps_per_box = roi_feat_per_box['roi_orb']
if len(kps_per_box) is 0:
label = roi_feat_per_box['label']
print("extract 0 points for detection#%d(%s)." %
(i, label))
# raise ValueError("'kps_per_box' should not be an empty list!")
kps.extend(kps_per_box)
kps_feats.extend(desc_per_box)
self.kps = kps
self.kps_feats = kps_feats
self.roi_kp = roi_kp
self.roi_features = roi_features
return (kps, kps_feats, roi_kp, roi_features)
def ExtractORB(self, bbox=None, mask=None, label=None):
# using opencv ORB extractor
orb = self.extractors.get('orb', None)
if orb is None:
orb = cv2.ORB_create(edgeThreshold=5,
patchSize=31,
nlevels=8,
fastThreshold=20,
scaleFactor=1.2,
WTA_K=4,
scoreType=cv2.ORB_HARRIS_SCORE,
firstLevel=0,
nfeatures=1000)
self.extractors['orb'] = orb
img_grey = self.img_grey()
shp = img_grey.shape
if bbox is not None:
y1, x1, y2, x2 = bbox
## adjust parameters
h = y2 - y1
w = x2 - x1
# print("label %s, h:%d, w:%d" % (label, h, w))
if np.min([w, h]) < 50:
orb.setEdgeThreshold(0)
orb.setPatchSize(15)
orb.setScaleFactor(1.4) # 1.4**8 ~ 8
orb.setWTA_K(2)
else:
# restore
orb.setEdgeThreshold(5)
orb.setPatchSize(31)
orb.setScaleFactor(1.2)
orb.setWTA_K(4)
##
# crop image
new_img_grey = np.zeros(shp)
new_img_grey[y1:y2, x1:x2] = img_grey[y1:y2, x1:x2]
if self.SHOW_ROI:
display(new_img_grey)
img_grey = img_grey[y1:y2, x1:x2]
img_grey = cv2.resize(img_grey, (shp[0], shp[1]),
interpolation=cv2.INTER_CUBIC)
# img_grey = cv2.cvtColor(new_img_grey.astype('uint8'), cv2.COLOR_GRAY2BGR)
# compute key points vector
kp = orb.detect(img_grey, None)
# compute the descriptors with ORB
kp, des = orb.compute(img_grey, kp)
if bbox is not None:
y1, x1, y2, x2 = bbox
h = y2 - y1
w = x2 - x1
shp0 = img_grey.shape
def _mapping(keypoint):
x = keypoint.pt[0] * w / shp0[1] + x1
y = keypoint.pt[1] * h / shp0[0] + y1
keypoint.pt = (x, y)
return keypoint
kp = list(map(lambda p: _mapping(p), kp))
# kp = list(map(lambda idx: cv2.KeyPoint(kp[idx].x + x1, kp[idx].y + y1), indice))
if bbox is not None and len(
kp) > self.sample_size and self.sample_size is not -1:
indice = np.random.choice(len(kp), self.sample_size)
kp = list(map(lambda idx: kp[idx], indice))
des = list(map(lambda idx: des[idx], indice))
# filter out kp, des with mask
# @todo : TODO
# assert(len(kp) > 0)
if len(kp) == 0:
return [], []
return kp, des
def ExtractROI(self):
# using our semantic features extractor
sfe = self.extractors.get('sfe', None)
if sfe is None:
sfe = SemanticFeatureExtractor()
self.extractors['sfe'] = sfe
sfe.attach_to(self)
# defaults to opencv channel last format
img = self.img
detections = sfe.detect(img)
self._detections = detections
# compute the descriptors with our SemanticFeaturesExtractor.encodeDeepFeatures
kp, des = sfe.compute(img, detections)
return kp, des
def mark_as_first(self):
self.is_First = True
return self
def find_px(self, x, y):
H, W = self.img.shape[0:2]
idx = int(y * W + x)
return self.pixels.get(idx, None)
def copy_pose_from(self, other_frame):
self.R0 = other_frame.R0
self.t0 = other_frame.t0
camera = self.camera
if camera is None:
camera = Camera.clone(other_frame.camera)
self.camera = camera
return self
def update_pose(self, pose):
# see g2opy/python/types/slam3d/se3quat.h for details for the interface
# print("R before update shape: %s, data: \n%s\n" % (self.R0.shape, self.R0))
self.R0 = pose.orientation().matrix()
# print("R after update shape: %s, data: \n%s\n" % (self.R0.shape, self.R0))
# print("t before update shape: %s, data: \n%s\n" % (self.t0.shape, self.t0))
self.t0 = pose.position().reshape((3, 1))
# print("t after update shape: %s, data: \n%s\n" % (self.t0.shape, self.t0))
# update R1, t1
if self.pre is not None:
self.R1 = self.R0.dot(np.linalg.inv(self.pre.R0))
self.t1 = self.t0 - self.R1.dot(self.pre.t0)
# update camera
self.camera.R0 = self.R0
self.camera.t0 = self.t0
self.camera.R1 = self.R1
self.camera.t1 = self.t1
else:
pass
return self
def get_pose_mat(self):
if False: # self.camera is not None:
# print("camera.t0 shape: %s, data: \n%s\n" % (self.camera.t0.shape, self.camera.t0))
# print("frame.t0 shape: %s, data: \n%s\n" % (self.t0.shape, self.t0))
pose = g2o.SE3Quat(self.camera.R0, self.camera.t0.reshape(3, ))
else:
pose = g2o.SE3Quat(self.R0, self.t0.reshape(3, ))
return pose.matrix()
# retrieved current filled structure
def get_points(self):
pixels, cam_pts, points = [], [], []
# @todo : TODO return current frame 3d structure
for _, pixel in self.pixels.items():
for cam_pt in pixel.sources:
world = cam_pt.world
if world is None:
continue
points.append(world)
return points
def get_measurements(self):
pixels, cam_pts, points = [], [], []
# @todo : TODO return current frame 3d structure
for _, pixel in self.pixels.items():
for cam_pt in pixel.sources:
world = cam_pt.world
if world is None:
continue
points.append(world)
pixels.append(pixel)
return points, pixels
def get_landmarks(self):
landmarks = set()
for _, pixel in self.pixels.items():
if pixel.roi is not None:
landmarks.add(pixel.roi.findParent())
return list(landmarks)
# def match(self, reference_frame, kps, kps_feat):
# frame_key = reference_frame.seq
# img_shp = self.img.shape[0:2]
#
# H, W = self.img.shape[0:2]
# matches = []
# unmatched_detections = []
# THR = 0.7 # 0.95 #0.8 #0.7
#
# def _hamming_distance(x, y):
# from scipy.spatial import distance
# return distance.hamming(x, y)
#
# def _get_neighbors(R, row, col, feat_map, img_shp):
# H, W = img_shp[0:2]
# x1, y1 = (col - R, row - R)
# x2, y2 = (col + R, row + R)
#
# if x1 < 0:
# x1 = 0
# if y1 < 0:
# y1 = 0
#
# if x2 >= W:
# x2 = W - 1
# if y2 >= H:
# y2 = H - 1
#
# indice = feat_map[y1:y2, x1:x2] != -1
# return feat_map[y1:y2, x1:x2][indice]
#
# #
# inv_kp = {}
# for i, kp in enumerate(kps):
# x, y = kp.pt
# idx = int(y * W + x)
# inv_kp[idx] = i
#
# # init feat_map
# feat_map = np.full(img_shp, -1)
# for i, kp in enumerate(kps):
# x, y = kp.pt
# if int(y) >= img_shp[0] or int(y) < 0 or \
# int(x) >= img_shp[1] or int(x) < 0:
# continue
# feat_map[int(y), int(x)] = i
#
# cur_kp, cur_kp_features = self.kps, self.kps_feats
#
# lost_pixels = 0
# skipped_in_parent_searching = 0
# skipped_in_neighbors_searching = 0
# dist_larger_than_thr = 0
#
# print("reference frame #%d key points: %d" % (reference_frame.seq, len(kps)))
# print("cur frame #%d key points: %d" % (self.seq, len(cur_kp)))
# if frame_key is not self.seq:
# for i, kp in enumerate(cur_kp):
# x, y = kp.pt
# idx = int(y * W + x)
# px = self.pixels.get(idx, None)
# if px is None:
# # print("The pixel (%f,%f) is none!" % (x, y))
# lost_pixels += 1
# continue
#
# parent = px.parent
#
# if parent is None:
# # print("The parent of the pixel is none!")
# unmatched_detections.append(px)
# continue
#
# feat_l = cur_kp_features[i]
#
# # print("=====================")
# # print("searching starts from %s(Frame#%d) 's parent %s(Frame#%d)" % (px, px.frame.seq, parent, parent.frame.seq))
# flag = True
# while parent is not None and flag:
# if parent.frame.seq == frame_key:
# # print("find a matched pixel %s(Frame#%d) for Frame#%d" % (parent, parent.frame.seq, frame_key))
# flag = False
# else:
# parent = parent.parent
# if parent is not None:
# # print("trace back to pixel %s(Frame#%d)" % (parent, parent.frame.seq))
# pass
#
# if flag:
# skipped_in_parent_searching += 1
# continue
#
# # print("\n")
# # print("find a match for pixel %s" % px)
# # find a match
# # jdx = int(parent.r * W + parent.c)
# # j = inv_kp[jdx]
#
# # feat_r = kps_feat[j]
# # feat_r = parent.feature
# # dist = _hamming_distance(feat_l, feat_r)
#
# # perform local search
# indice = _get_neighbors(8, int(parent.r), int(parent.c), feat_map, img_shp)
# if len(indice) == 0:
# skipped_in_neighbors_searching += 1
# continue
#
# # KNN search
# feat_l = cur_kp_features[i]
#
# dist = None
# min_dist, min_ind = np.inf, None
#
# for ind in indice:
# feat_r = kps_feat[ind]
# dist = _hamming_distance(feat_l, feat_r)
# if min_dist > dist:
# min_dist = dist
# min_ind = ind
#
# if min_dist >= THR:
# dist_larger_than_thr += 1
# continue
#
# # matches.append(cv2.DMatch(i, j, dist))
# matches.append(cv2.DMatch(i, min_ind, min_dist))
#
# else:
# # see Tracker._match
# raise Exception("Not Implemented Yet")
#
# matches = sorted(matches, key=lambda mtch: mtch.distance)
# import pandas as pd
#
# print("lost pixels : %d" % lost_pixels)
# print("unmatched detections : %d" % len(unmatched_detections))
# print("skipped when searching parent : %d" % skipped_in_parent_searching)
# print("skipped when searching neighbors of parents (8PX neighbors) : %d" % skipped_in_neighbors_searching)
# print("skipped when min dist is larger than %f : %d" % (THR, dist_larger_than_thr))
# if len(matches) < 10:
# raise Exception("Unwanted matching results!")
#
# distances = [mtch.distance for mtch in matches]
#
# df = pd.DataFrame({
# "Dist": distances
# })
#
# print(df)
#
# l = len(self.kps)
# mask = np.ones((l, 1))
#
# print("Found %d matches using Union Find algorithm" % len(matches))
#
# return matches, mask.tolist()
def match(self, reference_frame, kps, kps_feat):
frame_key = reference_frame.seq
img_shp = self.img.shape[0:2]
H, W = self.img.shape[0:2]
matches = []
unmatched_detections = []
THR = 0.75
THR2 = 0.75
def _hamming_distance(x, y):
from scipy.spatial import distance
return distance.hamming(x, y)
def _get_neighbors(R, row, col, feat_map, img_shp):
H, W = img_shp[0:2]
x1, y1 = (col - R, row - R)
x2, y2 = (col + R, row + R)
if x1 < 0:
x1 = 0
if y1 < 0:
y1 = 0
# freshly added (!important)
if x2 < 0:
x2 = 0
if y2 < 0:
x2 = 0
if x2 >= W:
x2 = W - 1
if y2 >= H:
y2 = H - 1
indice = feat_map[y1:y2, x1:x2] != -1
return feat_map[y1:y2, x1:x2][indice]
#
inv_kp = {}
for i, kp in enumerate(kps):
x, y = kp.pt
idx = int(y * W + x)
inv_kp[idx] = i
# init feat_map
feat_map = np.full(img_shp, -1)
for i, kp in enumerate(kps):
x, y = kp.pt
if int(y) >= img_shp[0] or int(y) < 0 or \
int(x) >= img_shp[1] or int(x) < 0:
continue
feat_map[int(y), int(x)] = i
cur_kp, cur_kp_features = self.kps, self.kps_feats
lost_pixels = 0
skipped_in_parent_searching = 0
skipped_in_neighbors_searching = 0
dist_larger_than_thr = 0
print("reference frame #%d key points: %d" %
(reference_frame.seq, len(kps)))
print("cur frame #%d key points: %d" % (self.seq, len(cur_kp)))
dx, dy = 0, 0
cnt = 0
if frame_key is not self.seq:
for i, kp in enumerate(cur_kp):
x, y = kp.pt
idx = int(y * W + x)
px = self.pixels.get(idx, None)
if px is None:
# print("The pixel (%f,%f) is none!" % (x, y))
lost_pixels += 1
continue
parent = px.parent
if parent is None: # deactivate the following searching method
# print("The parent of the pixel is none!")
unmatched_detections.append((i, kp, px))
continue
feat_l = cur_kp_features[i]
# print("=====================")
# print("searching starts from %s(Frame#%d) 's parent %s(Frame#%d)" % (px, px.frame.seq, parent, parent.frame.seq))
flag = True
while parent is not None and flag:
if parent.frame.seq == frame_key:
# print("find a matched pixel %s(Frame#%d) for Frame#%d" % (parent, parent.frame.seq, frame_key))
flag = False
else:
parent = parent.parent
if parent is not None:
# print("trace back to pixel %s(Frame#%d)" % (parent, parent.frame.seq))
pass
if flag:
skipped_in_parent_searching += 1
unmatched_detections.append((i, kp, px))
continue
# print("\n")
# print("find a match for pixel %s" % px)
# find a match
# jdx = int(parent.r * W + parent.c)
# j = inv_kp[jdx]
# feat_r = kps_feat[j]
# feat_r = parent.feature
# dist = _hamming_distance(feat_l, feat_r)
dx += parent.c - px.c
dy += parent.r - px.r
cnt += 1
# perform local search
indice = _get_neighbors(10, int(parent.r), int(parent.c),
feat_map, img_shp)
if len(indice) == 0:
skipped_in_neighbors_searching += 1
continue
# KNN search
feat_l = cur_kp_features[i]
dist = None
min_dist, min_ind = np.inf, None
for ind in indice:
feat_r = kps_feat[ind]
dist = _hamming_distance(feat_l, feat_r)
if min_dist > dist:
min_dist = dist
min_ind = ind
if min_dist >= THR:
dist_larger_than_thr += 1
unmatched_detections.append((i, kp, px))
continue
# matches.append(cv2.DMatch(i, j, dist))
matches.append(cv2.DMatch(i, min_ind, min_dist))
else:
# see Tracker._match
raise Exception("Not Implemented Yet")
if cnt != 0:
dx /= cnt
dy /= cnt
print("Average dx:", dx)
print("Average dy:", dy)
# optical_flow = OpticalFlowKPntPredictor()
# optical_flow.Init()
# optical_flow.set_FromImg(self.img_grey())
# # compute accumulative flows
# pre = self.pre
# flow = pre.predictors['OpticalFlowKPnt'].get_flow()
# assert flow.shape[:2] == img_shp
# while pre.seq is not frame_key:
# pre = pre.pre
# flow1 = pre.predictors['OpticalFlowKPnt'].get_flow()
# assert flow.shape == flow1.shape
# flow += flow1
# # predict coord
# kps_coor_r = list(map(lambda kp: kp.pt, kps))
# # init feat_map
# feat_map = np.full(img_shp, -1)
# for i, kp in enumerate(kps_coor_r):
# x, y = kp
# shift_x = flow[int(y), int(x), 0]
# shift_y = flow[int(y), int(x), 1]
# x += shift_x
# y += shift_y
# if int(y) >= img_shp[0] or int(y) < 0 or \
# int(x) >= img_shp[1] or int(x) < 0:
# continue
# feat_map[int(y),int(x)] = i
for i, kp, px in unmatched_detections:
# if cnt > 10:
# target = [px.x+dx, px.y+dy] # slightly bad
# else:
# target = optical_flow.predict([px.data], reference_frame.img_grey())[0] # not stable
s = px.roi.map_keypoint_to_box(px.data)
try:
ref_roi = px.roi.findParent().findRoI(frame_key) # very good
except Exception as e:
# print(e)
continue
if ref_roi.check_box_sim(px.roi):
target = ref_roi.map_keypoint_from_box(s)
# update dx , dy
dx = (dx * cnt + target[0] - px.c) / (cnt + 1)
dy = (dy * cnt + target[1] - px.r) / (cnt + 1)
cnt += 1
else:
# target = [px.x+dx, px.y+dy]
continue
# target = [px.x, px.y] # very bad
indice = _get_neighbors(12, int(target[1]), int(target[0]),
feat_map, img_shp)
# indice = _get_neighbors(7, int(px.y), int(px.x), feat_map, img_shp) # bad and not stable
if len(indice) == 0:
continue
# KNN search
feat_l = cur_kp_features[i]
dist = None
min_dist, min_in = np.inf, None
for ind in indice:
feat_r = kps_feat[ind]
dist = _hamming_distance(feat_l, feat_r)
if min_dist > dist:
min_dist = dist
min_ind = ind
if cnt > 10:
if min_dist >= THR:
continue
else:
if min_dist >= THR2:
continue
kp_in_ref = kps[min_ind]
x, y = kp_in_ref.pt
jdx = int(y * W + x)
px_in_ref = reference_frame.pixels.get(jdx, None)
if px_in_ref is not None and px.parent is None:
px.parent = px_in_ref
matches.append(cv2.DMatch(i, min_ind, min_dist))
matches = sorted(matches, key=lambda mtch: mtch.distance)
import pandas as pd
print("lost pixels : %d" % lost_pixels)
print("unmatched detections : %d" % len(unmatched_detections))
print("skipped when searching parent : %d" %
skipped_in_parent_searching)
print(
"skipped when searching neighbors of parents (8PX neighbors) : %d"
% skipped_in_neighbors_searching)
print("skipped when min dist is larger than %f : %d" %
(THR, dist_larger_than_thr))
if len(matches) < 10:
raise Exception("Unwanted matching results!")
distances = [mtch.distance for mtch in matches]
df = pd.DataFrame({"Dist": distances})
print(df)
l = len(self.kps)
mask = np.ones((l, 1))
print("Found %d matches using Union Find algorithm" % len(matches))
return matches, mask.tolist()
def __repr__(self):
return "<Frame %d>" % self.identity.seq
def __str__(self):
return "<Frame %d>" % self.identity.seq