def printDebugEnd(cself):
print
print
for cidx, cam in enumerate(cself.cameras):
print "cam{0}".format(cidx)
print "----------"
print
print
corners, reprojs, rerrs = getReprojectionErrors(cself, cidx)
if len(rerrs) > 0:
me, se = getReprojectionErrorStatistics(rerrs)
print me[0]
print me[1]
print se[0]
print se[1]
print
p = cam.geometry.projection().getParameters().flatten(1)
for temp in p:
print temp
print
d = cam.geometry.projection().distortion().getParameters().flatten(1)
for temp in d:
print temp
if cidx > 0:
bidx = cidx - 1
T = sm.Transformation(cself.baselines[bidx].T())
for temp in T.t():
print temp
for temp in T.q():
print temp
print
print
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 printParameters(cself, dest=sys.stdout):
#get the covariances
std_baselines, std_cameras = recoverCovariance(cself)
#print cameras
print("Camera-system parameters:", file=dest)
for cidx, cam in enumerate(cself.cameras):
d = cam.geometry.projection().distortion().getParameters().flatten()
p = cam.geometry.projection().getParameters().flatten()
dd = std_cameras[cidx][0:d.shape[0]]
dp = std_cameras[cidx][d.shape[0]:]
print("\tcam{0} ({1}):".format(cidx, cam.dataset.topic), file=dest)
print("\t type: %s" % (type(cam.geometry)), file=dest)
print("\t distortion: %s +- %s" % (d, np.array(dd)), file=dest)
print("\t projection: %s +- %s" % (p, np.array(dp)), file=dest)
#reproj error statistics
corners, reprojs, rerrs = getReprojectionErrors(cself, cidx)
if len(rerrs) > 0:
me, se = getReprojectionErrorStatistics(rerrs)
try:
print("\t reprojection error: [%f, %f] +- [%f, %f]" %
(me[0], me[1], se[0], se[1]),
file=dest)
except:
print("\t Failed printing the reprojection error.", file=dest)
print(file=dest)
#print baselines
for bidx, baseline in enumerate(cself.baselines):
T = sm.Transformation(baseline.T())
dq = std_baselines[bidx][0:3]
dt = std_baselines[bidx][3:6]
print("\tbaseline T_{1}_{0}:".format(bidx, bidx + 1), file=dest)
print("\t q: %s +- %s" % (T.q(), np.array(dq)), file=dest)
print("\t t: %s +- %s" % (T.t(), np.array(dt)), file=dest)
print(file=dest)
def __init__(self, imuConfig, parsed):
#store input
self.imuConfig = imuConfig
#load dataset
self.dataset = initImuBagDataset(parsed.bagfile[0],
imuConfig.getRosTopic(),
parsed.bag_from_to)
#statistics
self.accelUncertaintyDiscrete, self.accelRandomWalk, self.accelUncertainty = self.imuConfig.getAccelerometerStatistics(
)
self.gyroUncertaintyDiscrete, self.gyroRandomWalk, self.gyroUncertainty = self.imuConfig.getGyroStatistics(
)
#init GyroBiasPrior (+ count for recursive averaging if we have more than 1 measurement = >1 cameras)
self.GyroBiasPrior = np.array([0, 0, 0])
self.GyroBiasPriorCount = 0
#imu position
self.T_ib = sm.Transformation()
#load the imu dataset
self.loadImuData()
def saveChainParametersYaml(cself, resultFile, graph):
cameraModels = {
acvb.DistortedPinhole: 'pinhole',
acvb.EquidistantPinhole: 'pinhole',
acvb.FovPinhole: 'pinhole',
acvb.Omni: 'omni',
acvb.DistortedOmni: 'omni',
acvb.ExtendedUnified: 'eucm',
acvb.DoubleSphere: 'ds'
}
distortionModels = {
acvb.DistortedPinhole: 'radtan',
acvb.EquidistantPinhole: 'equidistant',
acvb.FovPinhole: 'fov',
acvb.Omni: 'none',
acvb.DistortedOmni: 'radtan',
acvb.ExtendedUnified: 'none',
acvb.DoubleSphere: 'none'
}
chain = cr.CameraChainParameters(resultFile, createYaml=True)
for cam_id, cam in enumerate(cself.cameras):
cameraModel = cameraModels[cam.model]
distortionModel = distortionModels[cam.model]
#create new config file
camParams = cr.CameraParameters(resultFile, createYaml=True)
camParams.setRosTopic(cam.dataset.topic)
#set the data
P = cam.geometry.projection()
if cameraModel == 'omni':
camParams.setIntrinsics(
cameraModel,
[P.xi(), P.fu(), P.fv(),
P.cu(), P.cv()])
elif cameraModel == 'pinhole':
camParams.setIntrinsics(
cameraModel,
[P.fu(), P.fv(), P.cu(), P.cv()])
elif cameraModel == 'eucm':
camParams.setIntrinsics(
cameraModel,
[P.alpha(),
P.beta(), P.fu(),
P.fv(), P.cu(),
P.cv()])
elif cameraModel == 'ds':
camParams.setIntrinsics(
cameraModel,
[P.xi(), P.alpha(),
P.fu(), P.fv(),
P.cu(), P.cv()])
else:
raise RuntimeError("Invalid camera model {}.".format(cameraModel))
camParams.setResolution([P.ru(), P.rv()])
dist_coeffs = P.distortion().getParameters().flatten(1)
camParams.setDistortion(distortionModel, dist_coeffs)
chain.addCameraAtEnd(camParams)
for cam_id, cam in enumerate(cself.cameras):
overlaps = graph.getCamOverlaps(cam_id)
chain.setCamOverlaps(cam_id, overlaps)
#print all baselines in the camera chain
for bidx, baseline_dv in enumerate(cself.baselines):
baseline = sm.Transformation(baseline_dv.T())
camNr = bidx + 1
chain.setExtrinsicsLastCamToHere(camNr, baseline)
chain.writeYaml()
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 getInitialGuesses(self, cameras):
if not self.G:
raise RuntimeError("Graph is uninitialized!")
#################################################################
## STEP 0: check if all cameras in the chain are connected
## through common target point observations
## (=all vertices connected?)
#################################################################
if not self.isGraphConnected():
sm.logError(
"The cameras are not connected through mutual target observations! "
"Please provide another dataset...")
self.plotGraph()
sys.exit(0)
#################################################################
## STEP 1: get baseline initial guesses by calibrating good
## camera pairs using a stereo calibration
##
#################################################################
#first we need to find the best camera pairs to obtain the initial guesses
#--> use the pairs that share the most common observed target corners
#The graph is built with weighted edges that represent the number of common
#target corners, so we can use dijkstras algorithm to get the best pair
#configuration for the initial pair calibrations
weights = [1.0 / commonPoints for commonPoints in self.G.es["weight"]]
#choose the cam with the least edges as base_cam
outdegrees = self.G.vs.outdegree()
base_cam_id = outdegrees.index(min(outdegrees))
#solve for shortest path (=optimal transformation chaining)
edges_on_path = self.G.get_shortest_paths(0,
weights=weights,
output="epath")
self.optimal_baseline_edges = set(
[item for sublist in edges_on_path for item in sublist])
#################################################################
## STEP 2: solve stereo calibration problem for the baselines
## (baselines are always from lower_id to higher_id cams!)
#################################################################
#calibrate all cameras in pairs
for baseline_edge_id in self.optimal_baseline_edges:
#get the cam_nrs from the graph edge (calibrate from low to high id)
vertices = self.G.es[baseline_edge_id].tuple
if vertices[0] < vertices[1]:
camL_nr = vertices[0]
camH_nr = vertices[1]
else:
camL_nr = vertices[1]
camH_nr = vertices[0]
print "\t initializing camera pair ({0},{1})... ".format(
camL_nr, camH_nr)
#run the pair extrinsic calibration
obs_list = self.obs_db.getAllObsTwoCams(camL_nr, camH_nr)
success, baseline_HL = kcc.stereoCalibrate(cameras[camL_nr],
cameras[camH_nr],
obs_list,
distortionActive=False)
if success:
sm.logDebug("baseline_{0}_{1}={2}".format(
camL_nr, camH_nr, baseline_HL.T()))
else:
sm.logError(
"initialization of camera pair ({0},{1}) failed ".format(
camL_nr, camH_nr))
sm.logError("estimated baseline_{0}_{1}={2}".format(
camL_nr, camH_nr, baseline_HL.T()))
#store the baseline in the graph
self.G.es[self.G.get_eid(camL_nr,
camH_nr)]["baseline_HL"] = baseline_HL
#################################################################
## STEP 3: transform from the "optimal" baseline chain to camera chain ordering
## (=> baseline_0 = T_c1_c0 |
#################################################################
#construct the optimal path graph
G_optimal_baselines = self.G.copy()
eid_not_optimal_path = set(range(0, len(G_optimal_baselines.es)))
for eid in self.optimal_baseline_edges:
eid_not_optimal_path.remove(eid)
G_optimal_baselines.delete_edges(eid_not_optimal_path)
#now we convert the arbitary baseline graph to baselines starting from
# cam0 and traverse the chain (cam0->cam1->cam2->camN)
baselines = []
for baseline_id in range(0, self.numCams - 1):
#find the shortest path on the graph
path = G_optimal_baselines.get_shortest_paths(
baseline_id, baseline_id + 1)[0]
#get the baseline from cam with id baseline_id to baseline_id+1
baseline_HL = sm.Transformation()
for path_idx in range(0, len(path) - 1):
source_vert = path[path_idx]
target_vert = path[path_idx + 1]
T_edge = self.G.es[self.G.get_eid(source_vert,
target_vert)]["baseline_HL"]
#correct the direction (baselines always from low to high cam id!)
T_edge = T_edge if source_vert < target_vert else T_edge.inverse(
)
#chain up
baseline_HL = T_edge * baseline_HL
#store in graph
baselines.append(baseline_HL)
#################################################################
## STEP 4: refine guess in full batch
#################################################################
success, baselines = kcc.solveFullBatch(cameras, baselines, self)
if not success:
sm.logWarn("Full batch refinement failed!")
return baselines
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 saveChainParametersYaml(cself, resultFile, graph, oldcamchain=None, cam_replace=None):
cameraModels = {acvb.DistortedPinhole: 'pinhole',
acvb.EquidistantPinhole: 'pinhole',
acvb.FovPinhole: 'pinhole',
acvb.Omni: 'omni',
acvb.DistortedOmni: 'omni',
acvb.ExtendedUnified: 'eucm',
acvb.DoubleSphere: 'ds'}
distortionModels = {acvb.DistortedPinhole: 'radtan',
acvb.EquidistantPinhole: 'equidistant',
acvb.FovPinhole: 'fov',
acvb.Omni: 'none',
acvb.DistortedOmni: 'radtan',
acvb.ExtendedUnified: 'none',
acvb.DoubleSphere: 'none'}
chain = cr.CameraChainParameters(resultFile, createYaml=True)
for cam_id, cam in enumerate(cself.cameras):
cameraModel = cameraModels[cam.model]
distortionModel = distortionModels[cam.model]
#create new config file
camParams = cr.CameraParameters(resultFile, createYaml=True)
camParams.setRosTopic(cam.dataset.topic)
#set the data
P = cam.geometry.projection()
if cameraModel == 'omni':
camParams.setIntrinsics(cameraModel, [P.xi(), P.fu(), P.fv(), P.cu(), P.cv()] )
elif cameraModel == 'pinhole':
camParams.setIntrinsics(cameraModel, [P.fu(), P.fv(), P.cu(), P.cv()] )
elif cameraModel == 'eucm':
camParams.setIntrinsics(cameraModel, [P.alpha(), P.beta(), P.fu(), P.fv(), P.cu(), P.cv()] )
elif cameraModel == 'ds':
camParams.setIntrinsics(cameraModel, [P.xi(), P.alpha(), P.fu(), P.fv(), P.cu(), P.cv()] )
else:
raise RuntimeError("Invalid camera model {}.".format(cameraModel))
camParams.setResolution( [P.ru(), P.rv()] )
dist_coeffs = P.distortion().getParameters().flatten(1)
camParams.setDistortion( distortionModel, dist_coeffs)
chain.addCameraAtEnd(camParams)
for cam_id, cam in enumerate(cself.cameras):
overlaps = graph.getCamOverlaps(cam_id)
chain.setCamOverlaps(cam_id, overlaps)
#print all baselines in the camera chain
for bidx, baseline_dv in enumerate(cself.baselines):
baseline = sm.Transformation( baseline_dv.T() )
camNr = bidx+1
chain.setExtrinsicsLastCamToHere(camNr, baseline)
# write yaml
if oldcamchain is None:
chain.writeYaml()
else:
for topic in cam_replace:
camConfig,camNr = chain.getCameraParametersforTopic(topic)
camConfig_old, camNr_old = oldcamchain.getCameraParametersforTopic(topic)
camConfig_old.data["intrinsics"] = camConfig.data["intrinsics"]
camConfig_old.data["distortion_coeffs"] = camConfig.data["distortion_coeffs"]
oldcamchain.data["cam{0}".format(camNr_old)]=camConfig_old.getYamlDict()
for topic in cam_replace:
camConfig,camNr = chain.getCameraParametersforTopic(topic)
camConfig_old, camNr_old = oldcamchain.getCameraParametersforTopic(topic)
if camNr == 0 :
print camNr
camH_topic = chain.getCameraParameters(camNr+1).getRosTopic()
camConfig_old_H, camNr_old_H = oldcamchain.getCameraParametersforTopic(camH_topic)
baseline = chain.getExtrinsicsLastCamToHere(camNr+1).inverse()
oldcamchain.setExtrinsicsSomeCamToHere(camNr_old, baseline, camNr_old_H)
else :
camL_topic = chain.getCameraParameters(camNr-1).getRosTopic()
camConfig_old_L, camNr_old_L = oldcamchain.getCameraParametersforTopic(camL_topic)
baseline = chain.getExtrinsicsLastCamToHere(camNr)
oldcamchain.setExtrinsicsSomeCamToHere(camNr_old, baseline, camNr_old_L)
oldcamchain.writeYaml(chain.yamlFile)
