def test_values(self): values = Values() E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 values.insert(0, Point2(0, 0)) values.insert(1, Point3(0, 0, 0)) values.insert(2, Rot2()) values.insert(3, Pose2()) values.insert(4, Rot3()) values.insert(5, Pose3()) values.insert(6, Cal3_S2()) values.insert(7, Cal3DS2()) values.insert(8, Cal3Bundler()) values.insert(9, E) values.insert(10, imuBias_ConstantBias()) # Special cases for Vectors and Matrices # Note that gtsam's Eigen Vectors and Matrices requires double-precision # floating point numbers in column-major (Fortran style) storage order, # whereas by default, numpy.array is in row-major order and the type is # in whatever the number input type is, e.g. np.array([1,2,3]) # will have 'int' type. # # The wrapper will automatically fix the type and storage order for you, # but for performance reasons, it's recommended to specify the correct # type and storage order. # for vectors, the order is not important, but dtype still is vec = np.array([1., 2., 3.]) values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) # Test with dtype int and the default order='C' # This still works as the wrapper converts to the correct type and order for you # but is nornally not recommended! mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol)) self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol)) self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) self.assertTrue(values.atimuBias_ConstantBias( 10).equals(imuBias_ConstantBias(), tol)) # special cases for Vector and Matrix: actualVector = values.atVector(11) self.assertTrue(np.allclose(vec, actualVector, tol)) actualMatrix = values.atMatrix(12) self.assertTrue(np.allclose(mat, actualMatrix, tol)) actualMatrix2 = values.atMatrix(13) self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
def n2g(numpy_arr, obj): if obj == "Quaternion": x, y, z, w = numpy_arr return gtsam.Rot3.Quaternion(w, x, y, z) elif obj == "Euler": roll, pitch, yaw = numpy_arr return gtsam.Rot3.Ypr(yaw, pitch, roll) elif obj == "Point2": x, y = numpy_arr return gtsam.Point2(x, y) elif obj == "Pose2": x, y, yaw = numpy_arr return gtsam.Pose2(x, y, yaw) elif obj == "Point3": x, y, z = numpy_arr return gtsam.Point3(x, y, z) elif obj == "Pose3": x, y, z, roll, pitch, yaw = numpy_arr return gtsam.Pose3(gtsam.Rot3.Ypr(yaw, pitch, roll), gtsam.Point3(x, y, z)) elif obj == "imuBiasConstantBias": imu_bias = numpy_arr return gtsam.imuBias_ConstantBias(np.array(imu_bias[:3]), np.array(imu_bias[3:])) elif obj == "Vector": return np.array(numpy_arr) else: raise NotImplementedError("Not implemented from numpy to " + obj)
def __init__(self, twist=None, bias=None, dt=1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting plt.ion() # Setup loop as default scenario if twist is not None: (W, V) = twist else: # default = loop with forward velocity 2m/s, while pitching up # with angular velocity 30 degree/sec (negative in FLU) W = np.array([0, -math.radians(30), 0]) V = np.array([2, 0, 0]) self.scenario = gtsam.ConstantTwistScenario(W, V) self.dt = dt self.maxDim = 5 self.labels = list('xyz') self.colors = list('rgb') # Create runner self.g = 10 # simple gravity constant self.params = self.defaultParams(self.g) if bias is not None: self.actualBias = bias else: accBias = np.array([0, 0.1, 0]) gyroBias = np.array([0, 0, 0]) self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias) self.runner = gtsam.ScenarioRunner( self.scenario, self.params, self.dt, self.actualBias)
def __init__(self): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Choose one of these twists to change scenario: zero_twist = (np.zeros(3), np.zeros(3)) forward_twist = (np.zeros(3), self.velocity) loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) sick_twist = ( np.array([math.radians(30), -math.radians(30), 0]), self.velocity) accBias = np.array([-0.3, 0.1, 0.2]) gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.imuBias_ConstantBias(accBias, gyroBias) dt = 1e-2 super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
def test_loop_runner(self): # Forward velocity 2m/s # Pitch up with angular velocity 6 degree/sec (negative in FLU) v = 2 w = math.radians(6) W = np.array([0, -w, 0]) V = np.array([v, 0, 0]) scenario = gtsam.ConstantTwistScenario(W, V) dt = 0.1 params = gtsam.PreintegrationParams.MakeSharedU(self.g) bias = gtsam.imuBias_ConstantBias() runner = gtsam.ScenarioRunner(scenario, params, dt, bias) # Test specific force at time 0: a is pointing up t = 0.0 a = w * v np.testing.assert_almost_equal(np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
def Optimize(self, meas_time, imu_samples): """Perform optimization""" # IMU preintegration until measurement time predicted_nav_state = self.ImuUpdate(imu_samples) # Add estimates self.new_initial_ests.insert(self.poseKey, predicted_nav_state.pose()) self.new_initial_ests.insert(self.velKey, predicted_nav_state.velocity()) self.new_initial_ests.insert(self.biasKey, self.current_bias) # Add IMU bias factor bias_factor = gtsam.BetweenFactorConstantBias( self.biasKey-1, self.biasKey, gtsam.imuBias_ConstantBias(), self.bias_cov) self.new_factors.add(bias_factor) # Compute result result = self.Isam2Update() if result: self.current_time = meas_time self.current_global_pose = result.atPose3(self.poseKey) self.current_global_vel = result.atVector(self.velKey) self.current_bias = result.atimuBias_ConstantBias(self.biasKey)
if __name__ == '__main__': imu_measurements, _, _, true_poses = np.load('circle_gold.npy') ############################## Algorithm parameters ############################ import argparse params = argparse.Namespace() # Time step length. # In real world, it will be different at each measurement, # and you'll have to take dtᵢ from data. params.dt = 1e-2 # IMU bias params.accelerometer_bias = np.array([0, 0.1, 0]) params.gyroscope_bias = np.array([0, 0, 0]) params.IMU_bias = gtsam.imuBias_ConstantBias(params.accelerometer_bias, params.gyroscope_bias) # 'circle_gold.npy' simulates a loop with forward velocity 2 m/s, # while pitching up with angular velocity 30 degrees/sec. params.initial_velocity = np.array([2, 0, 0]) params.initial_angular_vel = np.array([0, -math.radians(30), 0]) # not used here params.initial_pose = gtsam.Pose3() params.initial_state = gtsam.NavState(params.initial_pose, params.initial_velocity) # IMU preintegration algorithm parameters # "U" means "Z axis points up"; "MakeSharedD" would mean "Z axis points along the gravity" preintegration_params = gtsam.PreintegrationParams.MakeSharedU( 10) # 10 is the gravity force # Realistic noise parameters
def IMU_example(): """Run iSAM 2 example with IMU factor.""" # Start with a camera on x-axis looking at origin radius = 30 up = gtsam.Point3(0, 0, 1) target = gtsam.Point3(0, 0, 0) position = gtsam.Point3(radius, 0, 0) camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) scenario = gtsam.ConstantTwistScenario( angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = gtsam.ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initialEstimate = gtsam.Values() # Add a prior on pose x0. This indirectly specifies where the origin is. # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasnoise) newgraph.push_back(biasprior) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity n_velocity = vector3(0, angular_velocity * radius, 0) velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) # Simulate poses and imu measurements, adding them to the factor graph for i in range(80): t = i * delta_t # simulation time if i == 0: # First time add two poses pose_1 = scenario.pose(delta_t) initialEstimate.insert(X(0), pose_0.compose(DELTA)) initialEstimate.insert(X(1), pose_1.compose(DELTA)) elif i >= 2: # Add more poses as necessary pose_i = scenario.pose(t) initialEstimate.insert(X(i), pose_i.compose(DELTA)) if i > 0: # Add Bias variables periodically if i % 5 == 0: biasKey += 1 factor = gtsam.BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) newgraph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame nRb = scenario.rotation(t).matrix() bRn = np.transpose(nRb) measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) measuredOmega = scenario.omega_b(t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor imufac = gtsam.ImuFactor( X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) # insert new velocity, which is wrong initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution isam.update(newgraph, initialEstimate) result = isam.calculateEstimate() ISAM2_plot(result) # reset newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear()
def __init__(self, params): """ Initialize variables """ self.imu_meas_predict = [] # IMU measurements for pose prediction self.imu_opt_meas = [] # IMU measurements for pose prediction between measurement updates self.imu_samples = [] # imu samples self.opt_meas = [] # optimizes measurements self.__opt_meas_buffer_time = params['opt_meas_buffer_time'] """ IMU Preintegration """ # IMU preintegration parameters self.imu_preint_params = gtsam.PreintegrationParams(np.asarray(params['g'])) self.imu_preint_params.setAccelerometerCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2)) self.imu_preint_params.setGyroscopeCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2)) self.imu_preint_params.setIntegrationCovariance(np.eye(3) * params['int_cov_sigma']**2) self.imu_preint_params.setUse2ndOrderCoriolis(params['setUse2ndOrderCoriolis']) self.imu_preint_params.setOmegaCoriolis(np.array(params['omega_coriolis'])) """ Initialize Parameters """ # ISAM2 initialization isam2_params = gtsam.ISAM2Params() isam2_params.setRelinearizeThreshold(params['relinearize_th']) isam2_params.setRelinearizeSkip(params['relinearize_skip']) isam2_params.setFactorization(params['factorization']) # self.isam2 = gtsam.ISAM2(isam2_params) self.isam2 = gtsam.ISAM2() self.new_factors = gtsam.NonlinearFactorGraph() self.new_initial_ests = gtsam.Values() self.min_imu_sample = 2 # minimum imu sample count needed for integration # ISAM2 keys self.poseKey = gtsam.symbol(ord('x'), 0) self.velKey = gtsam.symbol(ord('v'), 0) self.biasKey = gtsam.symbol(ord('b'), 0) """ Set initial variables """ # Initial state self.last_opt_time = self.current_time = 0 self.current_global_pose = numpy_pose_to_gtsam( params['init_pos'], params['init_ori']) self.current_global_vel = np.asarray( params['init_vel']) self.current_bias = gtsam.imuBias_ConstantBias( np.asarray(params['init_acc_bias']), np.asarray(params['init_gyr_bias'])) self.imu_accum = gtsam.PreintegratedImuMeasurements(self.imu_preint_params) # uncertainty of the initial state self.init_pos_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([ params['init_ori_cov'], params['init_ori_cov'], params['init_ori_cov'], params['init_pos_cov'], params['init_pos_cov'], params['init_pos_cov']])) self.init_vel_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([ params['init_vel_cov'], params['init_vel_cov'], params['init_vel_cov']])) self.init_bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([ params['init_acc_bias_cov'], params['init_acc_bias_cov'], params['init_acc_bias_cov'], params['init_gyr_bias_cov'], params['init_gyr_bias_cov'], params['init_gyr_bias_cov']])) # measurement noise self.dvl_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray( params['dvl_cov'])) self.bar_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray( params['bar_cov'])) self.bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.concatenate(( params['sigma_acc_bias_evol'], params['sigma_gyr_bias_evol']))) """ Set initial state """ # Initial factors prior_pose_factor = gtsam.PriorFactorPose3( self.poseKey, self.current_global_pose, self.init_pos_cov) self.new_factors.add(prior_pose_factor) prior_vel_factor = gtsam.PriorFactorVector( self.velKey, self.current_global_vel, self.init_vel_cov) self.new_factors.add(prior_vel_factor) prior_bias_factor = gtsam.PriorFactorConstantBias( self.biasKey, self.current_bias, self.init_bias_cov) self.new_factors.add(prior_bias_factor) # Initial estimates self.new_initial_ests.insert(self.poseKey, self.current_global_pose) self.new_initial_ests.insert(self.velKey, self.current_global_vel) self.new_initial_ests.insert(self.biasKey, self.current_bias)