12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in fix/matlab_wrapper (pull request #242) 

Fix/matlab_wrapper
release/4.3a0
Chris Beall 2016-04-10 20:29:09 -04:00
parent f967a54c70 69c182d5a4
commit 5d32a321cc
4 changed files with 91 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam.h
View File

@ -2489,10 +2489,30 @@ class NavState {
gtsam::Pose3 pose() const;
};
#include <gtsam/navigation/PreintegratedRotation.h>
virtual class PreintegratedRotationParams {
PreintegratedRotationParams();
void setGyroscopeCovariance(Matrix cov);
void setOmegaCoriolis(Vector omega);
void setBodyPSensor(const gtsam::Pose3& pose);
Matrix getGyroscopeCovariance() const;
// TODO(frank): allow optional
// boost::optional<Vector3> getOmegaCoriolis() const;
// boost::optional<Pose3> getBodyPSensor() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
class PreintegrationParams {
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity);
// TODO(frank): add setters/getters or make this MATLAB wrapper feature
void setAccelerometerCovariance(Matrix cov);
void setIntegrationCovariance(Matrix cov);
void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const;
};
#include <gtsam/navigation/PreintegrationBase.h>

56
gtsam/navigation/PreintegratedRotation.h
View File

@ -26,6 +26,38 @@
namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct PreintegratedRotationParams {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
virtual void print(const std::string& s) const;
virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); }
void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); }
const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
boost::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};
/**
* PreintegratedRotation is the base class for all PreintegratedMeasurements
* classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
@ -33,29 +65,7 @@ namespace gtsam {
*/
class PreintegratedRotation {
public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
Params() : gyroscopeCovariance(I_3x3) {}
virtual void print(const std::string& s) const;
virtual bool equals(const Params& other, double tol=1e-9) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};
typedef PreintegratedRotationParams Params;
protected:
/// Parameters

12
gtsam/navigation/PreintegrationParams.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct PreintegrationParams: PreintegratedRotation::Params {
struct PreintegrationParams: PreintegratedRotationParams {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params {
void print(const std::string& s) const;
bool equals(const PreintegratedRotation::Params& other, double tol) const;
void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; }
const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
const Matrix3& getIntegrationCovariance() const { return integrationCovariance; }
bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; }
protected:
/// Default constructor for serialization only: uninitialized!
PreintegrationParams() {}
@ -60,7 +68,7 @@ protected:
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
boost::serialization::base_object<PreintegratedRotationParams>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);

46
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -1,3 +1,7 @@
% Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks
% author: Chris Beall
% date: July 2014
clear all;
clf;
@ -24,13 +28,19 @@ if(write_video)
open(videoObj);
end
% IMU parameters
%% IMU parameters
IMU_metadata.AccelerometerSigma = 1e-2;
IMU_metadata.GyroscopeSigma = 1e-2;
IMU_metadata.AccelerometerBiasSigma = 1e-6;
IMU_metadata.GyroscopeBiasSigma = 1e-6;
IMU_metadata.IntegrationSigma = 1e-1;
n_gravity = [0;0;-9.8];
IMU_params = PreintegrationParams(n_gravity);
IMU_params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3));
IMU_params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3));
IMU_params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3));
transformKey = 1000;
calibrationKey = 2000;
@ -52,7 +62,7 @@ K = Cal3_S2(20,1280,960);
% initialize K incorrectly
K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py());
isamParams = gtsam.ISAM2Params;
isamParams = ISAM2Params;
isamParams.setFactorization('QR');
isam = ISAM2(isamParams);
@ -63,7 +73,6 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
g = [0;0;-9.8];
w_coriolis = [0;0;0];
%% generate trajectory and landmarks
@ -99,7 +108,9 @@ zlabel('z');
title('Estimated vs. actual IMU-cam transform');
axis equal;
%% Main loop
for i=1:size(trajectory)-1
%% Preliminaries
xKey = symbol('x',i);
pose = trajectory.atPose3(xKey); % GT pose
pose_t = pose.translation(); % GT pose-translation
@ -134,6 +145,7 @@ for i=1:size(trajectory)-1
gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]);
fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov));
%% First-time initialization
if i==1
% camera transform
if use_camera_transform_noise
@ -153,12 +165,10 @@ for i=1:size(trajectory)-1
result = initial;
end
% priors on first two poses
%% priors on first two poses
if i < 3
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end
%% the 'normal' case
@ -181,12 +191,14 @@ for i=1:size(trajectory)-1
zKey = measurementKeys.at(zz);
lKey = symbol('l',symbolIndex(zKey));
fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ...
fg.add(ProjectionFactorPPPCCal3_S2(measurements.atPoint2(zKey), ...
z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
% only add landmark to values if doesn't exist yet
if ~result.exists(lKey)
noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
p = landmarks.atPoint3(lKey);
n = normrnd(0,landmark_noise,3,1);
noisy_landmark = Point3(p.x()+n(1),p.y()+n(2),p.z()+n(3));
initial.insert(lKey, noisy_landmark);
% and add a prior since its position is known
@ -195,14 +207,12 @@ for i=1:size(trajectory)-1
end
end % end landmark observations
%% IMU
deltaT = 1;
logmap = Pose3.Logmap(step);
omega = logmap(1:3);
velocity = logmap(4:6);
% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
@ -211,11 +221,9 @@ for i=1:size(trajectory)-1
% [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
% currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias);
accMeas = acc_omega(1:3)-g;
accMeas = acc_omega(1:3)-n_gravity;
omegaMeas = acc_omega(4:6);
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
@ -223,13 +231,13 @@ for i=1:size(trajectory)-1
fg.add(ImuFactor( ...
xKey_prev, currentVelKey-1, ...
xKey, currentVelKey, ...
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
currentBiasKey, currentSummarizedMeasurement));
% Bias evolution as given in the IMU metadata
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b)));
% ISAM update
%% ISAM update
isam.update(fg, initial);
result = isam.calculateEstimate();
@ -295,10 +303,8 @@ for i=1:size(trajectory)-1
end
% print out final camera transform
%% print out final camera transform and write video
result.at(transformKey);
if(write_video)
close(videoObj);
end