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

Fix Vector_() to Vec() in gtsam_unstable/slam

release/4.3a0
Jing Dong 2013-10-22 04:56:01 +00:00
parent 87f9a2ab03
commit de55dc0d66
16 changed files with 145 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam_unstable/slam/AHRS.cpp
View File

@ -60,7 +60,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
sigmas_v_a_ = esqrt(T * Pa_.diagonal()); sigmas_v_a_ = esqrt(T * Pa_.diagonal());
// gravity in nav frame // gravity in nav frame
n_g_ = Vector_(3, 0.0, 0.0, g_e); n_g_ = (Vec(3) << 0.0, 0.0, g_e);
n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!! n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!!
} }
@ -220,8 +220,8 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
Matrix b_g = skewSymmetric(increment* f_previous); Matrix b_g = skewSymmetric(increment* f_previous);
Matrix H = collect(3, &b_g, &I3, &Z3); Matrix H = collect(3, &b_g, &I3, &Z3);
// Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
// Matrix R = diag(Vector_(3, 1.0, 0.2, 1.0)); // good for L_twice // Matrix R = diag((Vec(3) << 1.0, 0.2, 1.0)); // good for L_twice
Matrix R = diag(Vector_(3, 0.01, 0.0001, 0.01)); Matrix R = diag((Vec(3) << 0.01, 0.0001, 0.01));
// update the Kalman filter // update the Kalman filter
KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R);

6
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -613,7 +613,7 @@ public:
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); Vector omega_earth_ECEF((Vec(3) << 0.0, 0.0, 7.292115e-5));
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g // Calculating g
@ -627,7 +627,7 @@ public:
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
double g_calc( g0/( pow(1 + height/Ro, 2) ) ); double g_calc( g0/( pow(1 + height/Ro, 2) ) );
g_ENU = Vector_(3, 0.0, 0.0, -g_calc); g_ENU = (Vec(3) << 0.0, 0.0, -g_calc);
// Calculate rho // Calculate rho
@ -636,7 +636,7 @@ public:
double rho_E = -Vn/(Rm + height); double rho_E = -Vn/(Rm + height);
double rho_N = Ve/(Rp + height); double rho_N = Ve/(Rp + height);
double rho_U = Ve*tan(lat_new)/(Rp + height); double rho_U = Ve*tan(lat_new)/(Rp + height);
rho_ENU = Vector_(3, rho_E, rho_N, rho_U); rho_ENU = (Vec(3) << rho_E, rho_N, rho_U);
} }
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){

6
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -346,7 +346,7 @@ public:
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); Vector omega_earth_ECEF((Vec(3) << 0.0, 0.0, 7.292115e-5));
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g // Calculating g
@ -360,7 +360,7 @@ public:
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
double g_calc( g0/( pow(1 + height/Ro, 2) ) ); double g_calc( g0/( pow(1 + height/Ro, 2) ) );
g_ENU = Vector_(3, 0.0, 0.0, -g_calc); g_ENU = (Vec(3) << 0.0, 0.0, -g_calc);
// Calculate rho // Calculate rho
@ -369,7 +369,7 @@ public:
double rho_E = -Vn/(Rm + height); double rho_E = -Vn/(Rm + height);
double rho_N = Ve/(Rp + height); double rho_N = Ve/(Rp + height);
double rho_U = Ve*tan(lat_new)/(Rp + height); double rho_U = Ve*tan(lat_new)/(Rp + height);
rho_ENU = Vector_(3, rho_E, rho_N, rho_U); rho_ENU = (Vec(3) << rho_E, rho_N, rho_U);
} }
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){

4
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -99,7 +99,7 @@ public:
<< std::endl; << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx(); return gtsam::ones(2) * 2.0 * K_->fx();
} }
return gtsam::Vector_(1, 0.0); return (Vec(1) << 0.0);
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
@ -218,7 +218,7 @@ public:
<< std::endl; << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx(); return gtsam::ones(2) * 2.0 * K_->fx();
} }
return gtsam::Vector_(1, 0.0); return (Vec(1) << 0.0);
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives

4
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -33,14 +33,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
if(g_e == 0) { if(g_e == 0) {
if (flat) if (flat)
// acceleration measured is along the z-axis. // acceleration measured is along the z-axis.
b_g = Vector_(3, 0.0, 0.0, norm_2(meanF)); b_g = (Vec(3) << 0.0, 0.0, norm_2(meanF));
else else
// acceleration measured is the opposite of gravity (10.13) // acceleration measured is the opposite of gravity (10.13)
b_g = -meanF; b_g = -meanF;
} else { } else {
if (flat) if (flat)
// gravity is downward along the z-axis since we are flat on the ground // gravity is downward along the z-axis since we are flat on the ground
b_g = Vector_(3,0.0,0.0,g_e); b_g = (Vec(3) << 0.0,0.0,g_e);
else else
// normalize b_g and attribute remainder to biases // normalize b_g and attribute remainder to biases
b_g = - g_e * meanF/meanF.norm(); b_g = - g_e * meanF/meanF.norm();

32
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -59,8 +59,8 @@ TEST( BetweenFactorEM, ConstructorAndEquals)
gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 5, 5, 1.0)));
double prior_outlier = 0.5; double prior_outlier = 0.5;
double prior_inlier = 0.5; double prior_inlier = 0.5;
@ -88,8 +88,8 @@ TEST( BetweenFactorEM, EvaluateError)
gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0)));
gtsam::Values values; gtsam::Values values;
values.insert(key1, p1); values.insert(key1, p1);
@ -103,8 +103,8 @@ TEST( BetweenFactorEM, EvaluateError)
Vector actual_err_wh = f.whitenedError(values); Vector actual_err_wh = f.whitenedError(values);
Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
Vector actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); Vector actual_err_wh_outlier = (Vec(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
// cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier.norm()<<endl; // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier.norm()<<endl;
// cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl; // cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;
@ -121,8 +121,8 @@ TEST( BetweenFactorEM, EvaluateError)
actual_err_wh = g.whitenedError(values); actual_err_wh = g.whitenedError(values);
actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); actual_err_wh_outlier = (Vec(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
// in case of outlier, outlier-mode whitented error should be dominant // in case of outlier, outlier-mode whitented error should be dominant
// CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm()); // CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm());
@ -136,7 +136,7 @@ TEST( BetweenFactorEM, EvaluateError)
BetweenFactorEM<gtsam::Pose2> h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier, BetweenFactorEM<gtsam::Pose2> h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier,
prior_inlier, prior_outlier); prior_inlier, prior_outlier);
actual_err_wh = h_EM.whitenedError(values); actual_err_wh = h_EM.whitenedError(values);
actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier ); BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
Vector actual_err_wh_stnd = h.whitenedError(values); Vector actual_err_wh_stnd = h.whitenedError(values);
@ -160,8 +160,8 @@ TEST (BetweenFactorEM, jacobian ) {
gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0)));
gtsam::Values values; gtsam::Values values;
values.insert(key1, p1); values.insert(key1, p1);
@ -182,7 +182,7 @@ TEST (BetweenFactorEM, jacobian ) {
// compare to standard between factor // compare to standard between factor
BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier ); BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
Vector actual_err_wh_stnd = h.whitenedError(values); Vector actual_err_wh_stnd = h.whitenedError(values);
Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
std::vector<gtsam::Matrix> H_actual_stnd_unwh(2); std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
(void)h.unwhitenedError(values, H_actual_stnd_unwh); (void)h.unwhitenedError(values, H_actual_stnd_unwh);
@ -223,8 +223,8 @@ TEST( BetweenFactorEM, CaseStudy)
gtsam::Pose2 p2(-0.0491752554, -0.289649075, -0.328993962); gtsam::Pose2 p2(-0.0491752554, -0.289649075, -0.328993962);
gtsam::Pose2 rel_pose_msr(0.0316191379, 0.0247539161, 0.004102182); gtsam::Pose2 rel_pose_msr(0.0316191379, 0.0247539161, 0.004102182);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.4021, 0.286, 0.428))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.4021, 0.286, 0.428)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 4.9821, 4.614, 1.8387))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 4.9821, 4.614, 1.8387)));
gtsam::Values values; gtsam::Values values;
values.insert(key1, p1); values.insert(key1, p1);
@ -244,8 +244,8 @@ TEST( BetweenFactorEM, CaseStudy)
Vector actual_err_unw = f.unwhitenedError(values); Vector actual_err_unw = f.unwhitenedError(values);
Vector actual_err_wh = f.whitenedError(values); Vector actual_err_wh = f.whitenedError(values);
Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
Vector actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); Vector actual_err_wh_outlier = (Vec(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
if (debug){ if (debug){
cout << "p_inlier_outler: "<<p_inlier_outler[0]<<", "<<p_inlier_outler[1]<<endl; cout << "p_inlier_outler: "<<p_inlier_outler[0]<<", "<<p_inlier_outler[1]<<endl;

6
gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -38,9 +38,9 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
Key biasKey1(31); Key biasKey1(31);
// IMU accumulation variables // IMU accumulation variables
Vector delta_pos_in_t0 = Vector_(3, 0.0, 0.0, 0.0); Vector delta_pos_in_t0 = (Vec(3) << 0.0, 0.0, 0.0);
Vector delta_vel_in_t0 = Vector_(3, 0.0, 0.0, 0.0); Vector delta_vel_in_t0 = (Vec(3) << 0.0, 0.0, 0.0);
Vector delta_angles = Vector_(3, 0.0, 0.0, 0.0); Vector delta_angles = (Vec(3) << 0.0, 0.0, 0.0);
double delta_t = 0.0; double delta_t = 0.0;
Matrix EquivCov_Overall = zeros(15,15); Matrix EquivCov_Overall = zeros(15,15);
Matrix Jacobian_wrt_t0_Overall = eye(15); Matrix Jacobian_wrt_t0_Overall = eye(15);

148
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -34,7 +34,7 @@ gtsam::Rot3 world_R_ECEF(
0.85173, -0.52399, 0, 0.85173, -0.52399, 0,
0.41733, 0.67835, -0.60471); 0.41733, 0.67835, -0.60471);
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
/* ************************************************************************* */ /* ************************************************************************* */
@ -55,13 +55,13 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor)
gtsam::Key Vel2(22); gtsam::Key Vel2(22);
gtsam::Key Bias1(31); gtsam::Key Bias1(31);
Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); Vector measurement_acc((Vec(3) << 0.1,0.2,0.4));
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03));
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -78,13 +78,13 @@ TEST( InertialNavFactor_GlobalVelocity, Equals)
gtsam::Key Vel2(22); gtsam::Key Vel2(22);
gtsam::Key Bias1(31); gtsam::Key Bias1(31);
Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); Vector measurement_acc((Vec(3) << 0.1,0.2,0.4));
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03));
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -104,17 +104,17 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); Vector measurement_acc((Vec(3) <<0.1,0.2,0.3-9.81));
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
@ -141,17 +141,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); Vector measurement_acc((Vec(3) <<0.1,0.2,0.3-9.81));
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
@ -177,16 +177,16 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration // Second test: zero angular motion, some acceleration
Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81)); Vector measurement_acc((Vec(3) <<0.0,0.0,0.0-9.81));
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
@ -212,16 +212,16 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration - generated in matlab // Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
@ -254,9 +254,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
//} //}
// //
//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
// LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005)); // LieVector angles((Vec(3) << 3.001, -1.0004, 2.0005));
// Rot3 R1(Rot3().RzRyRx(angles)); // Rot3 R1(Rot3().RzRyRx(angles));
// LieVector q(Vector_(3, 5.8, -2.2, 4.105)); // LieVector q((Vec(3) << 5.8, -2.2, 4.105));
// Rot3 qx(0.0, -q[2], q[1], // Rot3 qx(0.0, -q[2], q[1],
// q[2], 0.0, -q[0], // q[2], 0.0, -q[0],
// -q[1], q[0],0.0); // -q[1], q[0],0.0);
@ -285,15 +285,15 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.01); double measurement_dt(0.01);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14)); Vector measurement_gyro((Vec(3) << 3.14, 3.14/2, -3.14));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
@ -374,13 +374,13 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
gtsam::Key Vel2(22); gtsam::Key Vel2(22);
gtsam::Key Bias1(31); gtsam::Key Bias1(31);
Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4)); Vector measurement_acc((Vec(3) << 0.1, 0.2, 0.4));
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03));
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -400,13 +400,13 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
gtsam::Key Vel2(22); gtsam::Key Vel2(22);
gtsam::Key Bias1(31); gtsam::Key Bias1(31);
Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4)); Vector measurement_acc((Vec(3) << 0.1, 0.2, 0.4));
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03));
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -429,9 +429,9 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -440,9 +440,9 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); // Measured in ENU orientation Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = Vector_(3, 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vec(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
@ -469,9 +469,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -480,9 +480,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); // Measured in ENU orientation Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = Vector_(3, 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vec(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
@ -508,9 +508,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -519,9 +519,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
// Second test: zero angular motion, some acceleration // Second test: zero angular motion, some acceleration
Vector measurement_gyro(Vector_(3, 0.2, 0.1, -0.3)); // Measured in ENU orientation Vector measurement_gyro((Vec(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = Vector_(3, 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vec(3) << 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
@ -547,9 +547,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -558,9 +558,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
// Second test: zero angular motion, some acceleration - generated in matlab // Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_gyro(Vector_(3, 0.2, 0.1, -0.3)); // Measured in ENU orientation Vector measurement_gyro((Vec(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vec(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
@ -575,7 +575,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * (Vec(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g);
LieVector Vel2 = Vel1.compose( dv ); LieVector Vel2 = Vel1.compose( dv );
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
@ -596,9 +596,9 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.01); double measurement_dt(0.01);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -606,9 +606,9 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
Vector measurement_gyro(Vector_(3, 3.14/2, 3.14, +3.14)); // Measured in ENU orientation Vector measurement_gyro((Vec(3) << 3.14/2, 3.14, +3.14)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vec(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);

6
gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
View File

@ -66,9 +66,9 @@ TEST( InvDepthFactorVariant1, optimize) {
// Create a values with slightly incorrect initial conditions // Create a values with slightly incorrect initial conditions
Values values; Values values;
values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey1, pose1.retract((Vec(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); values.insert(poseKey2, pose2.retract((Vec(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract(Vector_(6, -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); values.insert(landmarkKey, expected.retract((Vec(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position // Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params; LevenbergMarquardtParams params;

6
gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp
View File

@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant2, optimize) {
// Create a values with slightly incorrect initial conditions // Create a values with slightly incorrect initial conditions
Values values; Values values;
values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey1, pose1.retract((Vec(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); values.insert(poseKey2, pose2.retract((Vec(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); values.insert(landmarkKey, expected.retract((Vec(3) << +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position // Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params; LevenbergMarquardtParams params;

6
gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp
View File

@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant3, optimize) {
// Create a values with slightly incorrect initial conditions // Create a values with slightly incorrect initial conditions
Values values; Values values;
values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey1, pose1.retract((Vec(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); values.insert(poseKey2, pose2.retract((Vec(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); values.insert(landmarkKey, expected.retract((Vec(3) << +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position // Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params; LevenbergMarquardtParams params;

4
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
View File

@ -147,7 +147,7 @@ TEST( MultiProjectionFactor, create ){
// Vector actualError(factor.evaluateError(pose, point)); // Vector actualError(factor.evaluateError(pose, point));
// //
// // The expected error is (-3.0, 0.0) pixels / UnitCovariance // // The expected error is (-3.0, 0.0) pixels / UnitCovariance
// Vector expectedError = Vector_(2, -3.0, 0.0); // Vector expectedError = (Vec(2) << -3.0, 0.0);
// //
// // Verify we get the expected error // // Verify we get the expected error
// CHECK(assert_equal(expectedError, actualError, 1e-9)); // CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -170,7 +170,7 @@ TEST( MultiProjectionFactor, create ){
// Vector actualError(factor.evaluateError(pose, point)); // Vector actualError(factor.evaluateError(pose, point));
// //
// // The expected error is (-3.0, 0.0) pixels / UnitCovariance // // The expected error is (-3.0, 0.0) pixels / UnitCovariance
// Vector expectedError = Vector_(2, -3.0, 0.0); // Vector expectedError = (Vec(2) << -3.0, 0.0);
// //
// // Verify we get the expected error // // Verify we get the expected error
// CHECK(assert_equal(expectedError, actualError, 1e-9)); // CHECK(assert_equal(expectedError, actualError, 1e-9));

10
gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
View File

@ -49,7 +49,7 @@ TEST( testRelativeElevationFactor, level_positive ) {
double measured = 0.0; double measured = 0.0;
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); EXPECT(assert_equal((Vec(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
@ -64,7 +64,7 @@ TEST( testRelativeElevationFactor, level_negative ) {
double measured = -1.0; double measured = -1.0;
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); EXPECT(assert_equal((Vec(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
@ -94,7 +94,7 @@ TEST( testRelativeElevationFactor, rotated_positive ) {
double measured = 0.0; double measured = 0.0;
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); EXPECT(assert_equal((Vec(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
@ -109,7 +109,7 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) {
double measured = -1.0; double measured = -1.0;
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); EXPECT(assert_equal((Vec(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
@ -124,7 +124,7 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) {
double measured = -1.0; double measured = -1.0;
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); EXPECT(assert_equal((Vec(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(

8
gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
View File

@ -74,20 +74,20 @@ TEST( SmartRangeFactor, unwhitenedError ) {
// Whenever there are two ranges or less, error should be zero // Whenever there are two ranges or less, error should be zero
Vector actual1 = f.unwhitenedError(values); Vector actual1 = f.unwhitenedError(values);
EXPECT(assert_equal(Vector_(1,0.0), actual1)); EXPECT(assert_equal((Vec(1) << 0.0), actual1));
f.addRange(2, r2); f.addRange(2, r2);
Vector actual2 = f.unwhitenedError(values); Vector actual2 = f.unwhitenedError(values);
EXPECT(assert_equal(Vector_(1,0.0), actual2)); EXPECT(assert_equal((Vec(1) << 0.0), actual2));
f.addRange(3, r3); f.addRange(3, r3);
vector<Matrix> H(3); vector<Matrix> H(3);
Vector actual3 = f.unwhitenedError(values); Vector actual3 = f.unwhitenedError(values);
EXPECT_LONGS_EQUAL(3, f.keys().size()); EXPECT_LONGS_EQUAL(3, f.keys().size());
EXPECT(assert_equal(Vector_(1,0.0), actual3)); EXPECT(assert_equal((Vec(1) << 0.0), actual3));
// Check keys and Jacobian // Check keys and Jacobian
Vector actual4 = f.unwhitenedError(values, H); // with H now ! Vector actual4 = f.unwhitenedError(values, H); // with H now !
EXPECT(assert_equal(Vector_(1,0.0), actual4)); EXPECT(assert_equal((Vec(1) << 0.0), actual4));
CHECK(assert_equal(Matrix_(1,3, 0.0,-1.0,0.0), H.front())); CHECK(assert_equal(Matrix_(1,3, 0.0,-1.0,0.0), H.front()));
CHECK(assert_equal(Matrix_(1,3, sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); CHECK(assert_equal(Matrix_(1,3, sqrt(2)/2,-sqrt(2)/2,0.0), H.back()));

26
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
View File

@ -60,8 +60,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals)
gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0)));
double prior_outlier = 0.5; double prior_outlier = 0.5;
double prior_inlier = 0.5; double prior_inlier = 0.5;
@ -97,8 +97,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError)
gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal; gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0)));
double prior_outlier = 0.01; double prior_outlier = 0.01;
double prior_inlier = 0.99; double prior_inlier = 0.99;
@ -136,8 +136,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2)
gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal; gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0)));
double prior_outlier = 0.01; double prior_outlier = 0.01;
double prior_inlier = 0.99; double prior_inlier = 0.99;
@ -184,8 +184,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Optimize)
gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1;
gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1;
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0)));
double prior_outlier = 0.01; double prior_outlier = 0.01;
double prior_inlier = 0.99; double prior_inlier = 0.99;
@ -245,8 +245,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0)));
double prior_outlier = 0.5; double prior_outlier = 0.5;
double prior_inlier = 0.5; double prior_inlier = 0.5;
@ -284,8 +284,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
// gtsam::Pose2 rel_pose_ideal = p1.between(p2); // gtsam::Pose2 rel_pose_ideal = p1.between(p2);
// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); // gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
// //
// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); // SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05)));
// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); // SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0)));
// //
// gtsam::Values values; // gtsam::Values values;
// values.insert(keyA, p1); // values.insert(keyA, p1);
@ -306,7 +306,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
// // compare to standard between factor // // compare to standard between factor
// BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier ); // BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier );
// Vector actual_err_wh_stnd = h.whitenedError(values); // Vector actual_err_wh_stnd = h.whitenedError(values);
// Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); // Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
// std::vector<gtsam::Matrix> H_actual_stnd_unwh(2); // std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
// (void)h.unwhitenedError(values, H_actual_stnd_unwh); // (void)h.unwhitenedError(values, H_actual_stnd_unwh);

12
gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -32,7 +32,7 @@ gtsam::Rot3 world_R_ECEF(
0.85173, -0.52399, 0, 0.85173, -0.52399, 0,
0.41733, 0.67835, -0.60471); 0.41733, 0.67835, -0.60471);
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
/* ************************************************************************* */ /* ************************************************************************* */
@ -54,16 +54,16 @@ int main() {
gtsam::Key BiasKey1(31); gtsam::Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration - generated in matlab // Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);