def initDesignVariables(self, problem, poseSpline, noTimeCalibration, \
estimateGravityLength=False, initialGravityEstimate=np.array([0.0, 9.81, 0.0])):
# Initialize the system pose spline
self.poseDv = asp.BSplinePoseDesignVariable(poseSpline)
addSplineDesignVariables(problem, self.poseDv)
if self.ImuList:
# Add the calibration target orientation design variable. (expressed as gravity vector in target frame)
if estimateGravityLength:
self.gravityDv = aopt.EuclideanPointDv(initialGravityEstimate)
else:
self.gravityDv = aopt.EuclideanDirection(
initialGravityEstimate)
self.gravityExpression = self.gravityDv.toExpression()
self.gravityDv.setActive(True)
problem.addDesignVariable(self.gravityDv, HELPER_GROUP_ID)
# Add targets, fix the first target
self.targets[0].addDesignVariables(problem, fixed=True)
for target in self.targets[1:]:
target.addDesignVariables(problem)
# Add all DVs for all IMUs
for imu in self.ImuList:
imu.addDesignVariables(problem)
# Add all DVs for the camera chain
self.CameraChain.addDesignVariables(problem, noTimeCalibration)
for lidar in self.LiDARList:
lidar.addDesignVariables(problem, noTimeCalibration)
def __initPoseDesignVariables(self, problem):
"""Get the design variable representation of the pose spline and add them to the problem"""
# get the design variable
self.__poseSpline_dv = asp.BSplinePoseDesignVariable(self.__poseSpline)
# activate all contained dv and add to problem
for i in range(0, self.__poseSpline_dv.numDesignVariables()):
dv = self.__poseSpline_dv.designVariable(i)
dv.setActive(self.__config.estimateParameters['pose'])
problem.addDesignVariable(dv, CALIBRATION_GROUP_ID)
def initDesignVariables(self, problem, poseSpline, noTimeCalibration, estimateGravityLength=False):
# Initialize the system pose spline (always attached to imu0)
self.poseDv = asp.BSplinePoseDesignVariable( poseSpline )
addSplineDesignVariables(problem, self.poseDv)
# Add the calibration target orientation design variable. (expressed as gravity vector in target frame)
if estimateGravityLength:
self.gravityDv = aopt.EuclideanPointDv( np.array([0.0,-9.81,0.0]) )
else:
self.gravityDv = aopt.EuclideanDirection( np.array([0.0,-9.81,0.0]) )
self.gravityExpression = self.gravityDv.toExpression()
self.gravityDv.setActive( True )
problem.addDesignVariable(self.gravityDv, HELPER_GROUP_ID)
#Add all DVs for all IMUs
for imu in self.ImuList:
imu.addDesignVariables( problem )
#Add all DVs for the camera chain
self.CameraChain.addDesignVariables( problem, noTimeCalibration )