def fmin_calibration_func(self,displacements):
# initial Mhat / camera center guess:
Mhat, residuals = camera_math.getMhat(self.calibration_raw_6d)
displaced_calibration = copy.copy(self.calibration_raw_6d)
diff_Mhat = 100
while diff_Mhat > 0.0005:
camera_center = np.array(camera_math.center(Mhat).T[0])
for i in range(displaced_calibration.shape[0]):
# for distance correction term, the pan portion uses the planar radial distance, the tilt portion uses the full distance
dist = np.linalg.norm(displaced_calibration[i,3:6] - camera_center)
planar_dist = np.cos(self.calibration_raw_6d[i,1])*dist
dist_arr = [planar_dist, dist]
for j in range(2):
displaced_calibration[i,j] = displaced_calibration[i,j] + np.pi/2 - np.arccos(displacements[j]/dist_arr[j])
new_Mhat, new_residuals = camera_math.getMhat(displaced_calibration)
diff_Mhat = np.sum( np.abs(new_Mhat - Mhat) )
Mhat = new_Mhat
self.Mhat = Mhat
self.camera_center = camera_center
residuals = new_residuals
print residuals, self.camera_center
return residuals
def calibrate(self):
self.Mhat, residuals = camera_math.getMhat(self.data)
self.camera_center = np.array(camera_math.center(self.Mhat).T[0])
print '*'*80
print 'DLT camera center: ', self.camera_center
print 'Mhat: '
print self.Mhat
self.Mhatinv = np.linalg.pinv(self.Mhat)
#self.Mhat3x3inv = np.linalg.inv(self.Mhat[:,0:3]) # this is the left 3x3 section of Mhat, inverted
#self.Mhat3x1 = self.Mhat[:,3] # this is the rightmost vertical vector of Mhat
self.focus.calibrate(data=self.data, camera_center=self.camera_center)
return
