Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues
dellaert
commit
ae1e454ffd
|
|
@ -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 ")
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@ install(FILES
|
|||
GtsamMatlabWrap.cmake
|
||||
GtsamPythonWrap.cmake
|
||||
GtsamTesting.cmake
|
||||
GtsamTestingObsolete.cmake
|
||||
README.html
|
||||
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")
|
||||
|
||||
|
|
|
|||
|
|
@ -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}\"")
|
||||
|
|
|
|||
|
|
@ -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 *)
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -96,14 +96,15 @@ struct traits<QUATERNION_TYPE> {
|
|||
Vector3 omega;
|
||||
|
||||
const double qw = q.w();
|
||||
// See Quaternion-Logmap.nb in doc for Taylor expansions
|
||||
if (qw > NearlyOne) {
|
||||
// Taylor expansion of (angle / s) at 1
|
||||
//return (2 + 2 * (1-qw) / 3) * q.vec();
|
||||
omega = (8. / 3. - 2. / 3. * qw) * q.vec();
|
||||
// (2 + 2 * (1-qw) / 3) * q.vec();
|
||||
omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
|
||||
} else if (qw < NearlyNegativeOne) {
|
||||
// Taylor expansion of (angle / s) at -1
|
||||
//return (-2 - 2 * (1 + qw) / 3) * q.vec();
|
||||
omega = (-8. / 3 + 2. / 3 * qw) * q.vec();
|
||||
// (-2 - 2 * (1 + qw) / 3) * q.vec();
|
||||
omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
|
||||
} else {
|
||||
// Normal, away from zero case
|
||||
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
|
||||
|
|
|
|||
|
|
@ -31,6 +31,10 @@
|
|||
# pragma clang diagnostic pop
|
||||
#endif
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
#include <tbb/mutex.h>
|
||||
#endif
|
||||
|
||||
#include <boost/random/variate_generator.hpp>
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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<Matrix&> 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<class ARCHIVE>
|
||||
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<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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -758,7 +758,6 @@ TEST(Pose3 , Invariants) {
|
|||
check_manifold_invariants(id,T3);
|
||||
check_manifold_invariants(T2,id);
|
||||
check_manifold_invariants(T2,T3);
|
||||
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -769,7 +768,6 @@ TEST(Pose3 , LieGroupDerivatives) {
|
|||
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
|
||||
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -777,14 +775,15 @@ TEST(Pose3 , ChartDerivatives) {
|
|||
Pose3 id;
|
||||
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
|
||||
CHECK_CHART_DERIVATIVES(id,id);
|
||||
CHECK_CHART_DERIVATIVES(id,T2);
|
||||
CHECK_CHART_DERIVATIVES(T2,id);
|
||||
CHECK_CHART_DERIVATIVES(T2,T3);
|
||||
// CHECK_CHART_DERIVATIVES(id,T2);
|
||||
// CHECK_CHART_DERIVATIVES(T2,id);
|
||||
// CHECK_CHART_DERIVATIVES(T2,T3);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(){ //TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
std::cout<<"testPose3 currently disabled!!" << std::endl;
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -39,6 +39,14 @@ TEST(Quaternion , Constructor) {
|
|||
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Logmap) {
|
||||
Q q1(5e-06, 0, 0, 1), q2(-5e-06, 0, 0, -1);
|
||||
Vector3 v1 = traits<Q>::Logmap(q1);
|
||||
Vector3 v2 = traits<Q>::Logmap(q2);
|
||||
EXPECT(assert_equal(v1, v2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Local) {
|
||||
Vector3 z_axis(0, 0, 1);
|
||||
|
|
@ -47,7 +55,7 @@ TEST(Quaternion , Local) {
|
|||
QuaternionJacobian H1, H2;
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = traits<Q>::Local(q1, q2, H1, H2);
|
||||
EXPECT(assert_equal((Vector)expected,actual));
|
||||
EXPECT(assert_equal((Vector )expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -69,7 +77,7 @@ TEST(Quaternion , Compose) {
|
|||
|
||||
Q expected = q1 * q2;
|
||||
Q actual = traits<Q>::Compose(q1, q2);
|
||||
EXPECT(traits<Q>::Equals(expected,actual));
|
||||
EXPECT(traits<Q>::Equals(expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -85,7 +93,7 @@ TEST(Quaternion , Between) {
|
|||
|
||||
Q expected = q1.inverse() * q2;
|
||||
Q actual = traits<Q>::Between(q1, q2);
|
||||
EXPECT(traits<Q>::Equals(expected,actual));
|
||||
EXPECT(traits<Q>::Equals(expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -94,36 +102,36 @@ TEST(Quaternion , Inverse) {
|
|||
Q expected(Eigen::AngleAxisd(-0.1, z_axis));
|
||||
|
||||
Q actual = traits<Q>::Inverse(q1);
|
||||
EXPECT(traits<Q>::Equals(expected,actual));
|
||||
EXPECT(traits<Q>::Equals(expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Invariants) {
|
||||
check_group_invariants(id,id);
|
||||
check_group_invariants(id,R1);
|
||||
check_group_invariants(R2,id);
|
||||
check_group_invariants(R2,R1);
|
||||
check_group_invariants(id, id);
|
||||
check_group_invariants(id, R1);
|
||||
check_group_invariants(R2, id);
|
||||
check_group_invariants(R2, R1);
|
||||
|
||||
check_manifold_invariants(id,id);
|
||||
check_manifold_invariants(id,R1);
|
||||
check_manifold_invariants(R2,id);
|
||||
check_manifold_invariants(R2,R1);
|
||||
check_manifold_invariants(id, id);
|
||||
check_manifold_invariants(id, R1);
|
||||
check_manifold_invariants(R2, id);
|
||||
check_manifold_invariants(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , LieGroupDerivatives) {
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , ChartDerivatives) {
|
||||
CHECK_CHART_DERIVATIVES(id,id);
|
||||
CHECK_CHART_DERIVATIVES(id,R2);
|
||||
CHECK_CHART_DERIVATIVES(R2,id);
|
||||
CHECK_CHART_DERIVATIVES(R2,R1);
|
||||
CHECK_CHART_DERIVATIVES(id, id);
|
||||
CHECK_CHART_DERIVATIVES(id, R2);
|
||||
CHECK_CHART_DERIVATIVES(R2, id);
|
||||
CHECK_CHART_DERIVATIVES(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -697,9 +697,8 @@ TEST(Rot3 , ChartDerivatives) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
// TestResult tr;
|
||||
// return TestRegistry::runAllTests(tr);
|
||||
std::cout << "testRot3 currently disabled!!" << std::endl;
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -15,20 +15,82 @@
|
|||
* @author Alireza Fathi
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Quaternion.h>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
|
||||
// No quaternion only tests
|
||||
//******************************************************************************
|
||||
TEST(Rot3Q , Compare) {
|
||||
using boost::none;
|
||||
|
||||
// We set up expected values with quaternions
|
||||
typedef Quaternion Q;
|
||||
typedef traits<Q> TQ;
|
||||
typedef TQ::ChartJacobian OJ;
|
||||
|
||||
// We check Rot3 expressions
|
||||
typedef Rot3 R;
|
||||
typedef traits<R> TR;
|
||||
|
||||
// Define test values
|
||||
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
|
||||
R R1(q1), R2(q2);
|
||||
|
||||
// Check Compose
|
||||
Q q3 = TQ::Compose(q1, q2, none, none);
|
||||
R R3 = TR::Compose(R1, R2, none, none);
|
||||
EXPECT(assert_equal(R(q3), R3));
|
||||
|
||||
// Check Retract
|
||||
Vector3 v(1e-5, 0, 0);
|
||||
Q q4 = TQ::Retract(q3, v);
|
||||
R R4 = TR::Retract(R3, v);
|
||||
EXPECT(assert_equal(R(q4), R4));
|
||||
|
||||
// Check Between
|
||||
Q q5 = TQ::Between(q3, q4);
|
||||
R R5 = R3.between(R4);
|
||||
EXPECT(assert_equal(R(q5), R5));
|
||||
|
||||
// Check toQuaternion
|
||||
EXPECT(assert_equal(q5, R5.toQuaternion()));
|
||||
|
||||
// Check Logmap
|
||||
Vector3 vQ = TQ::Logmap(q5);
|
||||
Vector3 vR = R::Logmap(R5);
|
||||
EXPECT(assert_equal(vQ, vR));
|
||||
|
||||
// Check Local
|
||||
vQ = TQ::Local(q3, q4);
|
||||
vR = TR::Local(R3, R4);
|
||||
EXPECT(assert_equal(vQ, vR));
|
||||
|
||||
// Check Retract/Local of Compose
|
||||
Vector3 vQ1 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, v)));
|
||||
Vector3 vR1 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, v)));
|
||||
EXPECT(assert_equal(vQ1, vR1));
|
||||
Vector3 vQ2 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, -v)));
|
||||
Vector3 vR2 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, -v)));
|
||||
EXPECT(assert_equal(vQ2, vR2));
|
||||
EXPECT(assert_equal<Vector3>((vQ1 - vQ2) / 0.2, (vR1 - vR2) / 0.2));
|
||||
|
||||
// Check Compose Derivatives
|
||||
Matrix HQ, HR;
|
||||
HQ = numericalDerivative42<Q, Q, Q, OJ, OJ>(TQ::Compose, q1, q2, none, none);
|
||||
HR = numericalDerivative42<R, R, R, OJ, OJ>(TR::Compose, R1, R2, none, none);
|
||||
EXPECT(assert_equal(HQ, HR));
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -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<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/linear/linearExceptions.h>
|
||||
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <boost/format.hpp>
|
||||
|
||||
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<String>(j_) + ".\n"
|
||||
+ boost::lexical_cast<String>(j_) +
|
||||
+ " (Symbol: " + boost::lexical_cast<String>(
|
||||
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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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_;}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue