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