Merge remote-tracking branch 'origin/develop' into feature/ExpressionsBALExample

doc/Mathematica/Quaternion-Logmap.nb

@@ -0,0 +1,226 @@
+(* Content-type: application/vnd.wolfram.mathematica *)
+
+(*** Wolfram Notebook File ***)
+(* http://www.wolfram.com/nb *)
+
+(* CreatedBy='Mathematica 10.0' *)
+
+(*CacheID: 234*)
+(* Internal cache information:
+NotebookFileLineBreakTest
+NotebookFileLineBreakTest
+NotebookDataPosition[       158,          7]
+NotebookDataLength[      6004,        217]
+NotebookOptionsPosition[      5104,        179]
+NotebookOutlinePosition[      5456,        195]
+CellTagsIndexPosition[      5413,        192]
+WindowFrame->Normal*)
+
+(* Beginning of Notebook Content *)
+Notebook[{
+Cell["\<\
+In Quaternion.h we have Logmap, but we have to be careful when qw approaches \
+-1 (from above) or 1 (from below). The Taylor expansions below are the basis \
+for the code.\
+\>", "Text",
+ CellChangeTimes->{{3.632651837171029*^9, 3.6326518973274307`*^9}}],
+
+Cell[CellGroupData[{
+
+Cell[BoxData[
+ RowBox[{"angle", "=", 
+  RowBox[{"2", 
+   RowBox[{"ArcCos", "[", "qw", "]"}]}]}]], "Input",
+ CellChangeTimes->{{3.6326509558588057`*^9, 3.632650976842943*^9}}],
+
+Cell[BoxData[
+ RowBox[{"2", " ", 
+  RowBox[{"ArcCos", "[", "qw", "]"}]}]], "Output",
+ CellChangeTimes->{{3.6326509669341784`*^9, 3.6326509795921097`*^9}}]
+}, Open  ]],
+
+Cell[CellGroupData[{
+
+Cell[BoxData[
+ RowBox[{"s", "=", 
+  RowBox[{"Sqrt", "[", 
+   RowBox[{"1", "-", 
+    RowBox[{"qw", "*", "qw"}]}], "]"}]}]], "Input",
+ CellChangeTimes->{{3.632650983796185*^9, 3.632650994132272*^9}}],
+
+Cell[BoxData[
+ SqrtBox[
+  RowBox[{"1", "-", 
+   SuperscriptBox["qw", "2"]}]]], "Output",
+ CellChangeTimes->{3.63265099440246*^9}]
+}, Open  ]],
+
+Cell[CellGroupData[{
+
+Cell[BoxData[
+ RowBox[{"factor", " ", "=", " ", 
+  RowBox[{"angle", "/", "s"}]}]], "Input",
+ CellChangeTimes->{{3.632650999925654*^9, 3.632651001339293*^9}, {
+  3.632651070297429*^9, 3.632651071527272*^9}}],
+
+Cell[BoxData[
+ FractionBox[
+  RowBox[{"2", " ", 
+   RowBox[{"ArcCos", "[", "qw", "]"}]}], 
+  SqrtBox[
+   RowBox[{"1", "-", 
+    SuperscriptBox["qw", "2"]}]]]], "Output",
+ CellChangeTimes->{3.632651001671771*^9, 3.632651072007021*^9}]
+}, Open  ]],
+
+Cell[CellGroupData[{
+
+Cell[BoxData[
+ RowBox[{"Expand", "[", 
+  RowBox[{"Series", "[", 
+   RowBox[{
+    FractionBox[
+     RowBox[{"2", " ", 
+      RowBox[{"ArcCos", "[", "qw", "]"}]}], 
+     SqrtBox[
+      RowBox[{"1", "-", 
+       SuperscriptBox["qw", "2"]}]]], ",", 
+    RowBox[{"{", 
+     RowBox[{"qw", ",", "1", ",", "1"}], "}"}], ",", 
+    RowBox[{"Assumptions", "->", 
+     RowBox[{"(", 
+      RowBox[{"qw", "<", "1"}], ")"}]}]}], "]"}], "]"}]], "Input",
+ CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
+  3.6326511716876993`*^9, 3.632651189491748*^9}, {3.632651248821335*^9, 
+  3.632651267905816*^9}}],
+
+Cell[BoxData[
+ InterpretationBox[
+  RowBox[{"2", "-", 
+   FractionBox[
+    RowBox[{"2", " ", 
+     RowBox[{"(", 
+      RowBox[{"qw", "-", "1"}], ")"}]}], "3"], "+", 
+   InterpretationBox[
+    SuperscriptBox[
+     RowBox[{"O", "[", 
+      RowBox[{"qw", "-", "1"}], "]"}], 
+     RowBox[{"3", "/", "2"}]],
+    SeriesData[$CellContext`qw, 1, {}, 0, 3, 2],
+    Editable->False]}],
+  SeriesData[$CellContext`qw, 1, {2, 0, 
+    Rational[-2, 3]}, 0, 3, 2],
+  Editable->False]], "Output",
+ CellChangeTimes->{{3.632651102947558*^9, 3.632651118218814*^9}, {
+  3.632651179610784*^9, 3.6326511898522263`*^9}, {3.632651249719887*^9, 
+  3.632651268312502*^9}}]
+}, Open  ]],
+
+Cell[CellGroupData[{
+
+Cell[BoxData[
+ RowBox[{"ArcCos", "[", 
+  RowBox[{"-", "1"}], "]"}]], "Input",
+ CellChangeTimes->{{3.632651352754121*^9, 3.63265135286866*^9}}],
+
+Cell[BoxData["\[Pi]"], "Output",
+ CellChangeTimes->{3.632651353300222*^9}]
+}, Open  ]],
+
+Cell[CellGroupData[{
+
+Cell[BoxData[
+ RowBox[{"Expand", "[", 
+  RowBox[{"Series", "[", 
+   RowBox[{
+    FractionBox[
+     RowBox[{
+      RowBox[{"-", "2"}], 
+      RowBox[{"ArcCos", "[", 
+       RowBox[{"-", "qw"}], "]"}]}], 
+     SqrtBox[
+      RowBox[{"1", "-", 
+       SuperscriptBox["qw", "2"]}]]], ",", 
+    RowBox[{"{", 
+     RowBox[{"qw", ",", 
+      RowBox[{"-", "1"}], ",", "1"}], "}"}], ",", 
+    RowBox[{"Assumptions", "->", 
+     RowBox[{"(", 
+      RowBox[{"qw", ">", 
+       RowBox[{"-", "1"}]}], ")"}]}]}], "]"}], "]"}]], "Input",
+ CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
+  3.6326511716876993`*^9, 3.6326512088422937`*^9}, {3.632651301817163*^9, 
+  3.6326513406015453`*^9}, {3.63265150259446*^9, 3.632651505055284*^9}, {
+  3.632651744223112*^9, 3.632651772717318*^9}}],
+
+Cell[BoxData[
+ InterpretationBox[
+  RowBox[{
+   RowBox[{"-", "2"}], "-", 
+   FractionBox[
+    RowBox[{"2", " ", 
+     RowBox[{"(", 
+      RowBox[{"qw", "+", "1"}], ")"}]}], "3"], "+", 
+   InterpretationBox[
+    SuperscriptBox[
+     RowBox[{"O", "[", 
+      RowBox[{"qw", "+", "1"}], "]"}], 
+     RowBox[{"3", "/", "2"}]],
+    SeriesData[$CellContext`qw, -1, {}, 0, 3, 2],
+    Editable->False]}],
+  SeriesData[$CellContext`qw, -1, {-2, 0, 
+    Rational[-2, 3]}, 0, 3, 2],
+  Editable->False]], "Output",
+ CellChangeTimes->{
+  3.632651209181905*^9, 3.6326513025091133`*^9, {3.632651332608609*^9, 
+   3.632651341031022*^9}, 3.632651506516138*^9, {3.632651746679185*^9, 
+   3.632651773032124*^9}}]
+}, Open  ]]
+},
+WindowSize->{808, 751},
+WindowMargins->{{4, Automatic}, {Automatic, 4}},
+FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (June 27, \
+2014)",
+StyleDefinitions->"Default.nb"
+]
+(* End of Notebook Content *)
+
+(* Internal cache information *)
+(*CellTagsOutline
+CellTagsIndex->{}
+*)
+(*CellTagsIndex
+CellTagsIndex->{}
+*)
+(*NotebookFileOutline
+Notebook[{
+Cell[558, 20, 263, 5, 49, "Text"],
+Cell[CellGroupData[{
+Cell[846, 29, 174, 4, 28, "Input"],
+Cell[1023, 35, 154, 3, 28, "Output"]
+}, Open  ]],
+Cell[CellGroupData[{
+Cell[1214, 43, 197, 5, 28, "Input"],
+Cell[1414, 50, 129, 4, 40, "Output"]
+}, Open  ]],
+Cell[CellGroupData[{
+Cell[1580, 59, 206, 4, 28, "Input"],
+Cell[1789, 65, 233, 7, 59, "Output"]
+}, Open  ]],
+Cell[CellGroupData[{
+Cell[2059, 77, 605, 17, 61, "Input"],
+Cell[2667, 96, 645, 19, 48, "Output"]
+}, Open  ]],
+Cell[CellGroupData[{
+Cell[3349, 120, 142, 3, 28, "Input"],
+Cell[3494, 125, 74, 1, 28, "Output"]
+}, Open  ]],
+Cell[CellGroupData[{
+Cell[3605, 131, 788, 22, 61, "Input"],
+Cell[4396, 155, 692, 21, 48, "Output"]
+}, Open  ]]
+}
+]
+*)
+
+(* End of internal cache information *)

gtsam/geometry/Quaternion.h

@@ -96,14 +96,15 @@ struct traits<QUATERNION_TYPE> {
     Vector3 omega;
 
     const double qw = q.w();
+    // See Quaternion-Logmap.nb in doc for Taylor expansions
     if (qw > NearlyOne) {
       // Taylor expansion of (angle / s) at 1
-      //return (2 + 2 * (1-qw) / 3) * q.vec();
+      // (2 + 2 * (1-qw) / 3) * q.vec();
-      omega = (8. / 3. - 2. / 3. * qw) * q.vec();
+      omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
     } else if (qw < NearlyNegativeOne) {
       // Taylor expansion of (angle / s) at -1
-      //return (-2 - 2 * (1 + qw) / 3) * q.vec();
+      // (-2 - 2 * (1 + qw) / 3) * q.vec();
-      omega = (-8. / 3 + 2. / 3 * qw) * q.vec();
+      omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
     } else {
       // Normal, away from zero case
       double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);

gtsam/geometry/tests/testPose3.cpp

@@ -758,7 +758,6 @@ TEST(Pose3 , Invariants) {
   check_manifold_invariants(id,T3);
   check_manifold_invariants(T2,id);
   check_manifold_invariants(T2,T3);
-
 }
 
 //******************************************************************************
@@ -769,7 +768,6 @@ TEST(Pose3 , LieGroupDerivatives) {
   CHECK_LIE_GROUP_DERIVATIVES(id,T2);
   CHECK_LIE_GROUP_DERIVATIVES(T2,id);
   CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
-
 }
 
 //******************************************************************************
@@ -777,14 +775,15 @@ TEST(Pose3 , ChartDerivatives) {
   Pose3 id;
   if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
     CHECK_CHART_DERIVATIVES(id,id);
-    CHECK_CHART_DERIVATIVES(id,T2);
+//    CHECK_CHART_DERIVATIVES(id,T2);
-    CHECK_CHART_DERIVATIVES(T2,id);
+//    CHECK_CHART_DERIVATIVES(T2,id);
-    CHECK_CHART_DERIVATIVES(T2,T3);
+//    CHECK_CHART_DERIVATIVES(T2,T3);
   }
 }
 
 /* ************************************************************************* */
-int main(){ //TestResult tr; return TestRegistry::runAllTests(tr);}
+int main() {
-  std::cout<<"testPose3 currently disabled!!" << std::endl;
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */

gtsam/geometry/tests/testQuaternion.cpp

@@ -39,6 +39,14 @@ TEST(Quaternion , Constructor) {
   Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
 }
 
+//******************************************************************************
+TEST(Quaternion , Logmap) {
+  Q q1(5e-06, 0, 0, 1), q2(-5e-06, 0, 0, -1);
+  Vector3 v1 = traits<Q>::Logmap(q1);
+  Vector3 v2 = traits<Q>::Logmap(q2);
+  EXPECT(assert_equal(v1, v2));
+}
+
 //******************************************************************************
 TEST(Quaternion , Local) {
   Vector3 z_axis(0, 0, 1);
@@ -47,7 +55,7 @@ TEST(Quaternion , Local) {
   QuaternionJacobian H1, H2;
   Vector3 expected(0, 0, 0.1);
   Vector3 actual = traits<Q>::Local(q1, q2, H1, H2);
-  EXPECT(assert_equal((Vector)expected,actual));
+  EXPECT(assert_equal((Vector )expected, actual));
 }
 
 //******************************************************************************
@@ -69,7 +77,7 @@ TEST(Quaternion , Compose) {
 
   Q expected = q1 * q2;
   Q actual = traits<Q>::Compose(q1, q2);
-  EXPECT(traits<Q>::Equals(expected,actual));
+  EXPECT(traits<Q>::Equals(expected, actual));
 }
 
 //******************************************************************************
@@ -85,7 +93,7 @@ TEST(Quaternion , Between) {
 
   Q expected = q1.inverse() * q2;
   Q actual = traits<Q>::Between(q1, q2);
-  EXPECT(traits<Q>::Equals(expected,actual));
+  EXPECT(traits<Q>::Equals(expected, actual));
 }
 
 //******************************************************************************
@@ -94,36 +102,36 @@ TEST(Quaternion , Inverse) {
   Q expected(Eigen::AngleAxisd(-0.1, z_axis));
 
   Q actual = traits<Q>::Inverse(q1);
-  EXPECT(traits<Q>::Equals(expected,actual));
+  EXPECT(traits<Q>::Equals(expected, actual));
 }
 
 //******************************************************************************
 TEST(Quaternion , Invariants) {
-  check_group_invariants(id,id);
+  check_group_invariants(id, id);
-  check_group_invariants(id,R1);
+  check_group_invariants(id, R1);
-  check_group_invariants(R2,id);
+  check_group_invariants(R2, id);
-  check_group_invariants(R2,R1);
+  check_group_invariants(R2, R1);
 
-  check_manifold_invariants(id,id);
+  check_manifold_invariants(id, id);
-  check_manifold_invariants(id,R1);
+  check_manifold_invariants(id, R1);
-  check_manifold_invariants(R2,id);
+  check_manifold_invariants(R2, id);
-  check_manifold_invariants(R2,R1);
+  check_manifold_invariants(R2, R1);
 }
 
 //******************************************************************************
 TEST(Quaternion , LieGroupDerivatives) {
-  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
-  CHECK_LIE_GROUP_DERIVATIVES(id,R2);
+  CHECK_LIE_GROUP_DERIVATIVES(id, R2);
-  CHECK_LIE_GROUP_DERIVATIVES(R2,id);
+  CHECK_LIE_GROUP_DERIVATIVES(R2, id);
-  CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
+  CHECK_LIE_GROUP_DERIVATIVES(R2, R1);
 }
 
 //******************************************************************************
 TEST(Quaternion , ChartDerivatives) {
-  CHECK_CHART_DERIVATIVES(id,id);
+  CHECK_CHART_DERIVATIVES(id, id);
-  CHECK_CHART_DERIVATIVES(id,R2);
+  CHECK_CHART_DERIVATIVES(id, R2);
-  CHECK_CHART_DERIVATIVES(R2,id);
+  CHECK_CHART_DERIVATIVES(R2, id);
-  CHECK_CHART_DERIVATIVES(R2,R1);
+  CHECK_CHART_DERIVATIVES(R2, R1);
 }
 
 //******************************************************************************

gtsam/geometry/tests/testRot3.cpp

@@ -697,9 +697,8 @@ TEST(Rot3 , ChartDerivatives) {
 
 /* ************************************************************************* */
 int main() {
-//  TestResult tr;
+  TestResult tr;
-//  return TestRegistry::runAllTests(tr);
+  return TestRegistry::runAllTests(tr);
-  std::cout << "testRot3 currently disabled!!" << std::endl;
 }
 /* ************************************************************************* */
 

gtsam/geometry/tests/testRot3Q.cpp

@@ -15,20 +15,82 @@
  * @author  Alireza Fathi
  */
 
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Quaternion.h>
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/lieProxies.h>
-
-#include <boost/math/constants/constants.hpp>
 
 #include <CppUnitLite/TestHarness.h>
 
+using namespace std;
+using namespace gtsam;
+
 #ifdef GTSAM_USE_QUATERNIONS
 
-// No quaternion only tests
+//******************************************************************************
+TEST(Rot3Q , Compare) {
+  using boost::none;
+
+  // We set up expected values with quaternions
+  typedef Quaternion Q;
+  typedef traits<Q> TQ;
+  typedef TQ::ChartJacobian OJ;
+
+  // We check Rot3 expressions
+  typedef Rot3 R;
+  typedef traits<R> TR;
+
+  // Define test values
+  Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
+  Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
+  R R1(q1), R2(q2);
+
+  // Check Compose
+  Q q3 = TQ::Compose(q1, q2, none, none);
+  R R3 = TR::Compose(R1, R2, none, none);
+  EXPECT(assert_equal(R(q3), R3));
+
+  // Check Retract
+  Vector3 v(1e-5, 0, 0);
+  Q q4 = TQ::Retract(q3, v);
+  R R4 = TR::Retract(R3, v);
+  EXPECT(assert_equal(R(q4), R4));
+
+  // Check Between
+  Q q5 = TQ::Between(q3, q4);
+  R R5 = R3.between(R4);
+  EXPECT(assert_equal(R(q5), R5));
+
+  // Check toQuaternion
+  EXPECT(assert_equal(q5, R5.toQuaternion()));
+
+  // Check Logmap
+  Vector3 vQ = TQ::Logmap(q5);
+  Vector3 vR = R::Logmap(R5);
+  EXPECT(assert_equal(vQ, vR));
+
+  // Check Local
+  vQ = TQ::Local(q3, q4);
+  vR = TR::Local(R3, R4);
+  EXPECT(assert_equal(vQ, vR));
+
+  // Check Retract/Local of Compose
+  Vector3 vQ1 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, v)));
+  Vector3 vR1 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, v)));
+  EXPECT(assert_equal(vQ1, vR1));
+  Vector3 vQ2 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, -v)));
+  Vector3 vR2 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, -v)));
+  EXPECT(assert_equal(vQ2, vR2));
+  EXPECT(assert_equal<Vector3>((vQ1 - vQ2) / 0.2, (vR1 - vR2) / 0.2));
+
+  // Check Compose Derivatives
+  Matrix HQ, HR;
+  HQ = numericalDerivative42<Q, Q, Q, OJ, OJ>(TQ::Compose, q1, q2, none, none);
+  HR = numericalDerivative42<R, R, R, OJ, OJ>(TR::Compose, R1, R2, none, none);
+  EXPECT(assert_equal(HQ, HR));
+
+}
 
 #endif