def __init__(self, target, estimateLandmarks=False): self.target = target # Create design variables and expressions for all target points. P_t_dv = [] P_t_ex = [] for i in range(0,self.target.size()): p_t_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(self.target.point(i))); p_t_dv.setActive(estimateLandmarks) p_t_ex = p_t_dv.toExpression() P_t_dv.append(p_t_dv) P_t_ex.append(p_t_ex) self.P_t_dv = P_t_dv self.P_t_ex = P_t_ex
def __buildOptimizationProblem(self, W): """Build the optimisation problem""" problem = inc.CalibrationOptimizationProblem() # Initialize all design variables. self.__initPoseDesignVariables(problem) ##### ## build error terms and add to problem # store all frames self.__frames = [] self.__reprojection_errors = [] # This code assumes that the order of the landmarks in the observations # is invariant across all observations. At least for the chessboards it is true. ##### # add all the landmarks once landmarks = [] landmarks_expr = [] for landmark in self.__observations[0].getCornersTargetFrame(): # design variable for landmark landmark_w_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(landmark)) landmark_w_dv.setActive(self.__config.estimateParameters['landmarks']); landmarks.append(landmark_w_dv) landmarks_expr.append(landmark_w_dv.toExpression()) problem.addDesignVariable(landmark_w_dv, CALIBRATION_GROUP_ID) ##### # activate design variables self.__camera_dv.setActive( self.__config.estimateParameters['intrinsics'], self.__config.estimateParameters['distortion'], self.__config.estimateParameters['shutter'] ) ##### # Add design variables # add the camera design variables last for optimal sparsity patterns problem.addDesignVariable(self.__camera_dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) ##### # Regularization term / motion prior motionError = asp.BSplineMotionError(self.__poseSpline_dv, W) problem.addErrorTerm(motionError) ##### # add a reprojection error for every corner of each observation for observation in self.__observations: # only process successful observations of a pattern if (observation.hasSuccessfulObservation()): # add a frame frame = self.__cameraModelFactory.frameType() frame.setGeometry(self.__camera) frame.setTime(observation.time()) self.__frames.append(frame) ##### # add an error term for every observed corner for index, point in enumerate(observation.getCornersImageFrame()): # keypoint time offset by line delay as expression type keypoint_time = self.__camera_dv.keypointTime(frame.time(), point) # from target to world transformation. T_w_t = self.__poseSpline_dv.transformationAtTime( keypoint_time, self.__config.timeOffsetConstantSparsityPattern, self.__config.timeOffsetConstantSparsityPattern ) T_t_w = T_w_t.inverse() # transform target point to camera frame p_t = T_t_w * landmarks_expr[index] # create the keypoint keypoint = acv.Keypoint2() keypoint.setMeasurement(point) inverseFeatureCovariance = self.__config.inverseFeatureCovariance; keypoint.setInverseMeasurementCovariance(np.eye(len(point)) * inverseFeatureCovariance) keypoint.setLandmarkId(index) frame.addKeypoint(keypoint) keypoint_index = frame.numKeypoints() - 1 # create reprojection error reprojection_error = self.__buildErrorTerm( frame, keypoint_index, p_t, self.__camera_dv, self.__poseSpline_dv ) self.__reprojection_errors.append(reprojection_error) problem.addErrorTerm(reprojection_error) return problem
def __buildOptimizationProblem(self, W): """Build the optimisation problem""" problem = inc.CalibrationOptimizationProblem() # Initialize all design variables. self.__initPoseDesignVariables(problem) ##### ## build error terms and add to problem # store all frames self.__frames = [] self.__reprojection_errors = [] # This code assumes that the order of the landmarks in the observations # is invariant across all observations. At least for the chessboards it is true. ##### # add all the landmarks once landmarks = [] landmarks_expr = [] target = self.__cameraGeometry.ctarget.detector.target() for idx in range(0, target.size()): # design variable for landmark landmark_w_dv = aopt.HomogeneousPointDv( sm.toHomogeneous(target.point(idx))) landmark_w_dv.setActive( self.__config.estimateParameters['landmarks']) landmarks.append(landmark_w_dv) landmarks_expr.append(landmark_w_dv.toExpression()) problem.addDesignVariable(landmark_w_dv, LANDMARK_GROUP_ID) ##### # activate design variables self.__camera_dv.setActive( self.__config.estimateParameters['intrinsics'], self.__config.estimateParameters['distortion'], self.__config.estimateParameters['shutter']) ##### # Add design variables # add the camera design variables last for optimal sparsity patterns problem.addDesignVariable(self.__camera_dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) ##### # Regularization term / motion prior motionError = asp.BSplineMotionError(self.__poseSpline_dv, W) problem.addErrorTerm(motionError) ##### # add a reprojection error for every corner of each observation for observation in self.__observations: # only process successful observations of a pattern if observation.hasSuccessfulObservation(): # add a frame frame = self.__cameraModelFactory.frameType() frame.setGeometry(self.__camera) frame.setTime(observation.time()) self.__frames.append(frame) ##### # add an error term for every observed corner corner_ids = observation.getCornersIdx() for index, point in enumerate( observation.getCornersImageFrame()): # keypoint time offset by line delay as expression type keypoint_time = self.__camera_dv.keypointTime( frame.time(), point) # from target to world transformation. T_w_t = self.__poseSpline_dv.transformationAtTime( keypoint_time, self.__config.timeOffsetConstantSparsityPattern, self.__config.timeOffsetConstantSparsityPattern) T_t_w = T_w_t.inverse() # transform target point to camera frame p_t = T_t_w * landmarks_expr[corner_ids[index]] # create the keypoint keypoint_index = frame.numKeypoints() keypoint = acv.Keypoint2() keypoint.setMeasurement(point) inverseFeatureCovariance = self.__config.inverseFeatureCovariance keypoint.setInverseMeasurementCovariance( np.eye(len(point)) * inverseFeatureCovariance) frame.addKeypoint(keypoint) # create reprojection error reprojection_error = self.__buildErrorTerm( frame, keypoint_index, p_t, self.__camera_dv, self.__poseSpline_dv) self.__reprojection_errors.append(reprojection_error) problem.addErrorTerm(reprojection_error) return problem
def solveFullBatch(cameras, baseline_guesses, graph): ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #add camera dvs for cam in cameras: cam.setDvActiveStatus(True, True, False) problem.addDesignVariable(cam.dv.distortionDesignVariable()) problem.addDesignVariable(cam.dv.projectionDesignVariable()) problem.addDesignVariable(cam.dv.shutterDesignVariable()) baseline_dvs = list() for baseline_idx in range(0, len(cameras) - 1): baseline_dv = aopt.TransformationDv(baseline_guesses[baseline_idx]) for i in range(0, baseline_dv.numDesignVariables()): problem.addDesignVariable(baseline_dv.getDesignVariable(i)) baseline_dvs.append(baseline_dv) #corner uncertainty cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #get the target target = cameras[0].ctarget.detector.target() #Add calibration target reprojection error terms for all camera in chain target_pose_dvs = list() #shuffle the views reprojectionErrors = [] timestamps = graph.obs_db.getAllViewTimestamps() for view_id, timestamp in enumerate(timestamps): #get all observations for all cams at this time obs_tuple = graph.obs_db.getAllObsAtTimestamp(timestamp) #create a target pose dv for all target views (= T_cam0_w) T0 = graph.getTargetPoseGuess(timestamp, cameras, baseline_guesses) target_pose_dv = addPoseDesignVariable(problem, T0) target_pose_dvs.append(target_pose_dv) for cidx, obs in obs_tuple: cam = cameras[cidx] #calibration target coords to camera X coords T_cam0_calib = target_pose_dv.toExpression().inverse() #build pose chain (target->cam0->baselines->camN) T_camN_calib = T_cam0_calib for idx in range(0, cidx): T_camN_calib = baseline_dvs[idx].toExpression() * T_camN_calib ## add error terms for i in range(0, target.size()): p_target = aopt.HomogeneousExpression( sm.toHomogeneous(target.point(i))) valid, y = obs.imagePoint(i) if valid: rerr = cameras[cidx].model.reprojectionError( y, invR, T_camN_calib * p_target, cameras[cidx].dv) problem.addErrorTerm(rerr) reprojectionErrors.append(rerr) sm.logDebug("solveFullBatch: added {0} camera error terms".format( len(reprojectionErrors))) ############################################ ## solve ############################################ options = aopt.Optimizer2Options() options.verbose = True if sm.getLoggingLevel( ) == sm.LoggingLevel.Debug else False options.nThreads = 4 options.convergenceDeltaX = 1e-3 options.convergenceDeltaJ = 1 options.maxIterations = 250 options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10) optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: sm.logDebug("Before optimization:") e2 = np.array([e.evaluateError() for e in reprojectionErrors]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) #run intrinsic calibration try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("calibrateIntrinsics: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("calibrateIntrinsics: Optimization failed!") success = False baselines = list() for baseline_dv in baseline_dvs: baselines.append(sm.Transformation(baseline_dv.T())) return success, baselines
def calibrateIntrinsics(cam_geometry, obslist, distortionActive=True, intrinsicsActive=True): #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: d = cam_geometry.geometry.projection().distortion().getParameters( ).flatten() p = cam_geometry.geometry.projection().getParameters().flatten() sm.logDebug("calibrateIntrinsics: intrinsics guess: {0}".format(p)) sm.logDebug("calibrateIntrinsics: distortion guess: {0}".format(d)) ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #add camera dvs cam_geometry.setDvActiveStatus(intrinsicsActive, distortionActive, False) problem.addDesignVariable(cam_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(cam_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(cam_geometry.dv.shutterDesignVariable()) #corner uncertainty cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #get the image and target points corresponding to the frame target = cam_geometry.ctarget.detector.target() #target pose dv for all target views (=T_camL_w) reprojectionErrors = [] sm.logDebug( "calibrateIntrinsics: adding camera error terms for {0} calibration targets" .format(len(obslist))) target_pose_dvs = list() for obs in obslist: success, T_t_c = cam_geometry.geometry.estimateTransformation(obs) target_pose_dv = addPoseDesignVariable(problem, T_t_c) target_pose_dvs.append(target_pose_dv) T_cam_w = target_pose_dv.toExpression().inverse() ## add error terms for i in range(0, target.size()): p_target = aopt.HomogeneousExpression( sm.toHomogeneous(target.point(i))) valid, y = obs.imagePoint(i) if valid: rerr = cam_geometry.model.reprojectionError( y, invR, T_cam_w * p_target, cam_geometry.dv) problem.addErrorTerm(rerr) reprojectionErrors.append(rerr) sm.logDebug("calibrateIntrinsics: added {0} camera error terms".format( len(reprojectionErrors))) ############################################ ## solve ############################################ options = aopt.Optimizer2Options() options.verbose = True if sm.getLoggingLevel( ) == sm.LoggingLevel.Debug else False options.nThreads = 4 options.convergenceDeltaX = 1e-3 options.convergenceDeltaJ = 1 options.maxIterations = 200 options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10) optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: sm.logDebug("Before optimization:") e2 = np.array([e.evaluateError() for e in reprojectionErrors]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) #run intrinsic calibration try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("calibrateIntrinsics: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("calibrateIntrinsics: Optimization failed!") success = False #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: d = cam_geometry.geometry.projection().distortion().getParameters( ).flatten() p = cam_geometry.geometry.projection().getParameters().flatten() sm.logDebug( "calibrateIntrinsics: guess for intrinsics cam: {0}".format(p)) sm.logDebug( "calibrateIntrinsics: guess for distortion cam: {0}".format(d)) return success
def stereoCalibrate(camL_geometry, camH_geometry, obslist, distortionActive=False, baseline=None): ##################################################### ## find initial guess as median of all pnp solutions ##################################################### if baseline is None: r = [] t = [] for obsL, obsH in obslist: #if we have observations for both camss if obsL is not None and obsH is not None: success, T_L = camL_geometry.geometry.estimateTransformation( obsL) success, T_H = camH_geometry.geometry.estimateTransformation( obsH) baseline = T_H.inverse() * T_L t.append(baseline.t()) rv = sm.RotationVector() r.append(rv.rotationMatrixToParameters(baseline.C())) r_median = np.median(np.asmatrix(r), axis=0).flatten().T R_median = rv.parametersToRotationMatrix(r_median) t_median = np.median(np.asmatrix(t), axis=0).flatten().T baseline_HL = sm.Transformation(sm.rt2Transform(R_median, t_median)) else: baseline_HL = baseline #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: dL = camL_geometry.geometry.projection().distortion().getParameters( ).flatten() pL = camL_geometry.geometry.projection().getParameters().flatten() dH = camH_geometry.geometry.projection().distortion().getParameters( ).flatten() pH = camH_geometry.geometry.projection().getParameters().flatten() sm.logDebug("initial guess for stereo calib: {0}".format( baseline_HL.T())) sm.logDebug("initial guess for intrinsics camL: {0}".format(pL)) sm.logDebug("initial guess for intrinsics camH: {0}".format(pH)) sm.logDebug("initial guess for distortion camL: {0}".format(dL)) sm.logDebug("initial guess for distortion camH: {0}".format(dH)) ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #baseline design variable baseline_dv = addPoseDesignVariable(problem, baseline_HL) #target pose dv for all target views (=T_camL_w) target_pose_dvs = list() for obsL, obsH in obslist: if obsL is not None: #use camL if we have an obs for this one success, T_t_cL = camL_geometry.geometry.estimateTransformation( obsL) else: success, T_t_cH = camH_geometry.geometry.estimateTransformation( obsH) T_t_cL = T_t_cH * baseline_HL #apply baseline for the second camera target_pose_dv = addPoseDesignVariable(problem, T_t_cL) target_pose_dvs.append(target_pose_dv) #add camera dvs camL_geometry.setDvActiveStatus(True, distortionActive, False) camH_geometry.setDvActiveStatus(True, distortionActive, False) problem.addDesignVariable(camL_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(camL_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(camL_geometry.dv.shutterDesignVariable()) problem.addDesignVariable(camH_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(camH_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(camH_geometry.dv.shutterDesignVariable()) ############################################ ## add error terms ############################################ #corner uncertainty # \todo pass in the detector uncertainty somehow. cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #Add reprojection error terms for both cameras reprojectionErrors0 = [] reprojectionErrors1 = [] for cidx, cam in enumerate([camL_geometry, camH_geometry]): sm.logDebug( "stereoCalibration: adding camera error terms for {0} calibration targets" .format(len(obslist))) #get the image and target points corresponding to the frame target = cam.ctarget.detector.target() #add error terms for all observations for view_id, obstuple in enumerate(obslist): #add error terms if we have an observation for this cam obs = obstuple[cidx] if obs is not None: T_cam_w = target_pose_dvs[view_id].toExpression().inverse() #add the baseline for the second camera if cidx != 0: T_cam_w = baseline_dv.toExpression() * T_cam_w for i in range(0, target.size()): p_target = aopt.HomogeneousExpression( sm.toHomogeneous(target.point(i))) valid, y = obs.imagePoint(i) if valid: # Create an error term. rerr = cam.model.reprojectionError( y, invR, T_cam_w * p_target, cam.dv) rerr.idx = i problem.addErrorTerm(rerr) if cidx == 0: reprojectionErrors0.append(rerr) else: reprojectionErrors1.append(rerr) sm.logDebug("stereoCalibrate: added {0} camera error terms".format( len(reprojectionErrors0) + len(reprojectionErrors1))) ############################################ ## solve ############################################ options = aopt.Optimizer2Options() options.verbose = True if sm.getLoggingLevel( ) == sm.LoggingLevel.Debug else False options.nThreads = 4 options.convergenceDeltaX = 1e-3 options.convergenceDeltaJ = 1 options.maxIterations = 200 options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10) optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: sm.logDebug("Before optimization:") e2 = np.array([e.evaluateError() for e in reprojectionErrors0]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) e2 = np.array([e.evaluateError() for e in reprojectionErrors1]) sm.logDebug( " Reprojection error squarred (camH): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) sm.logDebug("baseline={0}".format( baseline_dv.toTransformationMatrix())) try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("stereoCalibrate: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("stereoCalibrate: Optimization failed!") success = False if sm.getLoggingLevel() == sm.LoggingLevel.Debug: sm.logDebug("After optimization:") e2 = np.array([e.evaluateError() for e in reprojectionErrors0]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) e2 = np.array([e.evaluateError() for e in reprojectionErrors1]) sm.logDebug( " Reprojection error squarred (camH): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: dL = camL_geometry.geometry.projection().distortion().getParameters( ).flatten() pL = camL_geometry.geometry.projection().getParameters().flatten() dH = camH_geometry.geometry.projection().distortion().getParameters( ).flatten() pH = camH_geometry.geometry.projection().getParameters().flatten() sm.logDebug("guess for intrinsics camL: {0}".format(pL)) sm.logDebug("guess for intrinsics camH: {0}".format(pH)) sm.logDebug("guess for distortion camL: {0}".format(dL)) sm.logDebug("guess for distortion camH: {0}".format(dH)) if success: baseline_HL = sm.Transformation(baseline_dv.toTransformationMatrix()) return success, baseline_HL else: #return the initial guess if we fail return success, baseline_HL
def solveFullBatch(cameras, baseline_guesses, graph): ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #add camera dvs for cam in cameras: cam.setDvActiveStatus(True, True, False) problem.addDesignVariable(cam.dv.distortionDesignVariable()) problem.addDesignVariable(cam.dv.projectionDesignVariable()) problem.addDesignVariable(cam.dv.shutterDesignVariable()) baseline_dvs = list() for baseline_idx in range(0, len(cameras)-1): baseline_dv = aopt.TransformationDv(baseline_guesses[baseline_idx]) for i in range(0, baseline_dv.numDesignVariables()): problem.addDesignVariable(baseline_dv.getDesignVariable(i)) baseline_dvs.append( baseline_dv ) #corner uncertainty cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #get the target target = cameras[0].ctarget.detector.target() #Add calibration target reprojection error terms for all camera in chain target_pose_dvs = list() #shuffle the views reprojectionErrors = []; timestamps = graph.obs_db.getAllViewTimestamps() for view_id, timestamp in enumerate(timestamps): #get all observations for all cams at this time obs_tuple = graph.obs_db.getAllObsAtTimestamp(timestamp) #create a target pose dv for all target views (= T_cam0_w) T0 = graph.getTargetPoseGuess(timestamp, cameras, baseline_guesses) target_pose_dv = addPoseDesignVariable(problem, T0) target_pose_dvs.append(target_pose_dv) for cidx, obs in obs_tuple: cam = cameras[cidx] #calibration target coords to camera X coords T_cam0_calib = target_pose_dv.toExpression().inverse() #build pose chain (target->cam0->baselines->camN) T_camN_calib = T_cam0_calib for idx in range(0, cidx): T_camN_calib = baseline_dvs[idx].toExpression() * T_camN_calib ## add error terms for i in range(0, target.size()): p_target = aopt.HomogeneousExpression(sm.toHomogeneous(target.point(i))); valid, y = obs.imagePoint(i) if valid: rerr = cameras[cidx].model.reprojectionError(y, invR, T_camN_calib * p_target, cameras[cidx].dv) problem.addErrorTerm(rerr) reprojectionErrors.append(rerr) sm.logDebug("solveFullBatch: added {0} camera error terms".format(len(reprojectionErrors))) ############################################ ## solve ############################################ options = aopt.Optimizer2Options() options.verbose = True if sm.getLoggingLevel()==sm.LoggingLevel.Debug else False options.nThreads = 4 options.convergenceDeltaX = 1e-3 options.convergenceDeltaJ = 1 options.maxIterations = 250 options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10) optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #verbose output if sm.getLoggingLevel()==sm.LoggingLevel.Debug: sm.logDebug("Before optimization:") e2 = np.array([ e.evaluateError() for e in reprojectionErrors ]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}".format(np.mean(e2), np.median(e2), np.std(e2) ) ) #run intrinsic calibration try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("calibrateIntrinsics: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("calibrateIntrinsics: Optimization failed!") success = False baselines=list() for baseline_dv in baseline_dvs: baselines.append( sm.Transformation(baseline_dv.T()) ) return success, baselines
def calibrateIntrinsics(cam_geometry, obslist, distortionActive=True, intrinsicsActive=True): #verbose output if sm.getLoggingLevel()==sm.LoggingLevel.Debug: d = cam_geometry.geometry.projection().distortion().getParameters().flatten() p = cam_geometry.geometry.projection().getParameters().flatten() sm.logDebug("calibrateIntrinsics: intrinsics guess: {0}".format(p)) sm.logDebug("calibrateIntrinsics: distortion guess: {0}".format(d)) ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #add camera dvs cam_geometry.setDvActiveStatus(intrinsicsActive, distortionActive, False) problem.addDesignVariable(cam_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(cam_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(cam_geometry.dv.shutterDesignVariable()) #corner uncertainty cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #get the image and target points corresponding to the frame target = cam_geometry.ctarget.detector.target() #target pose dv for all target views (=T_camL_w) reprojectionErrors = []; sm.logDebug("calibrateIntrinsics: adding camera error terms for {0} calibration targets".format(len(obslist))) target_pose_dvs=list() for obs in obslist: success, T_t_c = cam_geometry.geometry.estimateTransformation(obs) target_pose_dv = addPoseDesignVariable(problem, T_t_c) target_pose_dvs.append(target_pose_dv) T_cam_w = target_pose_dv.toExpression().inverse() ## add error terms for i in range(0, target.size()): p_target = aopt.HomogeneousExpression(sm.toHomogeneous(target.point(i))); valid, y = obs.imagePoint(i) if valid: rerr = cam_geometry.model.reprojectionError(y, invR, T_cam_w * p_target, cam_geometry.dv) problem.addErrorTerm(rerr) reprojectionErrors.append(rerr) sm.logDebug("calibrateIntrinsics: added {0} camera error terms".format(len(reprojectionErrors))) ############################################ ## solve ############################################ options = aopt.Optimizer2Options() options.verbose = True if sm.getLoggingLevel()==sm.LoggingLevel.Debug else False options.nThreads = 4 options.convergenceDeltaX = 1e-3 options.convergenceDeltaJ = 1 options.maxIterations = 200 options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10) optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #verbose output if sm.getLoggingLevel()==sm.LoggingLevel.Debug: sm.logDebug("Before optimization:") e2 = np.array([ e.evaluateError() for e in reprojectionErrors ]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}".format(np.mean(e2), np.median(e2), np.std(e2) ) ) #run intrinsic calibration try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("calibrateIntrinsics: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("calibrateIntrinsics: Optimization failed!") success = False #verbose output if sm.getLoggingLevel()==sm.LoggingLevel.Debug: d = cam_geometry.geometry.projection().distortion().getParameters().flatten() p = cam_geometry.geometry.projection().getParameters().flatten() sm.logDebug("calibrateIntrinsics: guess for intrinsics cam: {0}".format(p)) sm.logDebug("calibrateIntrinsics: guess for distortion cam: {0}".format(d)) return success
def stereoCalibrate(camL_geometry, camH_geometry, obslist, distortionActive=False, baseline=None): ##################################################### ## find initial guess as median of all pnp solutions ##################################################### if baseline is None: r=[]; t=[] for obsL, obsH in obslist: #if we have observations for both camss if obsL is not None and obsH is not None: success, T_L = camL_geometry.geometry.estimateTransformation(obsL) success, T_H = camH_geometry.geometry.estimateTransformation(obsH) baseline = T_H.inverse()*T_L t.append(baseline.t()) rv=sm.RotationVector() r.append(rv.rotationMatrixToParameters( baseline.C() )) r_median = np.median(np.asmatrix(r), axis=0).flatten().T R_median = rv.parametersToRotationMatrix(r_median) t_median = np.median(np.asmatrix(t), axis=0).flatten().T baseline_HL = sm.Transformation( sm.rt2Transform(R_median, t_median) ) else: baseline_HL = baseline #verbose output if sm.getLoggingLevel()==sm.LoggingLevel.Debug: dL = camL_geometry.geometry.projection().distortion().getParameters().flatten() pL = camL_geometry.geometry.projection().getParameters().flatten() dH = camH_geometry.geometry.projection().distortion().getParameters().flatten() pH = camH_geometry.geometry.projection().getParameters().flatten() sm.logDebug("initial guess for stereo calib: {0}".format(baseline_HL.T())) sm.logDebug("initial guess for intrinsics camL: {0}".format(pL)) sm.logDebug("initial guess for intrinsics camH: {0}".format(pH)) sm.logDebug("initial guess for distortion camL: {0}".format(dL)) sm.logDebug("initial guess for distortion camH: {0}".format(dH)) ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #baseline design variable baseline_dv = addPoseDesignVariable(problem, baseline_HL) #target pose dv for all target views (=T_camL_w) target_pose_dvs = list() for obsL, obsH in obslist: if obsL is not None: #use camL if we have an obs for this one success, T_t_cL = camL_geometry.geometry.estimateTransformation(obsL) else: success, T_t_cH = camH_geometry.geometry.estimateTransformation(obsH) T_t_cL = T_t_cH*baseline_HL #apply baseline for the second camera target_pose_dv = addPoseDesignVariable(problem, T_t_cL) target_pose_dvs.append(target_pose_dv) #add camera dvs camL_geometry.setDvActiveStatus(True, distortionActive, False) camH_geometry.setDvActiveStatus(True, distortionActive, False) problem.addDesignVariable(camL_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(camL_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(camL_geometry.dv.shutterDesignVariable()) problem.addDesignVariable(camH_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(camH_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(camH_geometry.dv.shutterDesignVariable()) ############################################ ## add error terms ############################################ #corner uncertainty # \todo pass in the detector uncertainty somehow. cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #Add reprojection error terms for both cameras reprojectionErrors0 = []; reprojectionErrors1 = [] for cidx, cam in enumerate([camL_geometry, camH_geometry]): sm.logDebug("stereoCalibration: adding camera error terms for {0} calibration targets".format(len(obslist))) #get the image and target points corresponding to the frame target = cam.ctarget.detector.target() #add error terms for all observations for view_id, obstuple in enumerate(obslist): #add error terms if we have an observation for this cam obs=obstuple[cidx] if obs is not None: T_cam_w = target_pose_dvs[view_id].toExpression().inverse() #add the baseline for the second camera if cidx!=0: T_cam_w = baseline_dv.toExpression() * T_cam_w for i in range(0, target.size()): p_target = aopt.HomogeneousExpression(sm.toHomogeneous(target.point(i))); valid, y = obs.imagePoint(i) if valid: # Create an error term. rerr = cam.model.reprojectionError(y, invR, T_cam_w * p_target, cam.dv) rerr.idx = i problem.addErrorTerm(rerr) if cidx==0: reprojectionErrors0.append(rerr) else: reprojectionErrors1.append(rerr) sm.logDebug("stereoCalibrate: added {0} camera error terms".format( len(reprojectionErrors0)+len(reprojectionErrors1) )) ############################################ ## solve ############################################ options = aopt.Optimizer2Options() options.verbose = True if sm.getLoggingLevel()==sm.LoggingLevel.Debug else False options.nThreads = 4 options.convergenceDeltaX = 1e-3 options.convergenceDeltaJ = 1 options.maxIterations = 200 options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10) optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #verbose output if sm.getLoggingLevel()==sm.LoggingLevel.Debug: sm.logDebug("Before optimization:") e2 = np.array([ e.evaluateError() for e in reprojectionErrors0 ]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}".format(np.mean(e2), np.median(e2), np.std(e2) ) ) e2 = np.array([ e.evaluateError() for e in reprojectionErrors1 ]) sm.logDebug( " Reprojection error squarred (camH): mean {0}, median {1}, std: {2}".format(np.mean(e2), np.median(e2), np.std(e2) ) ) sm.logDebug("baseline={0}".format(baseline_dv.toTransformationMatrix())) try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("stereoCalibrate: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("stereoCalibrate: Optimization failed!") success = False if sm.getLoggingLevel()==sm.LoggingLevel.Debug: sm.logDebug("After optimization:") e2 = np.array([ e.evaluateError() for e in reprojectionErrors0 ]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}".format(np.mean(e2), np.median(e2), np.std(e2) ) ) e2 = np.array([ e.evaluateError() for e in reprojectionErrors1 ]) sm.logDebug( " Reprojection error squarred (camH): mean {0}, median {1}, std: {2}".format(np.mean(e2), np.median(e2), np.std(e2) ) ) #verbose output if sm.getLoggingLevel()==sm.LoggingLevel.Debug: dL = camL_geometry.geometry.projection().distortion().getParameters().flatten() pL = camL_geometry.geometry.projection().getParameters().flatten() dH = camH_geometry.geometry.projection().distortion().getParameters().flatten() pH = camH_geometry.geometry.projection().getParameters().flatten() sm.logDebug("guess for intrinsics camL: {0}".format(pL)) sm.logDebug("guess for intrinsics camH: {0}".format(pH)) sm.logDebug("guess for distortion camL: {0}".format(dL)) sm.logDebug("guess for distortion camH: {0}".format(dH)) if success: baseline_HL = sm.Transformation(baseline_dv.toTransformationMatrix()) return success, baseline_HL else: #return the intiial guess if we fail return success, baseline_HL
def __buildOptimizationProblem(self, W): """Build the optimisation problem""" problem = inc.CalibrationOptimizationProblem() # Initialize all design variables. self.__initPoseDesignVariables(problem) ##### ## build error terms and add to problem # store all frames self.__frames = [] self.__reprojection_errors = [] ##### # activate design variables self.__camera_dv.setActive( self.__config.estimateParameters['intrinsics'], self.__config.estimateParameters['distortion'], self.__config.estimateParameters['shutter']) if self.frameTimeToFirstLineTimeDv is not None: self.frameTimeToFirstLineTimeDv.setActive(True) problem.addDesignVariable(self.frameTimeToFirstLineTimeDv, CALIBRATION_GROUP_ID) ##### # Add design variables # add the camera design variables last for optimal sparsity patterns problem.addDesignVariable(self.__camera_dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) ##### # Regularization term / motion prior motionError = asp.BSplineMotionError(self.__poseSpline_dv, W) problem.addErrorTerm(motionError) ##### # add a reprojection error for every corner of each observation for observation in self.__observations: # only process successful observations of a pattern if (observation.hasSuccessfulObservation()): # add a frame frame = self.__cameraModelFactory.frameType() frame.setGeometry(self.__camera) frame.setTime(observation.time()) self.__frames.append(frame) ##### # add an error term for every observed corner for imagePoint, targetPoint in zip( observation.getCornersImageFrame(), observation.getCornersTargetFrame()): # keypoint time offset by line delay as expression type if self.frameTimeToFirstLineTimeDv is not None: keypoint_time = self.__camera_dv.keypointTime( frame.time(), imagePoint ) + self.frameTimeToFirstLineTimeDv.toExpression() else: keypoint_time = self.__camera_dv.keypointTime( frame.time(), imagePoint) + self.frameTimeToFirstLineTime # from camera to target transformation. T_t_c = self.__poseSpline_dv.transformationAtTime( keypoint_time, self.__config.timeOffsetConstantSparsityPattern, self.__config.timeOffsetConstantSparsityPattern) T_c_t = T_t_c.inverse() # transform target point to camera frame p_c = T_c_t * aopt.HomogeneousExpression( sm.toHomogeneous(targetPoint)) # create the keypoint keypoint = acv.Keypoint2() keypoint.setMeasurement(imagePoint) inverseFeatureCovariance = self.__config.inverseFeatureCovariance keypoint.setInverseMeasurementCovariance( np.eye(len(imagePoint)) * inverseFeatureCovariance) frame.addKeypoint(keypoint) keypoint_index = frame.numKeypoints() - 1 # create reprojection error reprojection_error = self.__buildErrorTerm( frame, keypoint_index, p_c, self.__camera_dv, self.__poseSpline_dv) self.__reprojection_errors.append(reprojection_error) problem.addErrorTerm(reprojection_error) return problem
def __buildOptimizationProblem(self, W): """Build the optimisation problem""" problem = inc.CalibrationOptimizationProblem() # Initialize all design variables. self.__initPoseDesignVariables(problem) ##### ## build error terms and add to problem # store all frames self.__frames = [] self.__reprojection_errors = [] # This code assumes that the order of the landmarks in the observations # is invariant across all observations. At least for the chessboards it is true. ##### # add all the landmarks once landmarks = [] landmarks_expr = {} keypoint_ids0 = self.__observations[0].getCornersIdx() for idx, landmark in enumerate(self.__observations[0].getCornersTargetFrame()): # design variable for landmark landmark_w_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(landmark)) landmark_w_dv.setActive(self.__config.estimateParameters['landmarks']); landmarks.append(landmark_w_dv) landmarks_expr[keypoint_ids0[idx]] = landmark_w_dv.toExpression() problem.addDesignVariable(landmark_w_dv, CALIBRATION_GROUP_ID) ##### # activate design variables self.__camera_dv.setActive( self.__config.estimateParameters['intrinsics'], self.__config.estimateParameters['distortion'], self.__config.estimateParameters['shutter'] ) ##### # Add design variables # add the camera design variables last for optimal sparsity patterns problem.addDesignVariable(self.__camera_dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) ##### # Regularization term / motion prior motionError = asp.BSplineMotionError(self.__poseSpline_dv, W) problem.addErrorTerm(motionError) ##### # add a reprojection error for every corner of each observation for frameid, observation in enumerate(self.__observations): # only process successful observations of a pattern if (observation.hasSuccessfulObservation()): # add a frame frame = self.__cameraModelFactory.frameType() frame.setGeometry(self.__camera) frame.setTime(observation.time()) self.__frames.append(frame) ##### # add an error term for every observed corner for index, point in enumerate(observation.getCornersImageFrame()): # keypoint time offset by line delay as expression type keypoint_time = self.__camera_dv.keypointTime(frame.time(), point) # from target to world transformation. T_w_t = self.__poseSpline_dv.transformationAtTime( keypoint_time, self.__config.timeOffsetConstantSparsityPattern, self.__config.timeOffsetConstantSparsityPattern ) T_t_w = T_w_t.inverse() # we only have the the first image's design variables # so any landmark that is not in that frame won't be in the problem # thus we must skip those measurements that are of a keypoint that isn't visible keypoint_ids = observation.getCornersIdx() if not np.any(keypoint_ids[index]==keypoint_ids0): sm.logWarn("landmark {0} in frame {1} not in first frame".format(keypoint_ids[index],frameid)) continue # transform target point to camera frame p_t = T_t_w * landmarks_expr[keypoint_ids[index]] # create the keypoint keypoint_index = frame.numKeypoints() keypoint = acv.Keypoint2() keypoint.setMeasurement(point) inverseFeatureCovariance = self.__config.inverseFeatureCovariance; keypoint.setInverseMeasurementCovariance(np.eye(len(point)) * inverseFeatureCovariance) keypoint.setLandmarkId(keypoint_index) frame.addKeypoint(keypoint) # create reprojection error reprojection_error = self.__buildErrorTerm( frame, keypoint_index, p_t, self.__camera_dv, self.__poseSpline_dv ) self.__reprojection_errors.append(reprojection_error) problem.addErrorTerm(reprojection_error) return problem