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

Updated some usages that were missed

release/4.3a0
Richard Roberts 2014-11-23 15:24:55 -08:00
parent ce95583e68
commit 3ba9e8f405
8 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Pose2SLAMExample_g2o.cpp
View File

@ -64,7 +64,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph; NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8).finished()); noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl; std::cout << "Adding prior on pose 0 " << std::endl;

2
examples/Pose2SLAMExample_lago.cpp
View File

@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph; NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8).finished()); noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print(); graphWithPrior.print();

4
gtsam/geometry/tests/testRot3.cpp
View File

@ -499,7 +499,7 @@ TEST( Rot3, RQ)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, expmapStability ) { TEST( Rot3, expmapStability ) {
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7).finished(); Vector w = Vector3(78e-9, 5e-8, 97e-7);
double theta = w.norm(); double theta = w.norm();
double theta2 = theta*theta; double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w); Rot3 actualR = Rot3::Expmap(w);
@ -515,7 +515,7 @@ TEST( Rot3, expmapStability ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, logmapStability ) { TEST( Rot3, logmapStability ) {
Vector w = (Vector(3) << 1e-8, 0.0, 0.0).finished(); Vector w = Vector3(1e-8, 0.0, 0.0);
Rot3 R = Rot3::Expmap(w); Rot3 R = Rot3::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z(); // double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000); // std::cout.precision(5000);

2
gtsam/slam/lago.cpp
View File

@ -37,7 +37,7 @@ static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished()); noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished());
static const noiseModel::Diagonal::shared_ptr priorPose2Noise = static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8).finished()); noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
/* ************************************************************************* */ /* ************************************************************************* */
/** /**

4
gtsam/slam/tests/testLago.cpp
View File

@ -265,7 +265,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *g; NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4).finished()); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
VectorValues actualVV = lago::initializeOrientations(graphWithPrior); VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
@ -300,7 +300,7 @@ TEST( Lago, largeGraphNoisy ) {
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *g; NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4).finished()); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
Values actual = lago::initialize(graphWithPrior); Values actual = lago::initialize(graphWithPrior);

2
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -614,7 +614,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).finished()); Vector omega_earth_ECEF(Vector3(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

2
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -347,7 +347,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).finished()); Vector omega_earth_ECEF(Vector3(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

6
gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -31,7 +31,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).finished()); gtsam::Vector ECEF_omega_earth(Vector3(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,8 +54,8 @@ int main() {
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g(Vector3(0.0, 0.0, 9.81)); Vector world_g(Vector3(0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0).finished()); // NED system Vector world_rho(Vector3(0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5).finished()); gtsam::Vector ECEF_omega_earth(Vector3(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));