Merge remote-tracking branch 'origin/develop' into feature/ExpressionsBALExample
dellaert
commit
b06c353073
|
|
@ -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 *)
|
||||
|
|
@ -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();
|
||||
omega = (8. / 3. - 2. / 3. * qw) * q.vec();
|
||||
// (2 + 2 * (1-qw) / 3) * 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();
|
||||
omega = (-8. / 3 + 2. / 3 * qw) * q.vec();
|
||||
// (-2 - 2 * (1 + qw) / 3) * 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);
|
||||
|
|
|
|||
|
|
@ -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(T2,id);
|
||||
CHECK_CHART_DERIVATIVES(T2,T3);
|
||||
// CHECK_CHART_DERIVATIVES(id,T2);
|
||||
// CHECK_CHART_DERIVATIVES(T2,id);
|
||||
// CHECK_CHART_DERIVATIVES(T2,T3);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(){ //TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
std::cout<<"testPose3 currently disabled!!" << std::endl;
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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,R1);
|
||||
check_group_invariants(R2,id);
|
||||
check_group_invariants(R2,R1);
|
||||
check_group_invariants(id, id);
|
||||
check_group_invariants(id, R1);
|
||||
check_group_invariants(R2, id);
|
||||
check_group_invariants(R2, R1);
|
||||
|
||||
check_manifold_invariants(id,id);
|
||||
check_manifold_invariants(id,R1);
|
||||
check_manifold_invariants(R2,id);
|
||||
check_manifold_invariants(R2,R1);
|
||||
check_manifold_invariants(id, id);
|
||||
check_manifold_invariants(id, R1);
|
||||
check_manifold_invariants(R2, id);
|
||||
check_manifold_invariants(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , LieGroupDerivatives) {
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , ChartDerivatives) {
|
||||
CHECK_CHART_DERIVATIVES(id,id);
|
||||
CHECK_CHART_DERIVATIVES(id,R2);
|
||||
CHECK_CHART_DERIVATIVES(R2,id);
|
||||
CHECK_CHART_DERIVATIVES(R2,R1);
|
||||
CHECK_CHART_DERIVATIVES(id, id);
|
||||
CHECK_CHART_DERIVATIVES(id, R2);
|
||||
CHECK_CHART_DERIVATIVES(R2, id);
|
||||
CHECK_CHART_DERIVATIVES(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -697,9 +697,8 @@ TEST(Rot3 , ChartDerivatives) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
// TestResult tr;
|
||||
// return TestRegistry::runAllTests(tr);
|
||||
std::cout << "testRot3 currently disabled!!" << std::endl;
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue