def addPoseDesignVariable(problem, T0=sm.Transformation()):
q_Dv = aopt.RotationQuaternionDv(T0.q())
q_Dv.setActive(True)
problem.addDesignVariable(q_Dv)
t_Dv = aopt.EuclideanPointDv(T0.t())
t_Dv.setActive(True)
problem.addDesignVariable(t_Dv)
return aopt.TransformationBasicDv(q_Dv.toExpression(), t_Dv.toExpression())
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