diff --git a/CMakeLists.txt b/CMakeLists.txt index 0888a394e..38ee89760 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -424,6 +424,7 @@ message(STATUS " CPack Generator : ${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") message(STATUS "MATLAB toolbox flags ") diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 49069c57f..a6860f205 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -19,7 +19,6 @@ install(FILES GtsamMatlabWrap.cmake GtsamPythonWrap.cmake GtsamTesting.cmake - GtsamTestingObsolete.cmake README.html DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 2e505c11e..4b3af9810 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -109,9 +109,8 @@ add_custom_target(examples) # Add timing target add_custom_target(timing) -# Include obsolete macros - will be removed in the near future -include(GtsamTestingObsolete) - +# Add target to build tests without running +add_custom_target(all.tests) # Implementations of this file's macros: @@ -165,6 +164,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${script_name} COMMAND ${script_name}) add_dependencies(check.${groupName} ${script_name}) add_dependencies(check ${script_name}) + add_dependencies(all.tests ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) endif() @@ -195,6 +195,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${target_name} COMMAND ${target_name}) add_dependencies(check.${groupName} ${target_name}) add_dependencies(check ${target_name}) + add_dependencies(all.tests ${script_name}) # Add TOPSRCDIR set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") diff --git a/cmake/GtsamTestingObsolete.cmake b/cmake/obsolete/GtsamTestingObsolete.cmake similarity index 100% rename from cmake/GtsamTestingObsolete.cmake rename to cmake/obsolete/GtsamTestingObsolete.cmake diff --git a/doc/Mathematica/Quaternion-Logmap.nb b/doc/Mathematica/Quaternion-Logmap.nb new file mode 100644 index 000000000..154cd7e51 --- /dev/null +++ b/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 *) diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt index ea3dd0298..cc02a6ed2 100644 --- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -3,6 +3,7 @@ include_directories(.) # Find sources. file(GLOB metis_sources *.c) # Build libmetis. +add_definitions(-fPIC) add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) if(UNIX) target_link_libraries(metis m) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp new file mode 100644 index 000000000..088a84243 --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.cpp + * @date Dec 19, 2013 + * @author Alex Trevor + * @brief A plane, represented by a normal direction and perpendicular distance + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +/// The print fuction +void OrientedPlane3::print(const std::string& s) const { + Vector coeffs = planeCoefficients(); + cout << s << " : " << coeffs << endl; +} + +/* ************************************************************************* */ +OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr, + OptionalJacobian<3, 3> Hp) { + Matrix n_hr; + Matrix n_hp; + Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); + + Vector n_unit = plane.n_.unitVector(); + Vector unit_vec = n_rotated.unitVector(); + double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; + OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2), + pred_d); + + if (Hr) { + *Hr = gtsam::zeros(3, 6); + (*Hr).block<2, 3>(0, 0) = n_hr; + (*Hr).block<1, 3>(2, 3) = unit_vec; + } + if (Hp) { + Vector xrp = xr.translation().vector(); + Matrix n_basis = plane.n_.basis(); + Vector hpp = n_basis.transpose() * xrp; + *Hp = gtsam::zeros(3, 3); + (*Hp).block<2, 2>(0, 0) = n_hp; + (*Hp).block<1, 2>(2, 0) = hpp; + (*Hp)(2, 2) = 1; + } + + return (transformed_plane); +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { + Vector2 n_error = -n_.localCoordinates(plane.n_); + double d_error = d_ - plane.d_; + Vector3 e; + e << n_error(0), n_error(1), d_error; + return (e); +} + +/* ************************************************************************* */ +OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { + // Retract the Unit3 + Vector2 n_v(v(0), v(1)); + Unit3 n_retracted = n_.retract(n_v); + double d_retracted = d_ + v(2); + return OrientedPlane3(n_retracted, d_retracted); +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { + Vector n_local = n_.localCoordinates(y.n_); + double d_local = d_ - y.d_; + Vector3 e; + e << n_local(0), n_local(1), -d_local; + return e; +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::planeCoefficients() const { + Vector unit_vec = n_.unitVector(); + Vector3 a; + a << unit_vec[0], unit_vec[1], unit_vec[2], d_; + return a; +} + +/* ************************************************************************* */ + +} diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h new file mode 100644 index 000000000..28b67cb4e --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.h + * @date Dec 19, 2013 + * @author Alex Trevor + * @author Frank Dellaert + * @brief An infinite plane, represented by a normal direction and perpendicular distance + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/// Represents an infinite plane in 3D. +class OrientedPlane3: public DerivedValue { + +private: + + Unit3 n_; /// The direction of the planar normal + double d_; /// The perpendicular distance to this plane + +public: + enum { + dimension = 3 + }; + /// @name Constructors + /// @{ + + /// Default constructor + OrientedPlane3() : + n_(), d_(0.0) { + } + + /// Construct from a Unit3 and a distance + OrientedPlane3(const Unit3& s, double d) : + n_(s), d_(d) { + } + + OrientedPlane3(Vector vec) : + n_(vec(0), vec(1), vec(2)), d_(vec(3)) { + } + + /// Construct from a, b, c, d + OrientedPlane3(double a, double b, double c, double d) { + Point3 p(a, b, c); + n_ = Unit3(p); + d_ = d; + } + + /// The print fuction + void print(const std::string& s = std::string()) const; + + /// The equals function with tolerance + bool equals(const OrientedPlane3& s, double tol = 1e-9) const { + return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); + } + + /// Transforms a plane to the specified pose + static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, + OptionalJacobian<3, 3> Hp = boost::none); + + /// Computes the error between two poses + Vector3 error(const gtsam::OrientedPlane3& plane) const; + + /// Dimensionality of tangent space = 3 DOF + inline static size_t Dim() { + return 3; + } + + /// Dimensionality of tangent space = 3 DOF + inline size_t dim() const { + return 3; + } + + /// The retract function + OrientedPlane3 retract(const Vector& v) const; + + /// The local coordinates function + Vector3 localCoordinates(const OrientedPlane3& s) const; + + /// Returns the plane coefficients (a, b, c, d) + Vector3 planeCoefficients() const; + + inline Unit3 normal() const { + return n_; + } + + /// @} +}; + +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; + +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; + +} // namespace gtsam + diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 02ff31b21..cd093ca61 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -96,14 +96,15 @@ struct traits { 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); diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 611cf7cde..be48aecc9 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -31,6 +31,10 @@ # pragma clang diagnostic pop #endif +#ifdef GTSAM_USE_TBB +#include +#endif + #include #include @@ -65,8 +69,15 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return result; } +#ifdef GTSAM_USE_TBB +tbb::mutex unit3BasisMutex; +#endif + /* ************************************************************************* */ const Matrix32& Unit3::basis() const { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(unit3BasisMutex); +#endif // Return cached version if exists if (B_) @@ -108,7 +119,7 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { +Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix23 Bt = basis().transpose(); Vector2 xi = Bt * q.p_.vector(); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 50ffb55b7..7d145c213 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -38,7 +38,9 @@ private: public: - enum { dimension = 2 }; + enum { + dimension = 2 + }; /// @name Constructors /// @{ @@ -65,8 +67,8 @@ public: } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = - boost::none); + static Unit3 FromPoint3(const Point3& point, // + OptionalJacobian<2, 3> H = boost::none); /// Random direction, using boost::uniform_on_sphere static Unit3 Random(boost::mt19937 & rng); @@ -99,24 +101,29 @@ public: Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(OptionalJacobian<3,2> H = boost::none) const { + const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const { if (H) *H = basis(); return p_; } + /// Return unit-norm Vector + Vector unitVector(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return (p_.vector()); + } + /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { return s * d.p_; } /// Signed, vector-valued error between two directions - Vector2 error(const Unit3& q, - OptionalJacobian<2,2> H = boost::none) const; + Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; /// Distance between two directions - double distance(const Unit3& q, - OptionalJacobian<1,2> H = boost::none) const; + double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; /// @} @@ -153,25 +160,27 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(p_); - // homebrew serialize Eigen Matrix - ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); - ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); - ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); - ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); - ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); - ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); - } + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(p_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0, 1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1, 0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1, 1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2, 0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2, 1)); + } /// @} }; // Define GTSAM traits -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold { +}; -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold { +}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp new file mode 100644 index 000000000..14a387102 --- /dev/null +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3 class + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +//******************************************************************************* +TEST (OrientedPlane3, transform) +{ + // Test transforming a plane to a pose + 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 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3); + OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none); + EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9)); + + // Test the jacobians of transform + Matrix actualH1, expectedH1, actualH2, expectedH2; + { + expectedH1 = numericalDerivative11(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose); + + OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none); + EXPECT (assert_equal (expectedH1, actualH1, 1e-9)); + } + { + expectedH2 = numericalDerivative11 (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane); + + OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2); + EXPECT (assert_equal (expectedH2, actualH2, 1e-9)); + } + +} + +//******************************************************************************* +// Returns a random vector -- copied from testUnit3.cpp +inline static Vector randomVector(const Vector& minLimits, + const Vector& maxLimits) { + + // Get the number of dimensions and create the return vector + size_t numDims = dim(minLimits); + Vector vector = zero(numDims); + + // Create the random vector + for (size_t i = 0; i < numDims; i++) { + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } + return vector; +} + +//******************************************************************************* +TEST(OrientedPlane3, localCoordinates_retract) { + + size_t numIterations = 10000; + gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); + minPlaneLimit << -1.0, -1.0, -1.0, 0.01; + maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; + + Vector minXiLimit(3),maxXiLimit(3); + minXiLimit << -M_PI, -M_PI, -10.0; + maxXiLimit << M_PI, M_PI, 10.0; + for (size_t i = 0; i < numIterations; i++) { + + sleep(0); + + // Create a Plane + OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit)); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + + // Magnitude of the rotation can be at most pi + if (v12.segment<3>(0).norm () > M_PI) + v12.segment<3>(0) = v12.segment<3>(0) / M_PI; + OrientedPlane3 p2 = p1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = p1.localCoordinates(p2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + OrientedPlane3 actual_p2 = p1.retract(actual_v12); + EXPECT(assert_equal(p2, actual_p2, 1e-3)); + } +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 2b6edee66..a716406a4 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/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(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); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 5f1032916..82d3283bd 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/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::Logmap(q1); + Vector3 v2 = traits::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::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::Compose(q1, q2); - EXPECT(traits::Equals(expected,actual)); + EXPECT(traits::Equals(expected, actual)); } //****************************************************************************** @@ -85,7 +93,7 @@ TEST(Quaternion , Between) { Q expected = q1.inverse() * q2; Q actual = traits::Between(q1, q2); - EXPECT(traits::Equals(expected,actual)); + EXPECT(traits::Equals(expected, actual)); } //****************************************************************************** @@ -94,36 +102,36 @@ TEST(Quaternion , Inverse) { Q expected(Eigen::AngleAxisd(-0.1, z_axis)); Q actual = traits::Inverse(q1); - EXPECT(traits::Equals(expected,actual)); + EXPECT(traits::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); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index fef85a353..7e0dcc43f 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -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); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 65ca9e067..9ae8eef13 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -15,20 +15,82 @@ * @author Alireza Fathi */ -#include #include +#include #include #include -#include - -#include #include +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 TQ; + typedef TQ::ChartJacobian OJ; + + // We check Rot3 expressions + typedef Rot3 R; + typedef traits 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((vQ1 - vQ2) / 0.2, (vR1 - vR2) / 0.2)); + + // Check Compose Derivatives + Matrix HQ, HR; + HQ = numericalDerivative42(TQ::Compose, q1, q2, none, none); + HR = numericalDerivative42(TR::Compose, R1, R2, none, none); + EXPECT(assert_equal(HQ, HR)); + +} #endif diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 7aa210dad..098af8b6d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -886,6 +886,7 @@ namespace gtsam { typedef noiseModel::Gaussian::shared_ptr SharedGaussian; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; + typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 3f62ed6d8..88758ae7a 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -17,7 +17,7 @@ */ #include - +#include #include namespace gtsam { @@ -25,16 +25,19 @@ namespace gtsam { /* ************************************************************************* */ const char* IndeterminantLinearSystemException::what() const throw() { - if(!description_) + if(!description_) { description_ = String( "\nIndeterminant linear system detected while working near variable\n" - + boost::lexical_cast(j_) + ".\n" + + boost::lexical_cast(j_) + + + " (Symbol: " + boost::lexical_cast( + gtsam::DefaultKeyFormatter(gtsam::Symbol(j_))) + ").\n" "\n\ Thrown when a linear system is ill-posed. The most common cause for this\n\ error is having underconstrained variables. Mathematically, the system is\n\ underdetermined. See the GTSAM Doxygen documentation at\n\ http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for\n\ more information."); + } return description_->c_str(); } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index bd0fd62e0..29b9b6e66 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -83,7 +83,7 @@ public: integrationCovariance_(integrationErrorCovariance) {} /// methods to access class variables - const bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;} + bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;} const Vector3& deltaPij() const {return deltaPij_;} const Vector3& deltaVij() const {return deltaVij_;} const imuBias::ConstantBias& biasHat() const { return biasHat_;} diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp new file mode 100644 index 000000000..7ec72825b --- /dev/null +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -0,0 +1,54 @@ +/* + * OrientedPlane3Factor.cpp + * + * Created on: Jan 29, 2014 + * Author: Natesh Srinivasan + */ + + +#include "OrientedPlane3Factor.h" + +using namespace std; + +namespace gtsam { + +//*************************************************************************** + +void OrientedPlane3DirectionPrior::print(const string& s) const { + cout << "Prior Factor on " << landmarkKey_ << "\n"; + measured_p_.print("Measured Plane"); + this->noiseModel_->print(" noise model: "); +} + +//*************************************************************************** + +bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); +} + +//*************************************************************************** + +Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, + boost::optional H) const { + +if(H) { + Matrix H_p; + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q,H_p); + H->resize(2,3); + H->block <2,2>(0,0) << H_p; + H->block <2,1>(0,2) << Matrix::Zero(2, 1); + return e; +} else { + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q); + return e; +} + +} +} + diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h new file mode 100644 index 000000000..7827a5c2c --- /dev/null +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -0,0 +1,95 @@ +/* + * @file OrientedPlane3Factor.cpp + * @brief OrientedPlane3 Factor class + * @author Alex Trevor + * @date December 22, 2013 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * Factor to measure a planar landmark from a given pose + */ +class OrientedPlane3Factor: public NoiseModelFactor2 { + +protected: + Symbol poseSymbol_; + Symbol landmarkSymbol_; + Vector measured_coeffs_; + OrientedPlane3 measured_p_; + + typedef NoiseModelFactor2 Base; + +public: + + /// Constructor + OrientedPlane3Factor () + {} + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol + OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel, + const Symbol& pose, + const Symbol& landmark) + : Base (noiseModel, pose, landmark), + poseSymbol_ (pose), + landmarkSymbol_ (landmark), + measured_coeffs_ (z) + { + measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); + } + + /// print + void print(const std::string& s="PlaneFactor") const; + virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2); + Vector err(3); + err << predicted_plane.error (measured_p_); + return (err); + }; +}; + +// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior +class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { +protected: + OrientedPlane3 measured_p_; /// measured plane parameters + Key landmarkKey_; + typedef NoiseModelFactor1 Base; +public: + + typedef OrientedPlane3DirectionPrior This; + /// Constructor + OrientedPlane3DirectionPrior () + {} + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol + OrientedPlane3DirectionPrior (Key key, const Vector&z, + const SharedGaussian& noiseModel) + : Base (noiseModel, key), + landmarkKey_ (key) + { + measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); + } + /// print + void print(const std::string& s) const; + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + virtual Vector evaluateError(const OrientedPlane3& plane, + boost::optional H = boost::none) const; +}; + +} // gtsam + diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp new file mode 100644 index 000000000..fa16a0f91 --- /dev/null +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -0,0 +1,183 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3Factor class + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +TEST (OrientedPlane3, lm_translation_error) +{ + // Tests one pose, two measurements of the landmark that differ in range only. + // Normal along -x, 3m away + gtsam::Symbol lm_sym ('p', 0); + gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); + + gtsam::ISAM2 isam2; + gtsam::Values new_values; + gtsam::NonlinearFactorGraph new_graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + gtsam::Symbol init_sym ('x', 0); + gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), + gtsam::Point3 (0.0, 0.0, 0.0)); + gtsam::Vector sigmas(6); + sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; + gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas (sigmas) ); + new_values.insert (init_sym, init_pose); + new_graph.add (pose_prior); + + // Add two landmark measurements, differing in range + new_values.insert (lm_sym, test_lm0); + gtsam::Vector sigmas3(3); + sigmas3 << 0.1, 0.1, 0.1; + gtsam::Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); + new_graph.add (test_meas0); + gtsam::Vector test_meas1_mean(4); + test_meas1_mean << -1.0, 0.0, 0.0, 1.0; + gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); + new_graph.add (test_meas1); + + // Optimize + gtsam::ISAM2Result result = isam2.update (new_graph, new_values); + gtsam::Values result_values = isam2.calculateEstimate (); + gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + gtsam::OrientedPlane3 expected_plane_landmark (-1.0, 0.0, 0.0, 2.0); + EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); +} + +TEST (OrientedPlane3, lm_rotation_error) +{ + // Tests one pose, two measurements of the landmark that differ in angle only. + // Normal along -x, 3m away + gtsam::Symbol lm_sym ('p', 0); + gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); + + gtsam::ISAM2 isam2; + gtsam::Values new_values; + gtsam::NonlinearFactorGraph new_graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + gtsam::Symbol init_sym ('x', 0); + gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), + gtsam::Point3 (0.0, 0.0, 0.0)); + gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas ((Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); + new_values.insert (init_sym, init_pose); + new_graph.add (pose_prior); + +// // Add two landmark measurements, differing in angle + new_values.insert (lm_sym, test_lm0); + Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add (test_meas0); + Vector test_meas1_mean(4); + test_meas1_mean << 0.0, -1.0, 0.0, 3.0; + gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add (test_meas1); + + // Optimize + gtsam::ISAM2Result result = isam2.update (new_graph, new_values); + gtsam::Values result_values = isam2.calculateEstimate (); + gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + gtsam::OrientedPlane3 expected_plane_landmark (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3.0); + EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { + + // Example: pitch and roll of aircraft in an ENU Cartesian frame. + // If pitch and roll are zero for an aerospace frame, + // that means Z is pointing down, i.e., direction of Z = (0,0,-1) + + Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin + + // Factor + Key key(1); + SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0)); + OrientedPlane3DirectionPrior factor(key, planeOrientation, model); + + // Create a linearization point at the zero-error point + Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); + Vector theta2 = Vector4(0.0, 0.1, - 0.8, 10.0); + Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); + + + OrientedPlane3 T1(theta1); + OrientedPlane3 T2(theta2); + OrientedPlane3 T3(theta3); + + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1); + + Matrix expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2); + + Matrix expectedH3 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2, actualH3; + factor.evaluateError(T1, actualH1); + factor.evaluateError(T2, actualH2); + factor.evaluateError(T3, actualH3); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); + EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */