From de55dc0d660bdf8f5d698c82486cdfc2536e8fa4 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Tue, 22 Oct 2013 04:56:01 +0000 Subject: [PATCH] Fix Vector_() to Vec() in gtsam_unstable/slam --- gtsam_unstable/slam/AHRS.cpp | 6 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 6 +- .../slam/InertialNavFactor_GlobalVelocity.h | 6 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 4 +- gtsam_unstable/slam/Mechanization_bRn2.cpp | 4 +- .../slam/tests/testBetweenFactorEM.cpp | 32 ++-- .../testEquivInertialNavFactor_GlobalVel.cpp | 6 +- .../testInertialNavFactor_GlobalVelocity.cpp | 148 +++++++++--------- .../slam/tests/testInvDepthFactorVariant1.cpp | 6 +- .../slam/tests/testInvDepthFactorVariant2.cpp | 6 +- .../slam/tests/testInvDepthFactorVariant3.cpp | 6 +- .../slam/tests/testMultiProjectionFactor.cpp | 4 +- .../tests/testRelativeElevationFactor.cpp | 10 +- .../slam/tests/testSmartRangeFactor.cpp | 8 +- .../testTransformBtwRobotsUnaryFactorEM.cpp | 26 +-- .../timeInertialNavFactor_GlobalVelocity.cpp | 12 +- 16 files changed, 145 insertions(+), 145 deletions(-) diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 32ead0c15..15f8ba9ce 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -60,7 +60,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, sigmas_v_a_ = esqrt(T * Pa_.diagonal()); // 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 !!! } @@ -220,8 +220,8 @@ std::pair AHRS::aidGeneral( Matrix b_g = skewSymmetric(increment* f_previous); Matrix H = collect(3, &b_g, &I3, &Z3); // 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(Vector_(3, 0.01, 0.0001, 0.01)); +// Matrix R = diag((Vec(3) << 1.0, 0.2, 1.0)); // good for L_twice + Matrix R = diag((Vec(3) << 0.01, 0.0001, 0.01)); // update the Kalman filter KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 121295d50..54a557e03 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -613,7 +613,7 @@ public: 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; // Calculating g @@ -627,7 +627,7 @@ public: 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 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 @@ -636,7 +636,7 @@ public: double rho_E = -Vn/(Rm + height); double rho_N = Ve/(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){ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index f67c47e2a..c16194254 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -346,7 +346,7 @@ public: 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; // Calculating g @@ -360,7 +360,7 @@ public: 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 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 @@ -369,7 +369,7 @@ public: double rho_E = -Vn/(Rm + height); double rho_N = Ve/(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){ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 0d75d5e6b..96d556767 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -99,7 +99,7 @@ public: << std::endl; 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 @@ -218,7 +218,7 @@ public: << std::endl; 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 diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 91cc17f6d..682a5ad84 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -33,14 +33,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, if(g_e == 0) { if (flat) // 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 // acceleration measured is the opposite of gravity (10.13) b_g = -meanF; } else { if (flat) // 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 // normalize b_g and attribute remainder to biases b_g = - g_e * meanF/meanF.norm(); diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 6d3dc7127..7fb16e4ab 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -59,8 +59,8 @@ TEST( BetweenFactorEM, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); 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_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 5, 5, 1.0))); double prior_outlier = 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_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -103,8 +103,8 @@ TEST( BetweenFactorEM, EvaluateError) 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_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + 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: "< h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier, prior_inlier, prior_outlier); 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 h(key1, key2, rel_pose_msr, model_inlier ); 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_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -182,7 +182,7 @@ TEST (BetweenFactorEM, jacobian ) { // compare to standard between factor BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); 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)); std::vector H_actual_stnd_unwh(2); (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 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_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 4.9821, 4.614, 1.8387))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.4021, 0.286, 0.428))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 4.9821, 4.614, 1.8387))); gtsam::Values values; values.insert(key1, p1); @@ -244,8 +244,8 @@ TEST( BetweenFactorEM, CaseStudy) Vector actual_err_unw = f.unwhitenedError(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_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_outlier = (Vec(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); if (debug){ cout << "p_inlier_outler: "< 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); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // First test: zero angular motion, some acceleration - Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); - Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); + Vector measurement_acc((Vec(3) <<0.1,0.2,0.3-9.81)); + Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0)); InertialNavFactor_GlobalVelocity 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); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + Vector measurement_acc((Vec(3) <<0.0,0.0,0.0-9.81)); + Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity 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); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity 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 ) { -// 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)); -// 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], // q[2], 0.0, -q[0], // -q[1], q[0],0.0); @@ -285,15 +285,15 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { gtsam::Key BiasKey1(31); double measurement_dt(0.01); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14)); + Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vec(3) << 3.14, 3.14/2, -3.14)); InertialNavFactor_GlobalVelocity 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 Bias1(31); - Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc((Vec(3) << 0.1, 0.2, 0.4)); + Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -400,13 +400,13 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) gtsam::Key Vel2(22); gtsam::Key Bias1(31); - Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc((Vec(3) << 0.1, 0.2, 0.4)); + Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -429,9 +429,9 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -440,9 +440,9 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) // 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); - 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 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); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -480,9 +480,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) // 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); - 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 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); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -519,9 +519,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) // 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); - 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 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); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); 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 - 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); - 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 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); Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); 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 ); imuBias::ConstantBias Bias1; @@ -596,9 +596,9 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { gtsam::Key BiasKey1(31); double measurement_dt(0.01); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); 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 - 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); - 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 factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index bdb36bc0a..0f092d803 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -66,9 +66,9 @@ TEST( InvDepthFactorVariant1, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract(Vector_(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(landmarkKey, expected.retract(Vector_(6, -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vec(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((Vec(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index b784d87f4..ffbbcc76b 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant2, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract(Vector_(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(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vec(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((Vec(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 5f5e8c35f..e901e6361 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant3, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract(Vector_(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(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vec(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((Vec(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 0d1976a24..ff6ae4457 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -147,7 +147,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // 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 // CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -170,7 +170,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // 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 // CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index be269be61..2c843b0dd 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -49,7 +49,7 @@ TEST( testRelativeElevationFactor, level_positive ) { double measured = 0.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); 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( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -64,7 +64,7 @@ TEST( testRelativeElevationFactor, level_negative ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); 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( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -94,7 +94,7 @@ TEST( testRelativeElevationFactor, rotated_positive ) { double measured = 0.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); 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( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -109,7 +109,7 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); 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( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -124,7 +124,7 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); 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( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); Matrix expH2 = numericalDerivative22( diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 7708caf66..c416a7d52 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -74,20 +74,20 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Whenever there are two ranges or less, error should be zero 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); 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); vector H(3); Vector actual3 = f.unwhitenedError(values); 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 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, sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index c404b2d33..e4913e6b2 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -60,8 +60,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); 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_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); double prior_outlier = 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_msr = rel_pose_ideal; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); double prior_outlier = 0.01; 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_msr = rel_pose_ideal; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -184,8 +184,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Optimize) gtsam::Pose2 currA_Tmsr_currB3 = 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_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); double prior_outlier = 0.01; 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_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); double prior_outlier = 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_msr = rel_pose_ideal.compose(noise); // -// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); -// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0))); // // gtsam::Values values; // values.insert(keyA, p1); @@ -306,7 +306,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) // // compare to standard between factor // BetweenFactor h(keyA, keyB, rel_pose_msr, model_inlier ); // 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)); // std::vector H_actual_stnd_unwh(2); // (void)h.unwhitenedError(values, H_actual_stnd_unwh); diff --git a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp index 3fcdceefe..5132df658 100644 --- a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp @@ -32,7 +32,7 @@ gtsam::Rot3 world_R_ECEF( 0.85173, -0.52399, 0, 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); /* ************************************************************************* */ @@ -54,16 +54,16 @@ int main() { gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);