def getUnit(coord):
pmat = ref.pmat
mzaxes = np.array([pmat[2][0], pmat[2][1], pmat[2][2]])
mxaxes = np.array([pmat[0][0], pmat[0][1], pmat[0][2]])
myaxes = cross(mzaxes, mxaxes)
myaxes /= norm(myaxes)
mxaxes = cross(myaxes, mzaxes)
xaxe = np.array([mxaxes[0], mxaxes[1], mxaxes[2], 0])
yaxe = np.array([myaxes[0], myaxes[1], myaxes[2], 0])
fx = xaxe @ pmat[0]
fy = yaxe @ pmat[1]
ftmp = fx + fy
if ftmp == 0:
return 1
fz = norm(coord - ref.center)
return 2 * fz / ftmp
# coord : -0.0093028 0.0375282 -0.0426621 1
# mcenter : -0.0093028 0.0375282 -0.0426621 1 // optical center
# mrays : -0.400778 -0.210337 0.891703 0 // optical ray
# mdscales : 0.000583982
# image : 0
# mx : -0.143964 0.969653 0.197606
# my : -0.903665 -0.0474331 -0.425605
# mz : -0.403316 -0.239841 0.88307
# proj : 0.400778 0.210337 -0.891703
# fx : -0.0299498
# fy : -0.0299498
# fz : -0.0299498
# vec0 : 0
# vec1 : -0.457662
# vec2 : 0.112559
return DoF
def calibrateImages(images):
for image in images:
logging.info(f'IMAGE {image.id:02d}:Calibrating images...')
ins = image.ins
ex = image.ex
pmat = ins @ ex
R = np.array([[ex[0][0], ex[0][1], ex[0][2]],
[ex[1][0], ex[1][1], ex[1][2]],
[ex[2][0], ex[2][1], ex[2][2]]])
t = np.array([ex[0][3], ex[1][3], ex[2][3]])
center = -inv(R) @ t
center = np.array([center[0], center[1], center[2], 1])
zaxis = np.array(pmat[2])
zaxis[3] = 0
ftmp = norm(zaxis)
zaxis /= ftmp
zaxis = np.array([zaxis[0], zaxis[1], zaxis[2]])
xaxis = np.array([pmat[0][0], pmat[0][1], pmat[0][2]])
yaxis = cross(zaxis, xaxis)
yaxis /= norm(yaxis)
xaxis = cross(yaxis, zaxis)
image.pmat = pmat
image.center = center
image.xaxis = xaxis
image.yaxis = yaxis
image.zaxis = zaxis
def processInput(window):
global camPos, power, iterations, cameraSpeed
deltaTime = 0.0
lastFrame = 0.0
currentFrame = glfw.get_time()
deltaTime = currentFrame - lastFrame
lastFrame = currentFrame
if (glfw.get_key(window, glfw.KEY_W) == glfw.PRESS):
camPos[0] += cameraSpeed * camFront[0]
camPos[1] += cameraSpeed * camFront[1]
camPos[2] += cameraSpeed * camFront[2]
if (glfw.get_key(window, glfw.KEY_S) == glfw.PRESS):
camPos[0] -= cameraSpeed * camFront[0]
camPos[1] -= cameraSpeed * camFront[1]
camPos[2] -= cameraSpeed * camFront[2]
if (glfw.get_key(window, glfw.KEY_A) == glfw.PRESS):
camPos -= cross(camFront, camUp) * cameraSpeed
if (glfw.get_key(window, glfw.KEY_D) == glfw.PRESS):
camPos += cross(camFront, camUp) * cameraSpeed
if (glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS):
camPos[1] += cameraSpeed
if (glfw.get_key(window, glfw.KEY_LEFT_SHIFT) == glfw.PRESS):
camPos[1] -= cameraSpeed
if (glfw.get_key(window, glfw.KEY_R) == glfw.PRESS):
camPos = [0.0, 0.0, 2.0]
if (glfw.get_key(window, glfw.KEY_O) == glfw.PRESS):
cameraSpeed /= 2.0
if (glfw.get_key(window, glfw.KEY_I) == glfw.PRESS):
cameraSpeed *= 2.0
if (glfw.get_key(window, glfw.KEY_KP_ADD) == glfw.PRESS):
power += 0.1
print(power)
if (glfw.get_key(window, glfw.KEY_KP_SUBTRACT) == glfw.PRESS):
power -= 0.1
print(power)
if (glfw.get_key(window, glfw.KEY_UP) == glfw.PRESS):
iterations += 1
if (glfw.get_key(window, glfw.KEY_DOWN) == glfw.PRESS):
iterations -= 1
if (glfw.get_key(window, glfw.KEY_ESCAPE) == glfw.PRESS):
glfw.set_window_should_close(window, True)
def decode(vect):
center = m_center + m_dscales * vect[0] * m_ray
angle1 = vect[1] * m_ascales
angle2 = vect[2] * m_ascales
fx = sin(angle1) * cos(angle2)
fy = sin(angle2)
fz = -cos(angle1) * cos(angle2)
pmat = ref.pmat
mzaxes = np.array([pmat[2][0], pmat[2][1], pmat[2][2]])
mxaxes = np.array([pmat[0][0], pmat[0][1], pmat[0][2]])
myaxes = cross(mzaxes, mxaxes)
myaxes /= norm(myaxes)
mxaxes = cross(myaxes, mzaxes)
ftmp = mxaxes * fx + myaxes * fy + mzaxes * fz
normal = np.array([ftmp[0], ftmp[1], ftmp[2], 0])
return center, normal
def encode(patch):
vect = []
unit = getUnit(patch.center)
unit2 = 2 * unit
ray = patch.center - ref.center
ray /= norm(ray)
global m_dscales
for image in VpStar:
diff = image.pmat @ patch.center - image.pmat @ (patch.center -
ray * unit2)
m_dscales += norm(diff)
m_dscales /= len(VpStar) - 1
m_dscales = unit2 / m_dscales
global m_ascales
m_ascales = pi / 48
vect.append((patch.center - m_center) @ m_ray / m_dscales)
pmat = ref.pmat
mzaxes = np.array([pmat[2][0], pmat[2][1], pmat[2][2]])
mxaxes = np.array([pmat[0][0], pmat[0][1], pmat[0][2]])
myaxes = cross(mzaxes, mxaxes)
myaxes /= norm(myaxes)
mxaxes = cross(myaxes, mzaxes)
proj_normal = np.array([patch.normal[0], patch.normal[1], patch.normal[2]])
fx = mxaxes @ proj_normal
fy = myaxes @ proj_normal
fz = mzaxes @ proj_normal
temp2 = asin(max(-1, min(1, fy)))
cosb = cos(temp2)
if cosb == 0:
temp1 = 0
else:
sina = fx / cosb
cosa = -fz / cosb
temp1 = acos(max(-1, min(1, cosa)))
if sina < 0:
temp1 = -temp1
vect.append(temp1 / m_ascales)
vect.append(temp2 / m_ascales)
return vect
def getPatchAxes(self) :
pmat= self.ref.pmat
xaxis = self.ref.xaxis
yaxis = self.ref.yaxis
normal3 = np.array([self.normal[0], self.normal[1], self.normal[2]])
yaxis3 = cross(normal3, xaxis)
yaxis3 /= norm(yaxis3)
xaxis3 = cross(yaxis3, normal3)
xaxe = np.array([xaxis[0], xaxis[1], xaxis[2], 0])
yaxe = np.array([yaxis[0], yaxis[1], yaxis[2], 0])
fx = xaxe @ pmat[0]
fy = yaxe @ pmat[1]
fz = norm(self.center - self.ref.center)
ftmp = fx + fy
pscale = fz / ftmp
pscale = 2 * fz / ftmp
pxaxis = np.array([xaxis3[0], xaxis3[1], xaxis3[2], 0])
pyaxis = np.array([yaxis3[0], yaxis3[1], yaxis3[2], 0])
pxaxis *= pscale
pyaxis *= pscale
a = pmat @ (self.center + pxaxis)
b = pmat @ (self.center + pyaxis)
c = pmat @ (self.center)
a /= a[2]
b /= b[2]
c /= c[2]
xdis = norm(a - c)
ydis = norm(b - c)
if xdis != 0 :
pxaxis /= xdis
if ydis != 0 :
pyaxis /= ydis
return pxaxis, pyaxis
def _distancePointToLineSegment(self, a, b, p):
'''
Returns tuple of minimum distance to the directed line segment AB from p, and the distance along AB of the point of intersection
'''
ab_mag2 = dot((b-a),(b-a))
pa_mag2 = dot((a-p),(a-p))
pb_mag2 = dot((b-p),(b-p))
if pa_mag2 + ab_mag2 <= pb_mag2:
return (sqrt(pa_mag2),0)
elif pb_mag2 + ab_mag2 <= pa_mag2:
return (sqrt(pb_mag2), sqrt(ab_mag2))
else:
c = cross((b-a),(p-a))
if ab_mag2 == 0:
raise ValueError, 'Division by zero magnitude line segment AB'
dist_to_line2 = dot(c,c)/ab_mag2
dist_to_line = sqrt(dist_to_line2)
dist_along_segment = sqrt(pa_mag2 - dist_to_line2)
return (dist_to_line, dist_along_segment)