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

Static methods should be uppercase.

release/4.3a0
Frank Dellaert 2016-01-26 23:09:58 -08:00
parent 0ad3763ecf
commit 4d93a33f61
43 changed files with 184 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -6,3 +6,4 @@
/examples/Data/pose3example-rewritten.txt
*.txt.user
*.txt.user.6d59f0c
/python-build/

6
examples/CreateSFMExampleData.cpp
View File

@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
SfM_data data;
// Create two cameras
Rot3 aRb = Rot3::yaw(M_PI_2);
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 identity, aPb(aRb, aTb);
data.cameras.push_back(SfM_Camera(pose1, K));
@ -66,7 +66,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
void create5PointExample1() {
// Create two cameras poses
Rot3 aRb = Rot3::yaw(M_PI_2);
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
@ -85,7 +85,7 @@ void create5PointExample1() {
void create5PointExample2() {
// Create two cameras poses
Rot3 aRb = Rot3::yaw(M_PI_2);
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(10, 0, 0);
Pose3 pose1, pose2(aRb, aTb);

19
gtsam/geometry/Rot3.h
View File

@ -146,13 +146,13 @@ namespace gtsam {
}
/// Positive yaw is to right (as in aircraft heading). See ypr
static Rot3 yaw (double t) { return Rz(t); }
static Rot3 Yaw (double t) { return Rz(t); }
/// Positive pitch is up (increasing aircraft altitude).See ypr
static Rot3 pitch(double t) { return Ry(t); }
static Rot3 Pitch(double t) { return Ry(t); }
//// Positive roll is to right (increasing yaw in aircraft).
static Rot3 roll (double t) { return Rx(t); }
static Rot3 Roll (double t) { return Rx(t); }
/**
* Returns rotation nRb from body to nav frame.
@ -163,7 +163,7 @@ namespace gtsam {
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf.
* Assumes vehicle coordinate frame X forward, Y right, Z down.
*/
static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion coefficients */
static Rot3 quaternion(double w, double x, double y, double z) {
@ -419,13 +419,13 @@ namespace gtsam {
/**
* Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
* @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
*/
Vector3 ypr() const;
/**
* Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
* @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
*/
Vector3 rpy() const;
@ -488,6 +488,13 @@ namespace gtsam {
static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); }
/// @}
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
static Rot3 yaw (double t) { return Yaw(t); }
static Rot3 pitch(double t) { return Pitch(t); }
static Rot3 roll (double t) { return Roll(t); }
static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);}
#endif
private:
/** Serialization function */

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

@ -135,7 +135,7 @@ TEST( CalibratedCamera, Dproject_point_pose)
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject_point_pose2)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const CalibratedCamera camera(pose1);
Matrix Dpose, Dpoint;
camera.project(point1, Dpose, Dpoint);
@ -165,7 +165,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity)
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject_point_pose2_infinity)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const CalibratedCamera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(pointAtInfinity, Dpose, Dpoint);

10
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -20,7 +20,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
//*************************************************************************
// Create two cameras and corresponding essential matrix E
Rot3 c1Rc2 = Rot3::yaw(M_PI_2);
Rot3 c1Rc2 = Rot3::Yaw(M_PI_2);
Point3 c1Tc2(0.1, 0, 0);
EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
@ -98,8 +98,8 @@ Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
}
TEST (EssentialMatrix, transform_to) {
// test with a more complicated EssentialMatrix
Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4)
* Rot3::roll(M_PI / 6.0);
Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4)
* Rot3::Roll(M_PI / 6.0);
Point3 aTb2(19.2, 3.7, 5.9);
EssentialMatrix E(aRb2, Unit3(aTb2));
//EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0)));
@ -159,7 +159,7 @@ TEST (EssentialMatrix, FromPose3_a) {
//*************************************************************************
TEST (EssentialMatrix, FromPose3_b) {
Matrix actualH;
Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
Point3 c1Tc2(0.4, 0.5, 0.6);
EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
@ -181,7 +181,7 @@ TEST (EssentialMatrix, streaming) {
//*************************************************************************
TEST (EssentialMatrix, epipoles) {
// Create an E
Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
Point3 c1Tc2(0.4, 0.5, 0.6);
EssentialMatrix E(c1Rc2, Unit3(c1Tc2));

2
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -59,7 +59,7 @@ OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) {
}
TEST (OrientedPlane3, transform) {
gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0),
gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0),
gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 plane(-1, 0, 0, 5);
OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);

2
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -242,7 +242,7 @@ TEST( PinholeCamera, Dproject2)
// Add a test with more arbitrary rotation
TEST( PinholeCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);

2
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2)
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);

16
gtsam/geometry/tests/testPose3.cpp
View File

@ -556,12 +556,12 @@ TEST( Pose3, between )
/* ************************************************************************* */
// some shared test values - pulled from equivalent test in Pose2
Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4);
Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI/4.0, 0.0, 0.0), l2);
Pose3 x1, x2(Rot3::Ypr(0.0, 0.0, 0.0), l2), x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2);
Pose3
xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
xl3(Rot3::ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
/* ************************************************************************* */
double range_proxy(const Pose3& pose, const Point3& point) {
@ -654,9 +654,9 @@ TEST( Pose3, unicycle )
{
// velocity in X should be X in inertial frame, rather than global frame
Vector x_step = delta(6,3,1.0);
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
}
/* ************************************************************************* */

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

@ -501,17 +501,17 @@ TEST( Rot3, yaw_pitch_roll )
double t = 0.1;
// yaw is around z axis
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
CHECK(assert_equal(Rot3::Rz(t),Rot3::Yaw(t)));
// pitch is around y axis
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
CHECK(assert_equal(Rot3::Ry(t),Rot3::Pitch(t)));
// roll is around x axis
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
CHECK(assert_equal(Rot3::Rx(t),Rot3::Roll(t)));
// Check compound rotation
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
Rot3 expected = Rot3::Yaw(0.1) * Rot3::Pitch(0.2) * Rot3::Roll(0.3);
CHECK(assert_equal(expected,Rot3::Ypr(0.1,0.2,0.3)));
CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr()));
}
@ -531,14 +531,14 @@ TEST( Rot3, RQ)
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try using ypr call, asserting that Rot3::Ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::Ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::Ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::Yaw (0.1).ypr()));
CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::Pitch(0.1).ypr()));
CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::Roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished();

10
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -40,7 +40,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
@ -150,7 +150,7 @@ TEST( triangulation, fourPoses) {
EXPECT(assert_equal(landmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
SimpleCamera camera3(pose3, *sharedCal);
Point2 z3 = camera3.project(landmark);
@ -167,7 +167,7 @@ TEST( triangulation, fourPoses) {
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way
Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
SimpleCamera camera4(pose4, *sharedCal);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
@ -214,7 +214,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
EXPECT(assert_equal(landmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera3(pose3, K3);
Point2 z3 = camera3.project(landmark);
@ -232,7 +232,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way
Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera4(pose4, K4);

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

@ -70,7 +70,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) {
}
TEST(Unit3, rotate) {
Rot3 R = Rot3::yaw(0.5);
Rot3 R = Rot3::Yaw(0.5);
Unit3 p(1, 0, 0);
Unit3 expected = Unit3(R.column(1));
Unit3 actual = R * p;
@ -95,7 +95,7 @@ static Unit3 unrotate_(const Rot3& R, const Unit3& p) {
}
TEST(Unit3, unrotate) {
Rot3 R = Rot3::yaw(-M_PI / 4.0);
Rot3 R = Rot3::Yaw(-M_PI / 4.0);
Unit3 p(1, 0, 0);
Unit3 expected = Unit3(1, 1, 0);
Unit3 actual = R.unrotate(p);

4
gtsam/navigation/GPSFactor.cpp
View File

@ -58,10 +58,10 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
// Estimate Rotation
double yaw = atan2(nV.y(), nV.x());
Rot3 nRy = Rot3::yaw(yaw); // yaw frame
Rot3 nRy = Rot3::Yaw(yaw); // yaw frame
Point3 yV = nRy.inverse() * nV; // velocity in yaw frame
double pitch = -atan2(yV.z(), yV.x()), roll = 0;
Rot3 nRb = Rot3::ypr(yaw, pitch, roll);
Rot3 nRb = Rot3::Ypr(yaw, pitch, roll);
// Construct initial pose
Pose3 nTb(nRb, nT); // nTb

2
gtsam/navigation/MagFactor.h
View File

@ -60,7 +60,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) {
Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none);
Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
if (HR) {
// assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2);

2
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -423,7 +423,7 @@ TEST (AHRSFactor, predictTest) {
// Predict
Rot3 x;
Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0);
Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0);
Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -293,10 +293,10 @@ TEST(CombinedImuFactor, PredictRotation) {
gravity, omegaCoriolis);
// Predict
Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Vector3 v(0, 0, 0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, x2, tol));
}

2
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -89,7 +89,7 @@ TEST(GPSData, init) {
// Check values values
EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4));
EXPECT( assert_equal(Rot3::ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5));
EXPECT( assert_equal(Rot3::Ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5));
Point3 expectedT(2.38461, -2.31289, -0.156011);
EXPECT(assert_equal(expectedT, T.translation(), 1e-5));
}

10
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -813,7 +813,7 @@ TEST(ImuFactor, PredictRotation) {
Vector3 v2;
ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown,
kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity;
expectedVelocity << 0, 0, 0;
EXPECT(assert_equal(expectedPose, x2));
@ -891,7 +891,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
double dt = 0.001;
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));
ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true);
@ -907,7 +907,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity,
omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
Pose3 expectedPose(Rot3::Ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
Vector3 expectedVelocity(0, 0, 0);
@ -942,7 +942,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
// table exerts an equal and opposite force w.r.t gravity
Vector3 measuredAcc(0, 0, -9.81);
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3());
Matrix3 accCov = 1e-7 * I_3x3;
Matrix3 gyroCov = 1e-8 * I_3x3;
@ -1025,7 +1025,7 @@ TEST(ImuFactor, serialization) {
using namespace gtsam::serializationTestHelpers;
Vector3 n_gravity(0, 0, -9.81);
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3());
Matrix3 accCov = 1e-7 * I_3x3;
Matrix3 gyroCov = 1e-8 * I_3x3;
Matrix3 integrationCov = 1e-9 * I_3x3;

2
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -40,7 +40,7 @@ Point3 nM(22653.29982, -1956.83010, 44202.47862);
// Let's assume scale factor,
double scale = 255.0 / 50000.0;
// ...ground truth orientation,
Rot3 nRb = Rot3::yaw(-0.1);
Rot3 nRb = Rot3::Yaw(-0.1);
Rot2 theta = nRb.yaw();
// ...and bias
Point3 bias(10, -10, 50);

4
gtsam/slam/dataset.cpp
View File

@ -510,7 +510,7 @@ GraphAndValues load3D(const string& filename) {
Key id;
double x, y, z, roll, pitch, yaw;
ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::ypr(yaw,pitch,roll);
Rot3 R = Rot3::Ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
}
@ -537,7 +537,7 @@ GraphAndValues load3D(const string& filename) {
Key id1, id2;
double x, y, z, roll, pitch, yaw;
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::ypr(yaw,pitch,roll);
Rot3 R = Rot3::Ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
Matrix m = eye(6);
for (int i = 0; i < 6; i++)

4
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -34,7 +34,7 @@ Point3 landmark4(10, 0.5, 1.2);
Point3 landmark5(10, -0.5, 1.2);
// First camera pose, looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
// Second camera 1 meter to the right of first camera
Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
// Third camera 1 meter above the first camera
@ -123,7 +123,7 @@ Camera cam3(pose_above, sharedBundlerK);
template<class CAMERA>
CAMERA perturbCameraPose(const CAMERA& camera) {
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 cameraPose = camera.pose();
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);

2
gtsam/slam/tests/testDataset.cpp
View File

@ -452,7 +452,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
SfM_data readData;
readBAL(filenameToRead, readData);
Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera

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

@ -47,7 +47,7 @@ TEST (OrientedPlane3Factor, lm_translation_error) {
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
Vector sigmas(6);
sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
@ -94,7 +94,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));

2
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1;
// Pose3 examples
const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);

2
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -27,7 +27,7 @@ const gtsam::Key poseKey = 1;
// Pose3 examples
const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3);
const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3);
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);

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

@ -47,7 +47,7 @@ template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
const PinholeCamera<CALIBRATION>& camera) {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 cameraPose = camera.pose();
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
@ -61,7 +61,7 @@ PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, perturbCameraPose) {
using namespace vanilla;
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 perturbed_level_pose = level_pose.compose(noise_pose);
Camera actualCamera(perturbed_level_pose, K2);

56
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -163,7 +163,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
Values values;
values.insert(x1, cam1.pose());
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
values.insert(x2, pose_right.compose(noise_pose));
@ -196,7 +196,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
@ -205,7 +205,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
SimpleCamera cam3(cameraPose3, *K);
// create arbitrary body_Pose_sensor (transforms from sensor to body)
Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
// These are the poses we want to estimate, from camera measurements
Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
@ -263,7 +263,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
Values values;
values.insert(x1, bodyPose1);
values.insert(x2, bodyPose2);
@ -317,8 +317,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
groundTruth.insert(x3, cam3.pose());
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -539,8 +539,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -606,8 +606,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -667,8 +667,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -792,7 +792,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -844,7 +844,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Values values;
values.insert(x1, level_pose);
@ -908,8 +908,8 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -995,7 +995,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -1063,8 +1063,8 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -1108,7 +1108,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
smartFactor1->add(measurements_cam1, views);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Values values;
values.insert(x1, cam1.pose());
@ -1148,7 +1148,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
boost::shared_ptr<GaussianFactor> factor = smartFactorInstance->linearize(
values);
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues;
rotValues.insert(x1, poseDrift.compose(level_pose));
@ -1161,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
Values tranValues;
@ -1203,7 +1203,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> factor = smartFactor->linearize(values);
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues;
rotValues.insert(x1, poseDrift.compose(level_pose));
@ -1216,7 +1216,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
Values tranValues;
@ -1278,8 +1278,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());
@ -1357,8 +1357,8 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, cam1.pose());

2
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -37,7 +37,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
SimpleCamera camera1(pose1, *sharedCal);

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -100,7 +100,7 @@ PoseRTV PoseRTV::flyingDynamics(
double pitch2 = r2.pitch();
double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force
double loss_lift = lift*fabs(sin(pitch2));
Rot3 yaw_correction_bn = Rot3::yaw(yaw2);
Rot3 yaw_correction_bn = Rot3::Yaw(yaw2);
Point3 forward(forward_accel, 0.0, 0.0);
Vector Acc_n =
yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame

12
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
pose2(Point3(1.0, 1.0, 0.0), Rot3::Ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::Ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::Ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Unit::Create(6);
@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
pose2(Point3(1.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);

8
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -193,7 +193,7 @@ TEST( testPoseRTV, transformed_from_1 ) {
Point3 T(1.0, 2.0, 3.0);
Velocity3 V(2.0, 3.0, 4.0);
PoseRTV start(R, T, V);
Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
Matrix actDTrans, actDGlobal;
PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
@ -212,7 +212,7 @@ TEST( testPoseRTV, transformed_from_2 ) {
Point3 T(1.0, 2.0, 3.0);
Velocity3 V(2.0, 3.0, 4.0);
PoseRTV start(R, T, V);
Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
Matrix actDTrans, actDGlobal;
PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
@ -229,14 +229,14 @@ TEST( testPoseRTV, transformed_from_2 ) {
TEST(testPoseRTV, RRTMbn) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3))));
EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3))));
}
/* ************************************************************************* */
TEST(testPoseRTV, RRTMnb) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3))));
EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3))));
}
/* ************************************************************************* */

2
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -16,7 +16,7 @@ const double tol=1e-5;
const double h = 0.01;
//const double deg2rad = M_PI/180.0;
//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
//Pose3 g1(Rot3::Ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished());
Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished());

2
gtsam_unstable/geometry/Pose3Upright.cpp
View File

@ -64,7 +64,7 @@ Rot2 Pose3Upright::rotation2() const {
/* ************************************************************************* */
Rot3 Pose3Upright::rotation() const {
return Rot3::yaw(theta());
return Rot3::Yaw(theta());
}
/* ************************************************************************* */

4
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -17,7 +17,7 @@ using namespace std;
using namespace gtsam;
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
/* ************************************************************************* */
@ -142,7 +142,7 @@ TEST(InvDepthFactor, backproject2)
// backwards facing camera
Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished());
double inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::Ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth);
Vector5 actual_vec;

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

@ -58,9 +58,9 @@ TEST( testPose3Upright, conversions ) {
EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol));
EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol));
EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol));
EXPECT(assert_equal(Rot3::yaw(0.1), pose.rotation(), tol));
EXPECT(assert_equal(Rot3::Yaw(0.1), pose.rotation(), tol));
EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol));
EXPECT(assert_equal(Pose3(Rot3::yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol));
EXPECT(assert_equal(Pose3(Rot3::Yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol));
}
/* ************************************************************************* */

36
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -56,14 +56,14 @@ TEST(Similarity3, Getters) {
//******************************************************************************
TEST(Similarity3, Getters2) {
Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7);
EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation()));
Similarity3 test(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), test.rotation()));
EXPECT(assert_equal(Point3(4, 5, 6), test.translation()));
EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9);
}
TEST(Similarity3, AdjointMap) {
Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Matrix7 result;
result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000,
6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000,
@ -76,7 +76,7 @@ TEST(Similarity3, AdjointMap) {
}
TEST(Similarity3, inverse) {
Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Matrix3 Re;
Re << -0.2248, 0.9024, -0.3676,
-0.3502, -0.4269, -0.8337,
@ -87,8 +87,8 @@ TEST(Similarity3, inverse) {
}
TEST(Similarity3, multiplication) {
Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11);
Similarity3 test1(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Similarity3 test2(Rot3::Ypr(1,2,3).inverse(), Point3(8,9,10), 11);
Matrix3 re;
re << 0.0688, 0.9863, -0.1496,
-0.5665, -0.0848, -0.8197,
@ -117,14 +117,14 @@ TEST(Similarity3, Manifold) {
v3 << 0, 0, 0, 1, 2, 3, 0;
EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
// Similarity3 other = Similarity3(Rot3::Ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
Vector vlocal = sim.localCoordinates(other);
EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1);
Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0),Point3(4,5,6),1);
Rot3 R = Rot3::Rodrigues(0.3,0,0);
Vector vlocal2 = sim.localCoordinates(other2);
@ -167,7 +167,7 @@ TEST(Similarity3, manifold_first_order)
}
TEST(Similarity3, Optimization) {
Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
Symbol key('x',1);
PriorFactor<Similarity3> factor(key, prior, model);
@ -187,10 +187,10 @@ TEST(Similarity3, Optimization) {
TEST(Similarity3, Optimization2) {
Similarity3 prior = Similarity3();
Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
Similarity3 m3 = Similarity3(Rot3::Ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
Similarity3 loop = Similarity3(1.42);
//prior.print("Goal Transform");
@ -220,10 +220,10 @@ TEST(Similarity3, Optimization2) {
Values initial;
initial.insert<Similarity3>(X(1), Similarity3());
initial.insert<Similarity3>(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
initial.insert<Similarity3>(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
initial.insert<Similarity3>(X(2), Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
initial.insert<Similarity3>(X(3), Similarity3(Rot3::Ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
initial.insert<Similarity3>(X(4), Similarity3(Rot3::Ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
initial.insert<Similarity3>(X(5), Similarity3(Rot3::Ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
//initial.print("Initial Estimate\n");

2
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -60,7 +60,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3));
double yaw = 0;
// This returns body-to-nav nRb
Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse();
Rot3 bRn = Rot3::Ypr(yaw, pitch, roll).inverse();
return Mechanization_bRn2(bRn, x_g, x_a);
}

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

@ -22,7 +22,7 @@ static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
// camera pose at (0,0,1) looking straight along the x-axis.
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;

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

@ -24,8 +24,8 @@ using namespace gtsam;
TEST( InvDepthFactorVariant1, optimize) {
// Create two poses looking in the x-direction
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0));
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5));
Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0));
Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5));
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
Point3 landmark(5, 0, 1);

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

@ -24,8 +24,8 @@ using namespace gtsam;
TEST( InvDepthFactorVariant2, optimize) {
// Create two poses looking in the x-direction
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
Point3 landmark(5, 0, 1);

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

@ -24,8 +24,8 @@ using namespace gtsam;
TEST( InvDepthFactorVariant3, optimize) {
// Create two poses looking in the x-direction
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
Point3 landmark(5, 0, 1);

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

@ -18,7 +18,7 @@ SharedNoiseModel model1 = noiseModel::Unit::Create(1);
const double tol = 1e-5;
const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
const Pose3 pose2(Rot3::pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0));
const Point3 point1(3.0, 4.0, 2.0);
const gtsam::Key poseKey = 1, pointKey = 2;

68
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -115,7 +115,7 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) {
/* *************************************************************************/
TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
StereoCamera level_camera(level_pose, K2);
@ -154,7 +154,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, noisy ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
StereoCamera level_camera(level_pose, K2);
@ -172,7 +172,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
Values values;
values.insert(x1, level_pose);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
values.insert(x2, level_pose_right.compose(noise_pose));
@ -206,7 +206,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2);
// create second camera 1 meter to the right of first camera
@ -257,8 +257,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
@ -354,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
@ -397,8 +397,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
@ -422,7 +422,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
@ -466,8 +466,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
@ -490,7 +490,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
@ -587,7 +587,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// views.push_back(x3);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K);
// // create second camera 1 meter to the right of first camera
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
@ -626,8 +626,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
//
// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
// Values values;
// values.insert(x1, pose1);
// values.insert(x2, pose2);
@ -648,7 +648,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// views.push_back(x3);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K2);
//
// // create second camera 1 meter to the right of first camera
@ -684,7 +684,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
//
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
// Values values;
// values.insert(x1, pose1);
// values.insert(x2, pose2);
@ -708,7 +708,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K);
// create second camera
@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
values.insert(x1, pose1);
values.insert(x2, pose2);
// initialize third pose with some noise, we expect it to move back to original pose3
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
values.insert(x3, pose3 * noise_pose);
@ -796,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// views.push_back(x3);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K2);
//
// // create second camera 1 meter to the right of first camera
@ -835,7 +835,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
// graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
//
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise
// Values values;
// values.insert(x1, pose1);
// values.insert(x2, pose2*noise_pose);
@ -862,7 +862,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// views.push_back(x3);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K);
//
// // create second camera 1 meter to the right of first camera
@ -908,8 +908,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
// graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
//
// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
// Values values;
// values.insert(x1, pose1);
// values.insert(x2, pose2);
@ -935,7 +935,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// views.push_back(x2);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K2);
//
// // create second camera 1 meter to the right of first camera
@ -955,7 +955,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
// smartFactor1->add(measurements_cam1,views, model, K2);
//
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
// Values values;
// values.insert(x1, pose1);
// values.insert(x2, pose2);
@ -977,7 +977,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera
@ -1005,7 +1005,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
smartFactorInstance->linearize(values);
// hessianFactor->print("Hessian factor \n");
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues;
rotValues.insert(x1, poseDrift.compose(pose1));
@ -1021,7 +1021,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
assert_equal(hessianFactor->information(),
hessianFactorRot->information(), 1e-7));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
Values tranValues;
@ -1047,7 +1047,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2);
// Second and third cameras in same place, which is a degenerate configuration
@ -1072,7 +1072,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
values);
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues;
rotValues.insert(x1, poseDrift.compose(pose1));
@ -1087,7 +1087,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
assert_equal(hessianFactor->information(),
hessianFactorRot->information(), 1e-6));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
Values tranValues;