def saveChainParametersYaml(cself, resultFile, graph):
cameraModels = {
acvb.PerspectivePinhole: 'pinhole',
acvb.DistortedPinhole: 'pinhole',
acvb.EquidistantPinhole: 'pinhole',
acvb.FovPinhole: 'pinhole',
acvb.DistortedOmni: 'omni'
}
distortionModels = {
acvb.PerspectivePinhole: 'perspective',
acvb.DistortedPinhole: 'radtan',
acvb.EquidistantPinhole: 'equidistant',
acvb.FovPinhole: 'fov',
acvb.DistortedOmni: 'radtan'
}
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()])
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 __saveParametersYaml(self):
# Create new config file
bagtag = os.path.splitext(self.__cameraGeometry.dataset.bagfile)[0]
resultFile = bagtag + "-camchain.yaml"
chain = cr.CameraChainParameters(resultFile, createYaml=True)
camParams = cr.CameraParameters(resultFile, createYaml=True)
camParams.setRosTopic(self.__cameraGeometry.dataset.topic)
# Intrinsics
cameraModels = {
# Rolling shutter
acvb.DistortedPinholeRs: 'pinhole',
acvb.EquidistantPinholeRs: 'pinhole',
acvb.DistortedOmniRs: 'omni',
# Global shutter
acvb.DistortedPinhole: 'pinhole',
acvb.EquidistantPinhole: 'pinhole',
acvb.DistortedOmni: 'omni'
}
cameraModel = cameraModels[self.__cameraGeometry.model]
proj = self.__camera_dv.projectionDesignVariable().value()
camParams.setIntrinsics(cameraModel, proj.getParameters().flatten())
camParams.setResolution([proj.ru(), proj.rv()])
# Distortion
distortionModels = {
# Rolling shutter
acvb.DistortedPinholeRs: 'radtan',
acvb.EquidistantPinholeRs: 'equidistant',
acvb.DistortedOmniRs: 'radtan',
# Global shutter
acvb.DistortedPinhole: 'radtan',
acvb.EquidistantPinhole: 'equidistant',
acvb.DistortedOmni: 'radtan'
}
distortionModel = distortionModels[self.__cameraGeometry.model]
dist = self.__camera_dv.distortionDesignVariable().value()
camParams.setDistortion(distortionModel,
dist.getParameters().flatten())
# Shutter
shutter = self.__camera_dv.shutterDesignVariable().value()
camParams.setLineDelay(shutter.lineDelay())
chain.addCameraAtEnd(camParams)
chain.writeYaml()
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)