def viewWorldPoint(self, point3d):
v = point3d.data.reshape(3, 1)
cam_pt = np.linalg.inv(self.R0).dot(v - self.t0)
ret = Point3D(cam_pt[0][0], cam_pt[1][0], cam_pt[2][0])
# used for debug
ret.seq = point3d.seq
return ret
def drawGrid(self, frame=None, gridNum=8, gridSize=0.1):
camera = frame.camera or self.camera
if camera is None:
raise Exception("Camera should not be None!")
H, W = frame.img.shape[:2]
halfGridNum = (gridNum - 1) / 2
grid = np.zeros((gridNum, gridNum, 2))
dv = np.array([0., 0., 0.])
for i in range(gridNum):
for j in range(gridNum):
x = (i - halfGridNum) * gridSize + dv[0]
y = (j - halfGridNum) * gridSize + dv[1]
z = 0. + dv[2]
v = np.array([x, y, z]).reshape(3, 1) - frame.t0
pt_cam = np.linalg.inv(frame.R0).dot(v)
pt_cam = pt_cam.reshape((3, ))
if np.abs(pt_cam[2]) < 0.001:
pt_cam[2] = 0.001
px = camera.view(Point3D(pt_cam[0], pt_cam[1], pt_cam[2]))
if px is None:
grid[i, j, 0] = -1
grid[i, j, 1] = -1
continue
grid[i, j, 0] = px.x
grid[i, j, 1] = px.y
# draw grid
mask = np.zeros_like(frame.img)
for i in range(gridNum):
for j in range(gridNum - 1):
if grid[i, j, 0] == -1 and grid[i, j, 1] == -1:
continue
if grid[i, j + 1, 0] == -1 and grid[i, j + 1, 1] == -1:
continue
mask = cv2.line(
mask, (int(grid[i, j, 0]), int(grid[i, j, 1])),
(int(grid[i, j + 1, 0]), int(grid[i, j + 1, 1])),
(255, 0, 255), 1)
for j in range(gridNum):
for i in range(gridNum - 1):
if grid[i, j, 0] == -1 and grid[i, j, 1] == -1:
continue
if grid[i + 1, j, 0] == -1 and grid[i + 1, j, 1] == -1:
continue
mask = cv2.line(
mask, (int(grid[i, j, 0]), int(grid[i, j, 1])),
(int(grid[i + 1, j, 0]), int(grid[i + 1, j, 1])),
(255, 0, 255), 1)
return mask
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
def reproj(self, pixel2d):
px = pixel2d
K = self.K
# compute normalized point in camera space
if isinstance(px, cv2.KeyPoint):
px = Pixel2D(px.pt[1], px.pt[0])
if isinstance(px, tuple):
px = Pixel2D(px[1], px[0])
return Point3D((px.x - K[0, 2]) / K[0, 0], (px.y - K[1, 2]) / K[1, 1],
1)
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
def drawAxis(self, frame=None):
# unit is mm
arrow_length = 0.3
dv = np.array([0., 0., 0.])
# compute the point to place an axis object, R, t are estiamted from ransac and posegraph optimization
H, W = frame.img.shape[:2]
rot_vec, _ = cv2.Rodrigues(frame.R0)
points = np.float32([[arrow_length, 0 + dv[1], 0 + dv[2]],
[0, arrow_length + dv[1], 0 + dv[2]],
[0, 0 + dv[1], arrow_length + dv[2]],
[0, 0 + dv[1], 0 + dv[2]]]).reshape(-1, 3)
# set distortion to zeros
axisPoints, _ = cv2.projectPoints(points, rot_vec, frame.t0,
frame.camera.K, (0, 0, 0, 0))
print("Axis in image pixels (OpenCV): \n%s" % axisPoints)
imagepoints = []
for p in points:
cam_pt = frame.camera.viewWorldPoint(Point3D(p[0], p[1], p[2]))
px = frame.camera.view(cam_pt)
imagepoints.append(px)
pass
imagepoints = np.array(imagepoints)
print("images points computed: \n%s" % imagepoints)
mask = np.zeros_like(frame.img)
# mask = cv2.line(mask, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (255,0,0), 5)
# mask = cv2.line(mask, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0,255,0), 5)
# mask = cv2.line(mask, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (0,0,255), 5)
mask = cv2.line(mask, tuple(imagepoints[3].data),
tuple(imagepoints[0].data), (255, 0, 0), 5)
mask = cv2.line(mask, tuple(imagepoints[3].data),
tuple(imagepoints[1].data), (0, 255, 0), 5)
mask = cv2.line(mask, tuple(imagepoints[3].data),
tuple(imagepoints[2].data), (0, 0, 255), 5)
return mask
def drawDepthImage(self, frame):
# dest = os.path.join(TUM_DATA_DIR, frame.depth_img)
img = frame.img
# depth_img = cv2.imread(dest, 0)
depth_img = frame.depth_img
H, W = img.shape[0:2]
SCALING_FACTOR = 5 # 5000.0
R_PRECISION = 1e-4
datum = Datum()
datum.attrs['points-redundant'] = []
datum.attrs['colors-redundant'] = []
K = frame.camera.K
def check_eq(retrived, right):
left = retrived.data
dist = np.linalg.norm(left - right)
if dist < R_PRECISION:
return True
else:
return False
for y in range(H):
for x in range(W):
# set RGB color for rendering
color = frame.img[int(y), int(x)]
Z = depth_img[int(y), int(x)]
Z /= SCALING_FACTOR
if Z == 0: continue
X = (x - K[0, 2]) * Z / K[0, 0]
Y = (y - K[1, 2]) * Z / K[1, 1]
# get pixel
idx = int(y * W + x)
px = frame.pixels.get(idx, None)
p = None
if True: # px is None:
p = Point3D(X, Y, Z)
else:
p_vec = np.array([X, Y, Z])
dist = None
best = None
for cam_pt in px.cam_pt_array:
w = cam_pt
if w is None:
continue
if best is None:
best = w
dist = np.linalg.norm(best.data - p_vec)
else:
dist1 = np.linalg.norm(w.data - p_vec)
if dist1 < dist:
best = w
dist = dist1
if best is None:
p = Point3D(X, Y, Z)
else:
p = best
print(
"[PCViewer.DrawDepthImage] Updating %s to (%f, %f, %f) "
% (p, X, Y, Z))
p.x = X
p.y = Y
p.z = Z
# p.set_color(color)
# camera to world
v = p.data.reshape((3, 1))
# Twc
v = frame.R0.dot(v) + frame.t0
v = v.reshape((3, ))
p.update(*v)
datum.attrs['points-redundant'].append(p)
datum.attrs['colors-redundant'].append(color)
datum.attrs['pose'] = self._tracker.cur.get_pose_mat()
self.logger.info("[Main Thread] put datum to queue")
self.attributes.put(datum)
pass
def topCentre(self):
x2, y2, z2 = self.topRight.data
x3, y3, z3 = self.centre.data
_topCentre = Point3D(x3, y3, z2)
return _topCentre
def Init(self):
x1, y1, z1 = self.bottomLeft.data
x2, y2, z2 = self.topRight.data
self.centre = Point3D(x1 + x2 / 2., y1 + y2 / 2., z1 + z2 / 2.)