def addCameraErrorTerms(self,
problem,
poseSplineDv,
T_cN_b,
blakeZissermanDf=0.0,
timeOffsetPadding=0.0):
print
print "Adding camera error terms ({0})".format(self.dataset.topic)
#progress bar
iProgress = sm.Progress2(len(self.targetObservations))
iProgress.sample()
allReprojectionErrors = list()
error_t = self.camera.reprojectionErrorType
for obs in self.targetObservations:
# Build a transformation expression for the time.
frameTime = self.cameraTimeToImuTimeDv.toExpression() + obs.time(
).toSec() + self.timeshiftCamToImuPrior
frameTimeScalar = frameTime.toScalar()
#as we are applying an initial time shift outside the optimization so
#we need to make sure that we dont add data outside the spline definition
if frameTimeScalar <= poseSplineDv.spline().t_min(
) or frameTimeScalar >= poseSplineDv.spline().t_max():
continue
T_w_b = poseSplineDv.transformationAtTime(frameTime,
timeOffsetPadding,
timeOffsetPadding)
T_b_w = T_w_b.inverse()
#calibration target coords to camera N coords
#T_b_w: from world to imu coords
#T_cN_b: from imu to camera N coords
T_c_w = T_cN_b * T_b_w
#get the image and target points corresponding to the frame
imageCornerPoints = np.array(obs.getCornersImageFrame()).T
targetCornerPoints = np.array(obs.getCornersTargetFrame()).T
#setup an aslam frame (handles the distortion)
frame = self.camera.frameType()
frame.setGeometry(self.camera.geometry)
#corner uncertainty
R = np.eye(2) * self.cornerUncertainty * self.cornerUncertainty
invR = np.linalg.inv(R)
for pidx in range(0, imageCornerPoints.shape[1]):
#add all image points
k = self.camera.keypointType()
k.setMeasurement(imageCornerPoints[:, pidx])
k.setInverseMeasurementCovariance(invR)
frame.addKeypoint(k)
reprojectionErrors = list()
for pidx in range(0, imageCornerPoints.shape[1]):
#add all target points
targetPoint = np.insert(targetCornerPoints.transpose()[pidx],
3, 1)
p = T_c_w * aopt.HomogeneousExpression(targetPoint)
#build and append the error term
rerr = error_t(frame, pidx, p)
#add blake-zisserman m-estimator
if blakeZissermanDf > 0.0:
mest = aopt.BlakeZissermanMEstimator(blakeZissermanDf)
rerr.setMEstimatorPolicy(mest)
problem.addErrorTerm(rerr)
reprojectionErrors.append(rerr)
allReprojectionErrors.append(reprojectionErrors)
#update progress bar
iProgress.sample()
print "\r Added {0} camera error terms ".format(
len(self.targetObservations))
self.allReprojectionErrors = allReprojectionErrors
def solveFullBatch(cameras, baseline_guesses, graph):
############################################
## solve the bundle adjustment
############################################
problem = aopt.OptimizationProblem()
#add camera dvs
for cam in cameras:
cam.setDvActiveStatus(True, True, False)
problem.addDesignVariable(cam.dv.distortionDesignVariable())
problem.addDesignVariable(cam.dv.projectionDesignVariable())
problem.addDesignVariable(cam.dv.shutterDesignVariable())
baseline_dvs = list()
for baseline_idx in range(0, len(cameras) - 1):
baseline_dv = aopt.TransformationDv(baseline_guesses[baseline_idx])
for i in range(0, baseline_dv.numDesignVariables()):
problem.addDesignVariable(baseline_dv.getDesignVariable(i))
baseline_dvs.append(baseline_dv)
#corner uncertainty
cornerUncertainty = 1.0
R = np.eye(2) * cornerUncertainty * cornerUncertainty
invR = np.linalg.inv(R)
#get the target
target = cameras[0].ctarget.detector.target()
#Add calibration target reprojection error terms for all camera in chain
target_pose_dvs = list()
#shuffle the views
reprojectionErrors = []
timestamps = graph.obs_db.getAllViewTimestamps()
for view_id, timestamp in enumerate(timestamps):
#get all observations for all cams at this time
obs_tuple = graph.obs_db.getAllObsAtTimestamp(timestamp)
#create a target pose dv for all target views (= T_cam0_w)
T0 = graph.getTargetPoseGuess(timestamp, cameras, baseline_guesses)
target_pose_dv = addPoseDesignVariable(problem, T0)
target_pose_dvs.append(target_pose_dv)
for cidx, obs in obs_tuple:
cam = cameras[cidx]
#calibration target coords to camera X coords
T_cam0_calib = target_pose_dv.toExpression().inverse()
#build pose chain (target->cam0->baselines->camN)
T_camN_calib = T_cam0_calib
for idx in range(0, cidx):
T_camN_calib = baseline_dvs[idx].toExpression() * T_camN_calib
## add error terms
for i in range(0, target.size()):
p_target = aopt.HomogeneousExpression(
sm.toHomogeneous(target.point(i)))
valid, y = obs.imagePoint(i)
if valid:
rerr = cameras[cidx].model.reprojectionError(
y, invR, T_camN_calib * p_target, cameras[cidx].dv)
problem.addErrorTerm(rerr)
reprojectionErrors.append(rerr)
sm.logDebug("solveFullBatch: added {0} camera error terms".format(
len(reprojectionErrors)))
############################################
## solve
############################################
options = aopt.Optimizer2Options()
options.verbose = True if sm.getLoggingLevel(
) == sm.LoggingLevel.Debug else False
options.nThreads = 4
options.convergenceDeltaX = 1e-3
options.convergenceDeltaJ = 1
options.maxIterations = 250
options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10)
optimizer = aopt.Optimizer2(options)
optimizer.setProblem(problem)
#verbose output
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
sm.logDebug("Before optimization:")
e2 = np.array([e.evaluateError() for e in reprojectionErrors])
sm.logDebug(
" Reprojection error squarred (camL): mean {0}, median {1}, std: {2}"
.format(np.mean(e2), np.median(e2), np.std(e2)))
#run intrinsic calibration
try:
retval = optimizer.optimize()
if retval.linearSolverFailure:
sm.logError("calibrateIntrinsics: Optimization failed!")
success = not retval.linearSolverFailure
except:
sm.logError("calibrateIntrinsics: Optimization failed!")
success = False
baselines = list()
for baseline_dv in baseline_dvs:
baselines.append(sm.Transformation(baseline_dv.T()))
return success, baselines
def 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 calibrateIntrinsics(cam_geometry,
obslist,
distortionActive=True,
intrinsicsActive=True):
#verbose output
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
d = cam_geometry.geometry.projection().distortion().getParameters(
).flatten()
p = cam_geometry.geometry.projection().getParameters().flatten()
sm.logDebug("calibrateIntrinsics: intrinsics guess: {0}".format(p))
sm.logDebug("calibrateIntrinsics: distortion guess: {0}".format(d))
############################################
## solve the bundle adjustment
############################################
problem = aopt.OptimizationProblem()
#add camera dvs
cam_geometry.setDvActiveStatus(intrinsicsActive, distortionActive, False)
problem.addDesignVariable(cam_geometry.dv.distortionDesignVariable())
problem.addDesignVariable(cam_geometry.dv.projectionDesignVariable())
problem.addDesignVariable(cam_geometry.dv.shutterDesignVariable())
#corner uncertainty
cornerUncertainty = 1.0
R = np.eye(2) * cornerUncertainty * cornerUncertainty
invR = np.linalg.inv(R)
#get the image and target points corresponding to the frame
target = cam_geometry.ctarget.detector.target()
#target pose dv for all target views (=T_camL_w)
reprojectionErrors = []
sm.logDebug(
"calibrateIntrinsics: adding camera error terms for {0} calibration targets"
.format(len(obslist)))
target_pose_dvs = list()
for obs in obslist:
success, T_t_c = cam_geometry.geometry.estimateTransformation(obs)
target_pose_dv = addPoseDesignVariable(problem, T_t_c)
target_pose_dvs.append(target_pose_dv)
T_cam_w = target_pose_dv.toExpression().inverse()
## add error terms
for i in range(0, target.size()):
p_target = aopt.HomogeneousExpression(
sm.toHomogeneous(target.point(i)))
valid, y = obs.imagePoint(i)
if valid:
rerr = cam_geometry.model.reprojectionError(
y, invR, T_cam_w * p_target, cam_geometry.dv)
problem.addErrorTerm(rerr)
reprojectionErrors.append(rerr)
sm.logDebug("calibrateIntrinsics: added {0} camera error terms".format(
len(reprojectionErrors)))
############################################
## solve
############################################
options = aopt.Optimizer2Options()
options.verbose = True if sm.getLoggingLevel(
) == sm.LoggingLevel.Debug else False
options.nThreads = 4
options.convergenceDeltaX = 1e-3
options.convergenceDeltaJ = 1
options.maxIterations = 200
options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10)
optimizer = aopt.Optimizer2(options)
optimizer.setProblem(problem)
#verbose output
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
sm.logDebug("Before optimization:")
e2 = np.array([e.evaluateError() for e in reprojectionErrors])
sm.logDebug(
" Reprojection error squarred (camL): mean {0}, median {1}, std: {2}"
.format(np.mean(e2), np.median(e2), np.std(e2)))
#run intrinsic calibration
try:
retval = optimizer.optimize()
if retval.linearSolverFailure:
sm.logError("calibrateIntrinsics: Optimization failed!")
success = not retval.linearSolverFailure
except:
sm.logError("calibrateIntrinsics: Optimization failed!")
success = False
#verbose output
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
d = cam_geometry.geometry.projection().distortion().getParameters(
).flatten()
p = cam_geometry.geometry.projection().getParameters().flatten()
sm.logDebug(
"calibrateIntrinsics: guess for intrinsics cam: {0}".format(p))
sm.logDebug(
"calibrateIntrinsics: guess for distortion cam: {0}".format(d))
return success
def __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