def fromTargetViewObservations(cls,
cameras,
target,
baselines,
T_tc_guess,
rig_observations,
useBlakeZissermanMest=True):
rval = CalibrationTargetOptimizationProblem()
#store the arguements in case we want to rebuild a modified problem
rval.cameras = cameras
rval.target = target
rval.baselines = baselines
rval.T_tc_guess = T_tc_guess
rval.rig_observations = rig_observations
# 1. Create a design variable for this pose
T_target_camera = T_tc_guess
rval.dv_T_target_camera = aopt.TransformationDv(T_target_camera)
for i in range(0, rval.dv_T_target_camera.numDesignVariables()):
rval.addDesignVariable(
rval.dv_T_target_camera.getDesignVariable(i),
TRANSFORMATION_GROUP_ID)
#2. add all baselines DVs
for baseline_dv in baselines:
for i in range(0, baseline_dv.numDesignVariables()):
rval.addDesignVariable(baseline_dv.getDesignVariable(i),
CALIBRATION_GROUP_ID)
#3. add landmark DVs
for p in target.P_t_dv:
rval.addDesignVariable(p, LANDMARK_GROUP_ID)
#4. add camera DVs
for camera in cameras:
if not camera.isGeometryInitialized:
raise RuntimeError(
'The camera geometry is not initialized. Please initialize with initGeometry() or initGeometryFromDataset()'
)
camera.setDvActiveStatus(True, True, False)
rval.addDesignVariable(camera.dv.distortionDesignVariable(),
CALIBRATION_GROUP_ID)
rval.addDesignVariable(camera.dv.projectionDesignVariable(),
CALIBRATION_GROUP_ID)
rval.addDesignVariable(camera.dv.shutterDesignVariable(),
CALIBRATION_GROUP_ID)
#4.add all observations for this view
cams_in_view = set()
rval.rerrs = dict()
rerr_cnt = 0
for cam_id, obs in rig_observations:
camera = cameras[cam_id]
cams_in_view.add(cam_id)
#add reprojection errors
#build baseline chain (target->cam0->baselines->camN)
T_cam0_target = rval.dv_T_target_camera.expression.inverse()
T_camN_calib = T_cam0_target
for idx in range(0, cam_id):
T_camN_calib = baselines[idx].toExpression() * T_camN_calib
# \todo pass in the detector uncertainty somehow.
cornerUncertainty = 1.0
R = np.eye(2) * cornerUncertainty * cornerUncertainty
invR = np.linalg.inv(R)
rval.rerrs[cam_id] = list()
for i in range(0, len(target.P_t_ex)):
p_target = target.P_t_ex[i]
valid, y = obs.imagePoint(i)
if valid:
rerr_cnt += 1
# Create an error term.
rerr = camera.model.reprojectionError(
y, invR, T_camN_calib * p_target, camera.dv)
rerr.idx = i
#add blake-zisserman mest
if useBlakeZissermanMest:
mest = aopt.BlakeZissermanMEstimator(2.0)
rerr.setMEstimatorPolicy(mest)
rval.addErrorTerm(rerr)
rval.rerrs[cam_id].append(rerr)
else:
rval.rerrs[cam_id].append(None)
sm.logDebug(
"Adding a view with {0} cameras and {1} error terms".format(
len(cams_in_view), rerr_cnt))
return rval
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