Example #1
0
 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
Example #2
0
 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
Example #3
0
    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
Example #4
0
    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
Example #5
0
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
Example #6
0
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
Example #7
0
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
Example #8
0
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
Example #9
0
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
Example #10
0
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
Example #11
0
    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
Example #12
0
    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