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

Fix Matrix_(...) to Mat() <<... in gtsam_unstable

release/4.3a0
Jing Dong 2013-11-13 05:51:03 +00:00
parent 55f65339aa
commit 0b3aac7048
9 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -134,7 +134,7 @@ public:
double H34 = 0;
double H35 = cos_phi/rho;
*H2 = J2 * gtsam::Matrix_(3,5,
*H2 = J2 * (Mat(3, 5) <<
H11, H12, H13, H14, H15,
H21, H22, H23, H24, H25,
H31, H32, H33, H34, H35);
@ -143,7 +143,7 @@ public:
double H16 = -cos_phi*cos_theta/rho2;
double H26 = -cos_phi*sin_theta/rho2;
double H36 = -sin_phi/rho2;
*H3 = J2 * gtsam::Matrix_(3,1,
*H3 = J2 * (Mat(3, 1) <<
H16,
H26,
H36);

4
gtsam_unstable/slam/AHRS.cpp
View File

@ -72,7 +72,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
double sp = sin(mech0_.bRn().inverse().pitch());
double cy = cos(0.0);
double sy = sin(0.0);
Matrix Omega_T = Matrix_(3, 3, cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0);
Matrix Omega_T = (Mat(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0);
// Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb
Vector b_g = mech0_.b_g(g_e);
@ -82,7 +82,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
double g23 = g2 * g2 + g3 * g3;
double g123 = g1 * g1 + g23;
double f = 1 / (std::sqrt(g23) * g123);
Matrix H_g = Matrix_(3, 3,
Matrix H_g = (Mat(3, 3) <<
0.0, g3 / g23, -(g2 / g23), // roll
std::sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch
0.0, 0.0, 0.0); // we don't know anything on yaw

4
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -557,12 +557,12 @@ public:
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
Matrix ENU_to_NED = Matrix_(3, 3,
Matrix ENU_to_NED = (Mat(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
Matrix NED_to_ENU = Matrix_(3, 3,
Matrix NED_to_ENU = (Mat(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);

4
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -481,12 +481,12 @@ public:
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
Matrix ENU_to_NED = Matrix_(3, 3,
Matrix ENU_to_NED = (Mat(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
Matrix NED_to_ENU = Matrix_(3, 3,
Matrix NED_to_ENU = (Mat(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);

4
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -290,12 +290,12 @@ public:
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
Matrix ENU_to_NED = Matrix_(3, 3,
Matrix ENU_to_NED = (Mat(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
Matrix NED_to_ENU = Matrix_(3, 3,
Matrix NED_to_ENU = (Mat(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);

4
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -13,8 +13,8 @@ namespace gtsam {
/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list<Vector>& U,
const std::list<Vector>& F, const double g_e, bool flat) {
Matrix Umat = Matrix_(3,U.size(), concatVectors(U));
Matrix Fmat = Matrix_(3,F.size(), concatVectors(F));
Matrix Umat = (Mat(3, U.size()) << concatVectors(U));
Matrix Fmat = (Mat(3, F.size()) << concatVectors(F));
return initialize(Umat, Fmat, g_e, flat);
}

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

@ -16,22 +16,22 @@ using namespace std;
using namespace gtsam;
// stationary interval of gyro U and acc F
Matrix stationaryU = trans(Matrix_(3,3,-0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003));
Matrix stationaryF = trans(Matrix_(3,3,0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021));
Matrix stationaryU = trans((Mat(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003));
Matrix stationaryF = trans((Mat(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021));
double g_e = 9.7963; // Atlanta
/* ************************************************************************* */
TEST (AHRS, cov) {
// samples stored by row
Matrix A = Matrix_(4, 3,
Matrix A = (Mat(4, 3) <<
1.0, 2.0, 3.0,
5.0, 7.0, 0.0,
9.0, 4.0, 7.0,
6.0, 3.0, 2.0);
Matrix actual = cov(trans(A));
Matrix expected = Matrix_(3, 3,
Matrix expected = (Mat(3, 3) <<
10.9167, 2.3333, 5.0000,
2.3333, 4.6667, -2.6667,
5.0000, -2.6667, 8.6667);
@ -43,7 +43,7 @@ TEST (AHRS, cov) {
TEST (AHRS, covU) {
Matrix actual = cov(10000*stationaryU);
Matrix expected = Matrix_(3, 3,
Matrix expected = (Mat(3, 3) <<
33.3333333, -1.66666667, 53.3333333,
-1.66666667, 0.333333333, -5.16666667,
53.3333333, -5.16666667, 110.333333);
@ -55,7 +55,7 @@ TEST (AHRS, covU) {
TEST (AHRS, covF) {
Matrix actual = cov(100*stationaryF);
Matrix expected = Matrix_(3, 3,
Matrix expected = (Mat(3, 3) <<
63.3808333, -0.432166667, -48.1706667,
-0.432166667, 8.31053333, -16.6792667,
-48.1706667, -16.6792667, 71.4297333);

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

@ -193,8 +193,8 @@ TEST( MultiProjectionFactor, create ){
// factor.evaluateError(pose, point, H1Actual, H2Actual);
//
// // The expected Jacobians
// Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
// Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.);
// Matrix H1Expected = (Mat(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
// Matrix H2Expected = (Mat(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
//
// // Verify the Jacobians are correct
// CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
@ -219,8 +219,8 @@ TEST( MultiProjectionFactor, create ){
// factor.evaluateError(pose, point, H1Actual, H2Actual);
//
// // The expected Jacobians
// Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
// Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376);
// Matrix H1Expected = (Mat(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
// Matrix H2Expected = (Mat(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
//
// // Verify the Jacobians are correct
// CHECK(assert_equal(H1Expected, H1Actual, 1e-3));

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

@ -88,8 +88,8 @@ TEST( SmartRangeFactor, unwhitenedError ) {
// Check keys and Jacobian
Vector actual4 = f.unwhitenedError(values, H); // with H now !
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.0)/2,-sqrt(2.0)/2,0.0), H.back()));
CHECK(assert_equal((Mat(1, 3) << 0.0,-1.0,0.0), H.front()));
CHECK(assert_equal((Mat(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0), H.back()));
// Test clone
NonlinearFactor::shared_ptr clone = f.clone();
@ -109,7 +109,7 @@ TEST( SmartRangeFactor, optimization ) {
initial.insert(2, pose2);
initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement
Vector actual5 = f.unwhitenedError(initial);
EXPECT(assert_equal(Vector_(1,sqrt(25.0+16.0)-sqrt(50.0)), actual5));
EXPECT(assert_equal((Vec(1) << sqrt(25.0+16.0)-sqrt(50.0)), actual5));
// Create Factor graph
NonlinearFactorGraph graph;