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 = [] 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 __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