def findOrientationPriorCameraToImu(self, imu): print print "Estimating imu-camera rotation prior" # build the problem problem = aopt.OptimizationProblem() # Add the rotation as design variable q_i_c_Dv = aopt.RotationQuaternionDv(self.T_extrinsic.q()) q_i_c_Dv.setActive(True) problem.addDesignVariable(q_i_c_Dv) # Add the gyro bias as design variable gyroBiasDv = aopt.EuclideanPointDv(np.zeros(3)) gyroBiasDv.setActive(True) problem.addDesignVariable(gyroBiasDv) #initialize a pose spline using the camera poses poseSpline = self.initPoseSplineFromCamera(timeOffsetPadding=0.0) for im in imu.imuData: tk = im.stamp.toSec() if tk > poseSpline.t_min() and tk < poseSpline.t_max(): #DV expressions R_i_c = q_i_c_Dv.toExpression() bias = gyroBiasDv.toExpression() #get the vision predicted omega and measured omega (IMU) omega_predicted = R_i_c * aopt.EuclideanExpression( np.matrix( poseSpline.angularVelocityBodyFrame(tk)).transpose()) omega_measured = im.omega #error term gerr = ket.GyroscopeError(omega_measured, im.omegaInvR, omega_predicted, bias) problem.addErrorTerm(gerr) #define the optimization options = aopt.Optimizer2Options() options.verbose = False options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() options.nThreads = 2 options.convergenceDeltaX = 1e-4 options.convergenceDeltaJ = 1 options.maxIterations = 50 #run the optimization optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #get the prior try: optimizer.optimize() except: sm.logFatal("Failed to obtain orientation prior!") sys.exit(-1) #overwrite the external rotation prior (keep the external translation prior) R_i_c = q_i_c_Dv.toRotationMatrix().transpose() self.T_extrinsic = sm.Transformation( sm.rt2Transform(R_i_c, self.T_extrinsic.t())) #set the gyro bias prior (if we have more than 1 cameras use recursive average) b_gyro = bias.toEuclidean() imu.GyroBiasPriorCount += 1 imu.GyroBiasPrior = ( imu.GyroBiasPriorCount - 1.0 ) / imu.GyroBiasPriorCount * imu.GyroBiasPrior + 1.0 / imu.GyroBiasPriorCount * b_gyro #print result print " Orientation prior camera-imu found as: (T_i_c)" print R_i_c print " Gyro bias prior found as: (b_gyro)" print b_gyro
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 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 findOrientationPriorCameraToImu(self, imu): print print "Estimating imu-camera rotation prior" # build the problem problem = aopt.OptimizationProblem() # Add the rotation as design variable q_i_c_Dv = aopt.RotationQuaternionDv( self.T_extrinsic.q() ) q_i_c_Dv.setActive( True ) problem.addDesignVariable(q_i_c_Dv) # Add the gyro bias as design variable gyroBiasDv = aopt.EuclideanPointDv( np.zeros(3) ) gyroBiasDv.setActive( True ) problem.addDesignVariable(gyroBiasDv) #initialize a pose spline using the camera poses poseSpline = self.initPoseSplineFromCamera( timeOffsetPadding=0.0 ) for im in imu.imuData: tk = im.stamp.toSec() if tk > poseSpline.t_min() and tk < poseSpline.t_max(): #DV expressions R_i_c = q_i_c_Dv.toExpression() bias = gyroBiasDv.toExpression() #get the vision predicted omega and measured omega (IMU) omega_predicted = R_i_c * aopt.EuclideanExpression( np.matrix( poseSpline.angularVelocityBodyFrame( tk ) ).transpose() ) omega_measured = im.omega #error term gerr = ket.GyroscopeError(omega_measured, im.omegaInvR, omega_predicted, bias) problem.addErrorTerm(gerr) #define the optimization options = aopt.Optimizer2Options() options.verbose = False options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() options.nThreads = 2 options.convergenceDeltaX = 1e-4 options.convergenceDeltaJ = 1 options.maxIterations = 50 #run the optimization optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #get the prior try: optimizer.optimize() except: sm.logFatal("Failed to obtain orientation prior!") sys.exit(-1) #overwrite the external rotation prior (keep the external translation prior) R_i_c = q_i_c_Dv.toRotationMatrix().transpose() self.T_extrinsic = sm.Transformation( sm.rt2Transform( R_i_c, self.T_extrinsic.t() ) ) #set the gyro bias prior (if we have more than 1 cameras use recursive average) b_gyro = bias.toEuclidean() imu.GyroBiasPriorCount += 1 imu.GyroBiasPrior = (imu.GyroBiasPriorCount-1.0)/imu.GyroBiasPriorCount * imu.GyroBiasPrior + 1.0/imu.GyroBiasPriorCount*b_gyro #print result print " Orientation prior camera-imu found as: (T_i_c)" print R_i_c print " Gyro bias prior found as: (b_gyro)" print b_gyro