def __init__(self, target, estimateLandmarks=False):
self.target = target
# Create design variables and expressions for all target points.
P_t_dv = []
P_t_ex = []
for i in range(0,self.target.size()):
p_t_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(self.target.point(i)));
p_t_dv.setActive(estimateLandmarks)
p_t_ex = p_t_dv.toExpression()
P_t_dv.append(p_t_dv)
P_t_ex.append(p_t_ex)
self.P_t_dv = P_t_dv
self.P_t_ex = P_t_ex
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 = []
# 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
