def addDesignVariables(self,
problem,
noExtrinsics=True,
noTimeCalibration=True,
baselinedv_group_id=ic.HELPER_GROUP_ID):
# Add the calibration design variables.
active = not noExtrinsics
self.T_c_b_Dv = aopt.TransformationDv(self.T_extrinsic,
rotationActive=active,
translationActive=active)
for i in range(0, self.T_c_b_Dv.numDesignVariables()):
problem.addDesignVariable(self.T_c_b_Dv.getDesignVariable(i),
baselinedv_group_id)
# Add the time delay design variable.
self.cameraTimeToImuTimeDv = aopt.Scalar(0.0)
self.cameraTimeToImuTimeDv.setActive(not noTimeCalibration)
problem.addDesignVariable(self.cameraTimeToImuTimeDv,
ic.CALIBRATION_GROUP_ID)
def registration(self, target, w=0.0,
objective_type='pt2pt',
maxiter=50, tol=0.001,
feature_fn=lambda x: x):
q = None
ftarget = feature_fn(target)
# build the problem
self.problem = aopt.OptimizationProblem()
T_init = np.array([[-0.99813747, -0.05730897, -0.02091093, 0.03399231],
[0.05726434, -0.99835533, 0.00272772, 0.31433327],
[-0.02103286, 0.00152519, 0.99977762, 0.22997991],
[0., 0., 0., 1.]])
self.T_b_l_Dv = aopt.TransformationDv(sm.Transformation(), rotationActive=True, translationActive=True)
for i in range(0, self.T_b_l_Dv.numDesignVariables()):
self.problem.addDesignVariable(self.T_b_l_Dv.getDesignVariable(i))
for _ in range(maxiter):
T_b_l = self.T_b_l_Dv.toExpression().toTransformationMatrix()
print("Inital laser to body transformation: T_b_l ")
print(T_b_l)
T_l_b = np.linalg.inv(T_b_l)
numPoints = self._source.shape[0]
source = np.hstack([self._source, np.ones((numPoints, 1), dtype=self._source.dtype)])
t_source = np.array([np.dot(T_l_b, np.dot(self._sensor_tfs[i], np.dot(T_b_l, source[i]))) for i in xrange(numPoints)])
t_source = t_source[:, :3]
util.showPointCloud([t_source, target])
fsource = feature_fn(t_source)
estep_res = self.expectation_step(fsource, ftarget, target, objective_type)
res = self.maximization_step(self._source, target, estep_res, w=w,
objective_type=objective_type)
if not q is None and abs(res.q - q) < tol:
break
q = res.q
return res.transformation
def initializeBaselineDVs(self, baseline_guesses):
self.baselines = list()
for baseline_idx in range(0, len(self.cameras) - 1):
self.baselines.append(
aopt.TransformationDv(baseline_guesses[baseline_idx]))
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 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
