diff --git a/.cproject b/.cproject
index 3a0c3254c..6664fce3b 100644
--- a/.cproject
+++ b/.cproject
@@ -322,6 +322,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -348,7 +356,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -356,7 +363,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -404,7 +410,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -412,7 +417,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -420,7 +424,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -436,20 +439,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -484,6 +478,7 @@
make
+
testGraph.run
true
false
@@ -579,6 +574,7 @@
make
+
testInference.run
true
false
@@ -586,6 +582,7 @@
make
+
testGaussianBayesNet.run
true
false
@@ -593,6 +590,7 @@
make
+
testGaussianFactor.run
true
false
@@ -600,6 +598,7 @@
make
+
testJunctionTree.run
true
false
@@ -607,6 +606,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -614,6 +614,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -747,14 +748,6 @@
true
true
-
- make
- -j2
- vSFMexample.run
- true
- true
- true
-
make
-j2
@@ -763,6 +756,14 @@
true
true
+
+ make
+ -j2
+ vSFMexample.run
+ true
+ true
+ true
+
make
-j2
@@ -973,7 +974,6 @@
make
-
testErrors.run
true
false
@@ -1259,6 +1259,14 @@
true
true
+
+ make
+ -j2
+ tests/testRot3.run
+ true
+ true
+ true
+
make
-j2
@@ -1341,6 +1349,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -1380,6 +1389,7 @@
make
+
testSimulated2D.run
true
false
@@ -1387,6 +1397,7 @@
make
+
testSimulated3D.run
true
false
@@ -1490,6 +1501,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -1511,46 +1523,6 @@
true
true
-
- make
- -j2
- install
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- dist
- true
- true
- true
-
make
-j2
@@ -1647,6 +1619,54 @@
true
true
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ dist
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
make
-j2
@@ -1679,14 +1699,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
@@ -2009,6 +2021,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -2035,7 +2055,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -2043,7 +2062,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -2091,7 +2109,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -2099,7 +2116,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -2107,7 +2123,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -2123,20 +2138,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -2171,6 +2177,7 @@
make
+
testGraph.run
true
false
@@ -2266,6 +2273,7 @@
make
+
testInference.run
true
false
@@ -2273,6 +2281,7 @@
make
+
testGaussianBayesNet.run
true
false
@@ -2280,6 +2289,7 @@
make
+
testGaussianFactor.run
true
false
@@ -2287,6 +2297,7 @@
make
+
testJunctionTree.run
true
false
@@ -2294,6 +2305,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -2301,6 +2313,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -2434,14 +2447,6 @@
true
true
-
- make
- -j2
- vSFMexample.run
- true
- true
- true
-
make
-j2
@@ -2450,6 +2455,14 @@
true
true
+
+ make
+ -j2
+ vSFMexample.run
+ true
+ true
+ true
+
make
-j2
@@ -2660,7 +2673,6 @@
make
-
testErrors.run
true
false
@@ -2946,6 +2958,14 @@
true
true
+
+ make
+ -j2
+ tests/testRot3.run
+ true
+ true
+ true
+
make
-j2
@@ -3028,6 +3048,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -3067,6 +3088,7 @@
make
+
testSimulated2D.run
true
false
@@ -3074,6 +3096,7 @@
make
+
testSimulated3D.run
true
false
@@ -3177,6 +3200,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -3198,46 +3222,6 @@
true
true
-
- make
- -j2
- install
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- dist
- true
- true
- true
-
make
-j2
@@ -3334,6 +3318,54 @@
true
true
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ dist
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
make
-j2
@@ -3366,14 +3398,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 167645b37..7156a2df1 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -152,6 +152,11 @@ namespace gtsam {
return Vector_(3,q(2),q(1),q(0));
}
+ Vector Rot3::rpy() const {
+ Vector q = xyz();
+ return Vector_(3,q(0),q(1),q(2));
+ }
+
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3::Logmap(const Rot3& R) {
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 769596dd0..33cf2b242 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -148,6 +148,12 @@ namespace gtsam {
*/
Vector ypr() const;
+ /**
+ * Use RQ to calculate roll-pitch-yaw angle representation
+ * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
+ */
+ Vector rpy() const;
+
/** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index d56a9308e..73129ec90 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -388,6 +388,7 @@ TEST( Rot3, RQ)
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
+ CHECK(assert_equal(Vector_(3,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_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));