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 = []
# This code assumes that the order of the landmarks in the observations
# is invariant across all observations. At least for the chessboards it is true.
#####
# add all the landmarks once
landmarks = []
landmarks_expr = []
target = self.__cameraGeometry.ctarget.detector.target()
for idx in range(0, target.size()):
# design variable for landmark
landmark_w_dv = aopt.HomogeneousPointDv(
sm.toHomogeneous(target.point(idx)))
landmark_w_dv.setActive(
self.__config.estimateParameters['landmarks'])
landmarks.append(landmark_w_dv)
landmarks_expr.append(landmark_w_dv.toExpression())
problem.addDesignVariable(landmark_w_dv, LANDMARK_GROUP_ID)
#####
# activate design variables
self.__camera_dv.setActive(
self.__config.estimateParameters['intrinsics'],
self.__config.estimateParameters['distortion'],
self.__config.estimateParameters['shutter'])
#####
# 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
corner_ids = observation.getCornersIdx()
for index, point in enumerate(
observation.getCornersImageFrame()):
# keypoint time offset by line delay as expression type
keypoint_time = self.__camera_dv.keypointTime(
frame.time(), point)
# from target to world transformation.
T_w_t = self.__poseSpline_dv.transformationAtTime(
keypoint_time,
self.__config.timeOffsetConstantSparsityPattern,
self.__config.timeOffsetConstantSparsityPattern)
T_t_w = T_w_t.inverse()
# transform target point to camera frame
p_t = T_t_w * landmarks_expr[corner_ids[index]]
# create the keypoint
keypoint_index = frame.numKeypoints()
keypoint = acv.Keypoint2()
keypoint.setMeasurement(point)
inverseFeatureCovariance = self.__config.inverseFeatureCovariance
keypoint.setInverseMeasurementCovariance(
np.eye(len(point)) * inverseFeatureCovariance)
frame.addKeypoint(keypoint)
# create reprojection error
reprojection_error = self.__buildErrorTerm(
frame, keypoint_index, p_t, self.__camera_dv,
self.__poseSpline_dv)
self.__reprojection_errors.append(reprojection_error)
problem.addErrorTerm(reprojection_error)
return problem
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
def buildProblem(self,
splineOrder=6,
poseKnotsPerSecond=70,
biasKnotsPerSecond=70,
doPoseMotionError=False,
mrTranslationVariance=1e6,
mrRotationVariance=1e5,
doBiasMotionError=True,
blakeZisserCam=-1,
huberAccel=-1,
huberGyro=-1,
noTimeCalibration=False,
noChainExtrinsics=True,
maxIterations=20,
gyroNoiseScale=1.0,
accelNoiseScale=1.0,
timeOffsetPadding=0.02,
verbose=False):
print "\tSpline order: %d" % (splineOrder)
print "\tPose knots per second: %d" % (poseKnotsPerSecond)
print "\tDo pose motion regularization: %s" % (doPoseMotionError)
print "\t\txddot translation variance: %f" % (mrTranslationVariance)
print "\t\txddot rotation variance: %f" % (mrRotationVariance)
print "\tBias knots per second: %d" % (biasKnotsPerSecond)
print "\tDo bias motion regularization: %s" % (doBiasMotionError)
print "\tBlake-Zisserman on reprojection errors %s" % blakeZisserCam
print "\tAcceleration Huber width (sigma): %f" % (huberAccel)
print "\tGyroscope Huber width (sigma): %f" % (huberGyro)
print "\tDo time calibration: %s" % (not noTimeCalibration)
print "\tMax iterations: %d" % (maxIterations)
print "\tTime offset padding: %f" % (timeOffsetPadding)
############################################
## initialize camera chain
############################################
#estimate the timeshift for all cameras to the main imu
self.noTimeCalibration = noTimeCalibration
if not noTimeCalibration:
for cam in self.CameraChain.camList:
cam.findTimeshiftCameraImuPrior(self.ImuList[0], verbose)
#obtain orientation prior between main imu and camera chain (if no external input provided)
#and initial estimate for the direction of gravity
self.CameraChain.findOrientationPriorCameraChainToImu(self.ImuList[0])
estimatedGravity = self.CameraChain.getEstimatedGravity()
############################################
## init optimization problem
############################################
#initialize a pose spline using the camera poses in the camera chain
poseSpline = self.CameraChain.initializePoseSplineFromCameraChain(
splineOrder, poseKnotsPerSecond, timeOffsetPadding)
# Initialize bias splines for all IMUs
for imu in self.ImuList:
imu.initBiasSplines(poseSpline, splineOrder, biasKnotsPerSecond)
# Now I can build the problem
problem = inc.CalibrationOptimizationProblem()
# Initialize all design variables.
self.initDesignVariables(problem,
poseSpline,
noTimeCalibration,
noChainExtrinsics,
initialGravityEstimate=estimatedGravity)
############################################
## add error terms
############################################
#Add calibration target reprojection error terms for all camera in chain
self.CameraChain.addCameraChainErrorTerms(
problem,
self.poseDv,
blakeZissermanDf=blakeZisserCam,
timeOffsetPadding=timeOffsetPadding)
# Initialize IMU error terms.
for imu in self.ImuList:
imu.addAccelerometerErrorTerms(problem,
self.poseDv,
self.gravityExpression,
mSigma=huberAccel,
accelNoiseScale=accelNoiseScale)
imu.addGyroscopeErrorTerms(problem,
self.poseDv,
mSigma=huberGyro,
gyroNoiseScale=gyroNoiseScale,
g_w=self.gravityExpression)
# Add the bias motion terms.
if doBiasMotionError:
imu.addBiasMotionTerms(problem)
# Add the pose motion terms.
if doPoseMotionError:
self.addPoseMotionTerms(problem, mrTranslationVariance,
mrRotationVariance)
# Add a gravity prior
self.problem = problem
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 = []
# This code assumes that the order of the landmarks in the observations
# is invariant across all observations. At least for the chessboards it is true.
#####
# add all the landmarks once
landmarks = []
landmarks_expr = {}
keypoint_ids0 = self.__observations[0].getCornersIdx()
for idx, landmark in enumerate(self.__observations[0].getCornersTargetFrame()):
# design variable for landmark
landmark_w_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(landmark))
landmark_w_dv.setActive(self.__config.estimateParameters['landmarks']);
landmarks.append(landmark_w_dv)
landmarks_expr[keypoint_ids0[idx]] = landmark_w_dv.toExpression()
problem.addDesignVariable(landmark_w_dv, CALIBRATION_GROUP_ID)
#####
# activate design variables
self.__camera_dv.setActive(
self.__config.estimateParameters['intrinsics'],
self.__config.estimateParameters['distortion'],
self.__config.estimateParameters['shutter']
)
#####
# 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 frameid, observation in enumerate(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 index, point in enumerate(observation.getCornersImageFrame()):
# keypoint time offset by line delay as expression type
keypoint_time = self.__camera_dv.keypointTime(frame.time(), point)
# from target to world transformation.
T_w_t = self.__poseSpline_dv.transformationAtTime(
keypoint_time,
self.__config.timeOffsetConstantSparsityPattern,
self.__config.timeOffsetConstantSparsityPattern
)
T_t_w = T_w_t.inverse()
# we only have the the first image's design variables
# so any landmark that is not in that frame won't be in the problem
# thus we must skip those measurements that are of a keypoint that isn't visible
keypoint_ids = observation.getCornersIdx()
if not np.any(keypoint_ids[index]==keypoint_ids0):
sm.logWarn("landmark {0} in frame {1} not in first frame".format(keypoint_ids[index],frameid))
continue
# transform target point to camera frame
p_t = T_t_w * landmarks_expr[keypoint_ids[index]]
# create the keypoint
keypoint_index = frame.numKeypoints()
keypoint = acv.Keypoint2()
keypoint.setMeasurement(point)
inverseFeatureCovariance = self.__config.inverseFeatureCovariance;
keypoint.setInverseMeasurementCovariance(np.eye(len(point)) * inverseFeatureCovariance)
keypoint.setLandmarkId(keypoint_index)
frame.addKeypoint(keypoint)
# create reprojection error
reprojection_error = self.__buildErrorTerm(
frame,
keypoint_index,
p_t,
self.__camera_dv,
self.__poseSpline_dv
)
self.__reprojection_errors.append(reprojection_error)
problem.addErrorTerm(reprojection_error)
return problem
