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

Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues

release/4.3a0
dellaert 2015-02-16 23:30:11 +01:00
parent ee3e096135 cd0f8c8c57
commit ae1e454ffd
22 changed files with 1065 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -424,6 +424,7 @@ message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ") message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") 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 ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
message(STATUS "MATLAB toolbox flags ") message(STATUS "MATLAB toolbox flags ")

1
cmake/CMakeLists.txt
View File

@ -19,7 +19,6 @@ install(FILES
GtsamMatlabWrap.cmake GtsamMatlabWrap.cmake
GtsamPythonWrap.cmake GtsamPythonWrap.cmake
GtsamTesting.cmake GtsamTesting.cmake
GtsamTestingObsolete.cmake
README.html README.html
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")

7
cmake/GtsamTesting.cmake
View File

@ -109,9 +109,8 @@ add_custom_target(examples)
# Add timing target # Add timing target
add_custom_target(timing) add_custom_target(timing)
# Include obsolete macros - will be removed in the near future # Add target to build tests without running
include(GtsamTestingObsolete) add_custom_target(all.tests)
# Implementations of this file's macros: # 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_test(NAME ${script_name} COMMAND ${script_name})
add_dependencies(check.${groupName} ${script_name}) add_dependencies(check.${groupName} ${script_name})
add_dependencies(check ${script_name}) add_dependencies(check ${script_name})
add_dependencies(all.tests ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION) if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
endif() endif()
@ -195,6 +195,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_test(NAME ${target_name} COMMAND ${target_name}) add_test(NAME ${target_name} COMMAND ${target_name})
add_dependencies(check.${groupName} ${target_name}) add_dependencies(check.${groupName} ${target_name})
add_dependencies(check ${target_name}) add_dependencies(check ${target_name})
add_dependencies(all.tests ${script_name})
# Add TOPSRCDIR # Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")

0
cmake/GtsamTestingObsolete.cmake → cmake/obsolete/GtsamTestingObsolete.cmake
View File

226
doc/Mathematica/Quaternion-Logmap.nb Normal file
View File

@ -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 *)

1
gtsam/3rdparty/metis/libmetis/CMakeLists.txt vendored
View File

@ -3,6 +3,7 @@ include_directories(.)
# Find sources. # Find sources.
file(GLOB metis_sources *.c) file(GLOB metis_sources *.c)
# Build libmetis. # Build libmetis.
add_definitions(-fPIC)
add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
if(UNIX) if(UNIX)
target_link_libraries(metis m) target_link_libraries(metis m)

103
gtsam/geometry/OrientedPlane3.cpp Normal file
View File

@ -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 <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
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;
}
/* ************************************************************************* */
}

116
gtsam/geometry/OrientedPlane3.h Normal file
View File

@ -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 <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {
/// Represents an infinite plane in 3D.
class OrientedPlane3: public DerivedValue<OrientedPlane3> {
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<OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
} // namespace gtsam

9
gtsam/geometry/Quaternion.h
View File

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

13
gtsam/geometry/Unit3.cpp
View File

@ -31,6 +31,10 @@
# pragma clang diagnostic pop # pragma clang diagnostic pop
#endif #endif
#ifdef GTSAM_USE_TBB
#include <tbb/mutex.h>
#endif
#include <boost/random/variate_generator.hpp> #include <boost/random/variate_generator.hpp>
#include <iostream> #include <iostream>
@ -65,8 +69,15 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
return result; return result;
} }
#ifdef GTSAM_USE_TBB
tbb::mutex unit3BasisMutex;
#endif
/* ************************************************************************* */ /* ************************************************************************* */
const Matrix32& Unit3::basis() const { const Matrix32& Unit3::basis() const {
#ifdef GTSAM_USE_TBB
tbb::mutex::scoped_lock lock(unit3BasisMutex);
#endif
// Return cached version if exists // Return cached version if exists
if (B_) 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 // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix23 Bt = basis().transpose(); Matrix23 Bt = basis().transpose();
Vector2 xi = Bt * q.p_.vector(); Vector2 xi = Bt * q.p_.vector();

41
gtsam/geometry/Unit3.h
View File

@ -38,7 +38,9 @@ private:
public: public:
enum { dimension = 2 }; enum {
dimension = 2
};
/// @name Constructors /// @name Constructors
/// @{ /// @{
@ -65,8 +67,8 @@ public:
} }
/// Named constructor from Point3 with optional Jacobian /// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = static Unit3 FromPoint3(const Point3& point, //
boost::none); OptionalJacobian<2, 3> H = boost::none);
/// Random direction, using boost::uniform_on_sphere /// Random direction, using boost::uniform_on_sphere
static Unit3 Random(boost::mt19937 & rng); static Unit3 Random(boost::mt19937 & rng);
@ -99,24 +101,29 @@ public:
Matrix3 skew() const; Matrix3 skew() const;
/// Return unit-norm Point3 /// 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) if (H)
*H = basis(); *H = basis();
return p_; return p_;
} }
/// Return unit-norm Vector
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = basis();
return (p_.vector());
}
/// Return scaled direction as Point3 /// Return scaled direction as Point3
friend Point3 operator*(double s, const Unit3& d) { friend Point3 operator*(double s, const Unit3& d) {
return s * d.p_; return s * d.p_;
} }
/// Signed, vector-valued error between two directions /// Signed, vector-valued error between two directions
Vector2 error(const Unit3& q, Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
OptionalJacobian<2,2> H = boost::none) const;
/// Distance between two directions /// Distance between two directions
double distance(const Unit3& q, double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
OptionalJacobian<1,2> H = boost::none) const;
/// @} /// @}
@ -156,12 +163,12 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(p_);
// homebrew serialize Eigen Matrix // homebrew serialize Eigen Matrix
ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); ar & boost::serialization::make_nvp("B12", (*B_)(0, 1));
ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); ar & boost::serialization::make_nvp("B21", (*B_)(1, 0));
ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); ar & boost::serialization::make_nvp("B22", (*B_)(1, 1));
ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); ar & boost::serialization::make_nvp("B31", (*B_)(2, 0));
ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); ar & boost::serialization::make_nvp("B32", (*B_)(2, 1));
} }
/// @} /// @}
@ -169,9 +176,11 @@ private:
}; };
// Define GTSAM traits // Define GTSAM traits
template <> struct traits<Unit3> : public internal::Manifold<Unit3> {}; template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
};
template <> struct traits<const Unit3> : public internal::Manifold<Unit3> {}; template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
};
} // namespace gtsam } // namespace gtsam

125
gtsam/geometry/tests/testOrientedPlane3.cpp Normal file
View File

@ -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 <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
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<OrientedPlane3, Pose3>(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<OrientedPlane3, OrientedPlane3> (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);
}
/* ************************************************************************* */

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

@ -758,7 +758,6 @@ TEST(Pose3 , Invariants) {
check_manifold_invariants(id,T3); check_manifold_invariants(id,T3);
check_manifold_invariants(T2,id); check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T3); check_manifold_invariants(T2,T3);
} }
//****************************************************************************** //******************************************************************************
@ -769,7 +768,6 @@ TEST(Pose3 , LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,T2); CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id); CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3); CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
} }
//****************************************************************************** //******************************************************************************
@ -777,14 +775,15 @@ TEST(Pose3 , ChartDerivatives) {
Pose3 id; Pose3 id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id); 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);
} }
/* ************************************************************************* */ /* ************************************************************************* */

48
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -39,6 +39,14 @@ TEST(Quaternion , Constructor) {
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); 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) { TEST(Quaternion , Local) {
Vector3 z_axis(0, 0, 1); Vector3 z_axis(0, 0, 1);
@ -47,7 +55,7 @@ TEST(Quaternion , Local) {
QuaternionJacobian H1, H2; QuaternionJacobian H1, H2;
Vector3 expected(0, 0, 0.1); Vector3 expected(0, 0, 0.1);
Vector3 actual = traits<Q>::Local(q1, q2, H1, H2); 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 expected = q1 * q2;
Q actual = traits<Q>::Compose(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 expected = q1.inverse() * q2;
Q actual = traits<Q>::Between(q1, 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 expected(Eigen::AngleAxisd(-0.1, z_axis));
Q actual = traits<Q>::Inverse(q1); Q actual = traits<Q>::Inverse(q1);
EXPECT(traits<Q>::Equals(expected,actual)); EXPECT(traits<Q>::Equals(expected, actual));
} }
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Invariants) { 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) { 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) { 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);
} }
//****************************************************************************** //******************************************************************************

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

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

72
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -15,20 +15,82 @@
* @author Alireza Fathi * @author Alireza Fathi
*/ */
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <boost/math/constants/constants.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
#ifdef GTSAM_USE_QUATERNIONS #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 #endif

1
gtsam/linear/NoiseModel.h
View File

@ -886,6 +886,7 @@ namespace gtsam {
typedef noiseModel::Gaussian::shared_ptr SharedGaussian; typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
typedef noiseModel::Constrained::shared_ptr SharedConstrained; typedef noiseModel::Constrained::shared_ptr SharedConstrained;
typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
/// traits /// traits
template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {}; template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};

9
gtsam/linear/linearExceptions.cpp
View File

@ -17,7 +17,7 @@
*/ */
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <gtsam/inference/Symbol.h>
#include <boost/format.hpp> #include <boost/format.hpp>
namespace gtsam { namespace gtsam {
@ -25,16 +25,19 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
const char* IndeterminantLinearSystemException::what() const throw() const char* IndeterminantLinearSystemException::what() const throw()
{ {
if(!description_) if(!description_) {
description_ = String( description_ = String(
"\nIndeterminant linear system detected while working near variable\n" "\nIndeterminant linear system detected while working near variable\n"
+ boost::lexical_cast<String>(j_) + ".\n" + boost::lexical_cast<String>(j_) +
+ " (Symbol: " + boost::lexical_cast<String>(
gtsam::DefaultKeyFormatter(gtsam::Symbol(j_))) + ").\n"
"\n\ "\n\
Thrown when a linear system is ill-posed. The most common cause for this\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\ error is having underconstrained variables. Mathematically, the system is\n\
underdetermined. See the GTSAM Doxygen documentation at\n\ underdetermined. See the GTSAM Doxygen documentation at\n\
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for\n\ http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for\n\
more information."); more information.");
}
return description_->c_str(); return description_->c_str();
} }

2
gtsam/navigation/PreintegrationBase.h
View File

@ -83,7 +83,7 @@ public:
integrationCovariance_(integrationErrorCovariance) {} integrationCovariance_(integrationErrorCovariance) {}
/// methods to access class variables /// methods to access class variables
const bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;} bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;}
const Vector3& deltaPij() const {return deltaPij_;} const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;} const Vector3& deltaVij() const {return deltaVij_;}
const imuBias::ConstantBias& biasHat() const { return biasHat_;} const imuBias::ConstantBias& biasHat() const { return biasHat_;}

54
gtsam/slam/OrientedPlane3Factor.cpp Normal file
View File

@ -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<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol);
}
//***************************************************************************
Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> 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;
}
}
}

95
gtsam/slam/OrientedPlane3Factor.h Normal file
View File

@ -0,0 +1,95 @@
/*
* @file OrientedPlane3Factor.cpp
* @brief OrientedPlane3 Factor class
* @author Alex Trevor
* @date December 22, 2013
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Key.h>
#include <iostream>
namespace gtsam {
/**
* Factor to measure a planar landmark from a given pose
*/
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
Symbol poseSymbol_;
Symbol landmarkSymbol_;
Vector measured_coeffs_;
OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3 > 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<OrientedPlane3> {
protected:
OrientedPlane3 measured_p_; /// measured plane parameters
Key landmarkKey_;
typedef NoiseModelFactor1<OrientedPlane3 > 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<Matrix&> H = boost::none) const;
};
} // gtsam

183
gtsam/slam/tests/testOrientedPlane3Factor.cpp Normal file
View File

@ -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 <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
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<gtsam::Pose3> 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<gtsam::OrientedPlane3>(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<gtsam::Pose3> 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<gtsam::OrientedPlane3>(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<Vector,OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1);
Matrix expectedH2 = numericalDerivative11<Vector,OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2);
Matrix expectedH3 = numericalDerivative11<Vector,OrientedPlane3>(
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);
}
/* ************************************************************************* */