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
