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