def fromTargetViewObservations(cls, cameras, target, baselines, T_tc_guess, rig_observations, useBlakeZissermanMest=True): rval = CalibrationTargetOptimizationProblem() #store the arguements in case we want to rebuild a modified problem rval.cameras = cameras rval.target = target rval.baselines = baselines rval.T_tc_guess = T_tc_guess rval.rig_observations = rig_observations # 1. Create a design variable for this pose T_target_camera = T_tc_guess rval.dv_T_target_camera = aopt.TransformationDv(T_target_camera) for i in range(0, rval.dv_T_target_camera.numDesignVariables()): rval.addDesignVariable( rval.dv_T_target_camera.getDesignVariable(i), TRANSFORMATION_GROUP_ID) #2. add all baselines DVs for baseline_dv in baselines: for i in range(0, baseline_dv.numDesignVariables()): rval.addDesignVariable(baseline_dv.getDesignVariable(i), CALIBRATION_GROUP_ID) #3. add landmark DVs for p in target.P_t_dv: rval.addDesignVariable(p, LANDMARK_GROUP_ID) #4. add camera DVs for camera in cameras: if not camera.isGeometryInitialized: raise RuntimeError( 'The camera geometry is not initialized. Please initialize with initGeometry() or initGeometryFromDataset()' ) camera.setDvActiveStatus(True, True, False) rval.addDesignVariable(camera.dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) rval.addDesignVariable(camera.dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) rval.addDesignVariable(camera.dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) #4.add all observations for this view cams_in_view = set() rval.rerrs = dict() rerr_cnt = 0 for cam_id, obs in rig_observations: camera = cameras[cam_id] cams_in_view.add(cam_id) #add reprojection errors #build baseline chain (target->cam0->baselines->camN) T_cam0_target = rval.dv_T_target_camera.expression.inverse() T_camN_calib = T_cam0_target for idx in range(0, cam_id): T_camN_calib = baselines[idx].toExpression() * T_camN_calib # \todo pass in the detector uncertainty somehow. cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) rval.rerrs[cam_id] = list() for i in range(0, len(target.P_t_ex)): p_target = target.P_t_ex[i] valid, y = obs.imagePoint(i) if valid: rerr_cnt += 1 # Create an error term. rerr = camera.model.reprojectionError( y, invR, T_camN_calib * p_target, camera.dv) rerr.idx = i #add blake-zisserman mest if useBlakeZissermanMest: mest = aopt.BlakeZissermanMEstimator(2.0) rerr.setMEstimatorPolicy(mest) rval.addErrorTerm(rerr) rval.rerrs[cam_id].append(rerr) else: rval.rerrs[cam_id].append(None) sm.logDebug( "Adding a view with {0} cameras and {1} error terms".format( len(cams_in_view), rerr_cnt)) return rval
def addCameraErrorTerms(self, problem, poseSplineDv, T_cN_b, blakeZissermanDf=0.0, timeOffsetPadding=0.0): print print "Adding camera error terms ({0})".format(self.dataset.topic) #progress bar iProgress = sm.Progress2(len(self.targetObservations)) iProgress.sample() allReprojectionErrors = list() error_t = self.camera.reprojectionErrorType for obs in self.targetObservations: # Build a transformation expression for the time. frameTime = self.cameraTimeToImuTimeDv.toExpression() + obs.time( ).toSec() + self.timeshiftCamToImuPrior frameTimeScalar = frameTime.toScalar() #as we are applying an initial time shift outside the optimization so #we need to make sure that we dont add data outside the spline definition if frameTimeScalar <= poseSplineDv.spline().t_min( ) or frameTimeScalar >= poseSplineDv.spline().t_max(): continue T_w_b = poseSplineDv.transformationAtTime(frameTime, timeOffsetPadding, timeOffsetPadding) T_b_w = T_w_b.inverse() #calibration target coords to camera N coords #T_b_w: from world to imu coords #T_cN_b: from imu to camera N coords T_c_w = T_cN_b * T_b_w #get the image and target points corresponding to the frame imageCornerPoints = np.array(obs.getCornersImageFrame()).T targetCornerPoints = np.array(obs.getCornersTargetFrame()).T #setup an aslam frame (handles the distortion) frame = self.camera.frameType() frame.setGeometry(self.camera.geometry) #corner uncertainty R = np.eye(2) * self.cornerUncertainty * self.cornerUncertainty invR = np.linalg.inv(R) for pidx in range(0, imageCornerPoints.shape[1]): #add all image points k = self.camera.keypointType() k.setMeasurement(imageCornerPoints[:, pidx]) k.setInverseMeasurementCovariance(invR) frame.addKeypoint(k) reprojectionErrors = list() for pidx in range(0, imageCornerPoints.shape[1]): #add all target points targetPoint = np.insert(targetCornerPoints.transpose()[pidx], 3, 1) p = T_c_w * aopt.HomogeneousExpression(targetPoint) #build and append the error term rerr = error_t(frame, pidx, p) #add blake-zisserman m-estimator if blakeZissermanDf > 0.0: mest = aopt.BlakeZissermanMEstimator(blakeZissermanDf) rerr.setMEstimatorPolicy(mest) problem.addErrorTerm(rerr) reprojectionErrors.append(rerr) allReprojectionErrors.append(reprojectionErrors) #update progress bar iProgress.sample() print "\r Added {0} camera error terms ".format( len(self.targetObservations)) self.allReprojectionErrors = allReprojectionErrors