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

fix Vector_() in gtsam_unstable and tests

release/4.3a0
jing 2014-01-23 02:03:12 -05:00
parent 90786c0203
commit 8641816b21
17 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/base/tests/testFixedVector.cpp
View File

@ -28,7 +28,7 @@ static const double tol = 1e-9;
/* ************************************************************************* */
TEST( testFixedVector, conversions ) {
double data1[] = {1.0, 2.0, 3.0};
Vector v1 = Vector_(3, data1);
Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]);
TestVector3 fv1(v1), fv2(data1);
Vector actFv2(fv2);

2
gtsam_unstable/geometry/BearingS2.cpp
View File

@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const {
/* ************************************************************************* */
Vector BearingS2::localCoordinates(const BearingS2& x) const {
return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0),
return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0),
elevation_.localCoordinates(x.elevation_)(0));
}

2
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
// extend line by random dist and angle to get BC
double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
double tABC = randomAngle().theta();
Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC));
Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC));
// extend from B to find C
double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);

2
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -128,7 +128,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
// calculate angle to change by
Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta());
Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0)));
Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0)));
// create a segment to use for intersection checking
// find the closest intersection

2
gtsam_unstable/geometry/tests/testPose3Upright.cpp
View File

@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) {
EXPECT(assert_equal(x1, x1.retract(zero(4)), tol));
EXPECT(assert_equal(x2, x2.retract(zero(4)), tol));
Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
EXPECT(assert_equal(x2, x1.retract(delta12), tol));
EXPECT(assert_equal(x1, x2.retract(delta21), tol));

2
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -266,7 +266,7 @@ namespace gtsam {
}
}
return Vector_(2, p_inlier, p_outlier);
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */

6
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -537,7 +537,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((Vector(3) << 0.0, 0.0, 7.292115e-5));
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g
@ -551,7 +551,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 = (Vector(3) << 0.0, 0.0, -g_calc);
// Calculate rho
@ -560,7 +560,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 = (Vector(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){

2
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -96,7 +96,7 @@ public:
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
return (gtsam::Vector(1) << 0.0);
}
/** return the measurement */

2
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

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

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

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

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -35,7 +35,7 @@ public:
}
Vector b_g(double g_e) {
Vector n_g = Vector_(3, 0.0, 0.0, g_e);
Vector n_g = (Vector_(3) << 0.0, 0.0, g_e);
return (bRn_ * n_g).vector();
}

2
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
assert(model->dim() == 1);
this->fillH();
}

2
gtsam_unstable/slam/RelativeElevationFactor.cpp
View File

@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
*H2 = zeros(1, 3);
(*H2)(0, 2) = -1.0;
}
return Vector_(1, hx - measured_);
return (Vector(1) << hx - measured_);
}
/* ************************************************************************* */

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -276,7 +276,7 @@ namespace gtsam {
}
}
return Vector_(2, p_inlier, p_outlier);
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */

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

@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) {
Mechanization_bRn2 mech;
KalmanFilter::State state;
// boost::tie(mech,state) = ahrs.initialize(g_e);
// Vector u = Vector_(3,0.05,0.0,0.0);
// Vector u = (Vector(3) << 0.05,0.0,0.0);
// double dt = 2;
// Rot3 expected;
// Mechanization_bRn2 mech2 = mech.integrate(u,dt);

2
tests/testGaussianJunctionTreeB.cpp
View File

@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) {
// Create a simple graph
NonlinearFactorGraph fg;
fg.add(PriorFactor<Pose2>(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)));
fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0))));
fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0))));
Values init;
init.insert(X(0), Pose2());

2
tests/testSimulated2DOriented.cpp
View File

@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior )
TEST( simulated2DOriented, constructor )
{
Pose2 measurement(0.2, 0.3, 0.1);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.));
simulated2DOriented::Odometry factor(measurement, model, X(1), X(2));
simulated2DOriented::Values config;