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

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

Conflicts:
	.cproject
release/4.3a0
Paul Drews 2015-02-20 00:36:52 -05:00
parent f9ccd23d4a f2fabc18c8
commit b53c26c934
81 changed files with 4296 additions and 1975 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3072
.cproject
View File

File diff suppressed because it is too large Load Diff

1
CMakeLists.txt
View File

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

1
cmake/CMakeLists.txt
View File

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

7
cmake/GtsamTesting.cmake
View File

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

3
examples/RangeISAMExample_plaza2.cpp
View File

@ -82,7 +82,8 @@ vector<RangeTriple> readTriples() {
ifstream is(data_file.c_str());
while (is) {
double t, sender, receiver, range;
double t, sender, range;
size_t receiver;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}

121
examples/SFMExampleExpressions_bal.cpp Normal file
View File

@ -0,0 +1,121 @@
/* ----------------------------------------------------------------------------
* 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 SFMExampleExpressions_bal.cpp
* @brief A structure-from-motion example done with Expressions
* @author Frank Dellaert
* @date January 2015
*/
/**
* This is the Expression version of SFMExample
* See detailed description of headers there, this focuses on explaining the AD part
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <vector>
using namespace std;
using namespace gtsam;
using namespace noiseModel;
using symbol_shorthand::C;
using symbol_shorthand::P;
// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc > 1)
filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
readBAL(filename, mydata);
cout
<< boost::format("read %1% tracks on %2% cameras\n")
% mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
ExpressionFactorGraph graph;
// Here we don't use a PriorFactor but directly the ExpressionFactor class
// First, we create an expression to the pose from the first camera
Expression<SfM_Camera> camera0_(C(0));
// Then, to get its pose:
Pose3_ pose0_(&SfM_Camera::getPose, camera0_);
// Finally, we say it should be equal to first guess
graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
noiseModel::Isotropic::Sigma(6, 0.1));
// similarly, we create a prior on the first point
Point3_ point0_(P(0));
graph.addExpressionFactor(point0_, mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1));
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
1.0); // one pixel in u and v
// Simulated measurements from each camera pose, adding them to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// Leaf expression for i^th camera
Expression<SfM_Camera> camera_(C(i));
// Below an expression for the prediction of the measurement:
Point2_ predict_ = project2<SfM_Camera>(camera_, point_);
// Again, here we use an ExpressionFactor
graph.addExpressionFactor(predict_, uv, noise);
}
j += 1;
}
// Create initial estimate
Values initial;
size_t i = 0;
j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;
try {
LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer lm(graph, initial, params);
result = lm.optimize();
} catch (exception& e) {
cout << e.what();
}
cout << "final error: " << graph.error(result) << endl;
return 0;
}
/* ************************************************************************* */

2
examples/SFMExample_bal.cpp
View File

@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
// Load the SfM data from file
SfM_data mydata;
assert(readBAL(filename, mydata));
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph

8
examples/TimeTBB.cpp
View File

@ -134,7 +134,7 @@ map<int, double> testWithMemoryAllocation()
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
tbb::tick_count t1 = tbb::tick_count::now();
cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[grainSize] = (t1 - t0).seconds();
timingResults[(int)grainSize] = (t1 - t0).seconds();
}
return timingResults;
@ -152,9 +152,9 @@ int main(int argc, char* argv[])
BOOST_FOREACH(size_t n, numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init(n);
results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[n].grainSizesWithAllocation = testWithMemoryAllocation();
tbb::task_scheduler_init init((int)n);
results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
cout << endl;
}

79
gtsam.h
View File

@ -629,28 +629,13 @@ class Cal3_S2 {
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
class Cal3DS2 {
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
Cal3DS2(Vector v);
Cal3DS2_Base();
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// TODO: D2d functions that start with an uppercase letter
// Standard Interface
double fx() const;
@ -658,14 +643,66 @@ class Cal3DS2 {
double skew() const;
double px() const;
double py() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
double k1() const;
double k2() const;
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
Cal3DS2(Vector v);
// Testable
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3Unified.h>
virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3Unified();
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
Cal3Unified(Vector v);
// Testable
bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
// Standard Interface
double xi() const;
gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3Unified retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
class Cal3_S2Stereo {
// Standard Constructors
Cal3_S2Stereo();

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

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

15
gtsam/base/testLie.h
View File

@ -38,22 +38,23 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
// Inverse
OJ none;
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
EXPECT(assert_equal<G>(t1.inverse(),T::Inverse(t1, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1)));
EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
// Compose
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
EXPECT(assert_equal<G>(t1 * t2,T::Compose(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
// Between
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
EXPECT(assert_equal<G>(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
}
// Do a comprehensive test of Lie Group Chart derivatives
template<typename G>
void testChartDerivatives(TestResult& result_, const std::string& name_,
@ -61,18 +62,18 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
Matrix H1, H2;
typedef traits<G> T;
typedef typename G::TangentVector V;
typedef typename T::TangentVector V;
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Retract
OJ none;
V w12 = T::Local(t1, t2);
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
EXPECT(assert_equal<G>(t2, T::Retract(t1,w12, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
// Local
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
}

3
gtsam/geometry/Cal3Bundler.h
View File

@ -174,4 +174,7 @@ private:
template<>
struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
template<>
struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
} // namespace gtsam

17
gtsam/geometry/Cal3DS2.h
View File

@ -68,7 +68,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@ -89,10 +89,20 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Base> clone() const {
return boost::shared_ptr<Base>(new Cal3DS2(*this));
}
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{
@ -112,5 +122,8 @@ private:
template<>
struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
template<>
struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
}

28
gtsam/geometry/Cal3DS2_Base.h
View File

@ -45,9 +45,6 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
Matrix3 K() const ;
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
Vector9 vector() const ;
/// @name Standard Constructors
/// @{
@ -59,6 +56,8 @@ public:
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
virtual ~Cal3DS2_Base() {}
/// @}
/// @name Advanced Constructors
/// @{
@ -70,7 +69,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
@ -106,6 +105,15 @@ public:
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/// return calibration matrix -- not really applicable
Matrix3 K() const;
/// return distortion parameter vector
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
/// Return all parameters as a vector
Vector9 vector() const;
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
@ -126,9 +134,19 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix29 D2d_calibration(const Point2& p) const ;
private:
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
}
/// @}
private:
/// @name Advanced Interface
/// @{

13
gtsam/geometry/Cal3Unified.h
View File

@ -50,9 +50,8 @@ private:
double xi_; // mirror parameter
public:
enum { dimension = 10 };
Vector10 vector() const ;
enum { dimension = 10 };
/// @name Standard Constructors
/// @{
@ -77,7 +76,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
@ -125,6 +124,11 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 10; } //TODO: make a final dimension variable
/// Return all parameters as a vector
Vector10 vector() const ;
/// @}
private:
/** Serialization function */
@ -142,5 +146,8 @@ private:
template<>
struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
template<>
struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
}

3
gtsam/geometry/Cal3_S2.h
View File

@ -226,4 +226,7 @@ private:
template<>
struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
template<>
struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
} // \ namespace gtsam

4
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -158,4 +158,8 @@ namespace gtsam {
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
template<>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam

3
gtsam/geometry/CalibratedCamera.h
View File

@ -204,5 +204,8 @@ private:
template<>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
template<>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
}

3
gtsam/geometry/EssentialMatrix.h
View File

@ -201,5 +201,8 @@ private:
template<>
struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
template<>
struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
} // gtsam

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

14
gtsam/geometry/PinholeCamera.h
View File

@ -151,11 +151,20 @@ public:
return pose_;
}
/// return pose
/// return pose, constant version
inline const Pose3& pose() const {
return pose_;
}
/// return pose, with derivative
inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
}
return pose_;
}
/// return calibration
inline Calibration& calibration() {
return K_;
@ -499,4 +508,7 @@ private:
template<typename Calibration>
struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
template<typename Calibration>
struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
} // \ gtsam

3
gtsam/geometry/Point3.h
View File

@ -197,4 +197,7 @@ namespace gtsam {
template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
}

3
gtsam/geometry/Pose2.h
View File

@ -293,5 +293,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
template<>
struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
template<>
struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
} // namespace gtsam

4
gtsam/geometry/Pose3.h
View File

@ -325,4 +325,8 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
template<>
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
template<>
struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
} // namespace gtsam

98
gtsam/geometry/Quaternion.h
View File

@ -15,8 +15,11 @@
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -37,19 +40,6 @@ struct traits<QUATERNION_TYPE> {
return Q::Identity();
}
static Q Compose(const Q &g, const Q & h) {
return g * h;
}
static Q Between(const Q &g, const Q & h) {
Q d = g.inverse() * h;
return d;
}
static Q Inverse(const Q &g) {
return g.inverse();
}
/// @}
/// @name Basic manifold traits
/// @{
@ -62,41 +52,36 @@ struct traits<QUATERNION_TYPE> {
/// @}
/// @name Lie group traits
/// @{
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
if (Hg)
*Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? )
static Q Compose(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
if (Hg) *Hg = h.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3;
return g * h;
}
static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
static Q Between(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
Q d = g.inverse() * h;
if (Hg)
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) )
if (Hg) *Hg = -d.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3;
return d;
}
static Q Inverse(const Q &g, ChartJacobian H) {
if (H)
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
static Q Inverse(const Q &g,
ChartJacobian H = boost::none) {
if (H) *H = -g.toRotationMatrix();
return g.inverse();
}
/// Exponential map, simply be converting omega to axis/angle representation
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) {
if (omega.isZero())
return Q::Identity();
if(H) *H = SO3::ExpmapDerivative(omega);
if (omega.isZero()) return Q::Identity();
else {
_Scalar angle = omega.norm();
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
}
if (H) CONCEPT_NOT_IMPLEMENTED;
}
/// We use our own Logmap, as there is a slight bug in Eigen
@ -106,43 +91,56 @@ struct traits<QUATERNION_TYPE> {
// define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10;
NearlyNegativeOne = -1.0 + 1e-10;
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();
return (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();
return (-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);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
return (angle / s) * q.vec();
angle += twoPi;
omega = (angle / s) * q.vec();
}
if (H) CONCEPT_NOT_IMPLEMENTED;
if(H) *H = SO3::LogmapDerivative(omega);
return omega;
}
/// @}
/// @name Manifold traits
/// @{
static TangentVector Local(const Q& origin, const Q& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return Logmap(Between(origin, other, Horigin, Hother));
// TODO: incorporate Jacobian of Logmap
static TangentVector Local(const Q& g, const Q& h,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
Q b = Between(g, h, H1, H2);
Matrix3 D_v_b;
TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
if (H1) *H1 = D_v_b * (*H1);
if (H2) *H2 = D_v_b * (*H2);
return v;
}
static Q Retract(const Q& origin, const TangentVector& v,
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return Compose(origin, Expmap(v), Horigin, Hv);
// TODO : incorporate Jacobian of Expmap
static Q Retract(const Q& g, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
Matrix3 D_h_v;
Q b = Expmap(v,H2 ? &D_h_v : 0);
Q h = Compose(g, b, H1, H2);
if (H2) *H2 = (*H2) * D_h_v;
return h;
}
/// @}
@ -150,9 +148,9 @@ struct traits<QUATERNION_TYPE> {
/// @{
static void Print(const Q& q, const std::string& str = "") {
if (str.size() == 0)
std::cout << "Eigen::Quaternion: ";
std::cout << "Eigen::Quaternion: ";
else
std::cout << str << " ";
std::cout << str << " ";
std::cout << q.vec().transpose() << std::endl;
}
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {

3
gtsam/geometry/Rot2.h
View File

@ -210,4 +210,7 @@ namespace gtsam {
template<>
struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
template<>
struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
} // gtsam

51
gtsam/geometry/Rot3.cpp
View File

@ -19,6 +19,7 @@
*/
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
@ -149,57 +150,13 @@ Vector Rot3::quaternion() const {
}
/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
double theta = x.norm(); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = x^, normalized by normx
const Matrix3 Y = skewSymmetric(x) / theta;
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
#endif
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
return SO3::ExpmapDerivative(x);
}
/* ************************************************************************* */
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
double theta = x.norm();
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
return SO3::LogmapDerivative(x);
}
/* ************************************************************************* */

6
gtsam/geometry/Rot3.h
View File

@ -205,9 +205,10 @@ namespace gtsam {
return Rot3();
}
/** compose two rotations */
/// Syntatic sugar for composing two rotations
Rot3 operator*(const Rot3& R2) const;
/// inverse of a rotation, TODO should be different for M/Q
Rot3 inverse() const {
return Rot3(Matrix3(transpose()));
}
@ -460,5 +461,8 @@ namespace gtsam {
template<>
struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
}

62
gtsam/geometry/Rot3M.cpp
View File

@ -23,6 +23,7 @@
#ifndef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
@ -111,25 +112,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
// get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2);
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG
double l_n = wwTxx + wwTyy + wwTzz;
if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double swx = wx * s, swy = wy * s, swz = wz * s;
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz;
return Rot3(
c + C00, -swz + C01, swy + C02,
swz + C01, c + C11, -swx + C12,
-swy + C02, swx + C12, c + C22);
return SO3::Rodrigues(w,theta);
}
/* ************************************************************************* */
@ -156,46 +139,7 @@ Point3 Rot3::rotate(const Point3& p,
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
static const double PI = boost::math::constants::pi<double>();
const Matrix3& rot = R.rot_;
// Get trace(R)
double tr = rot.trace();
Vector3 thetaR;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr+1.0) < 1e-10) {
if(std::abs(rot(2,2)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(2,2) )) *
Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
else if(std::abs(rot(1,1)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(1,1))) *
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) *
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
} else {
double magnitude;
double tr_3 = tr-3.0; // always negative
if (tr_3<-1e-7) {
double theta = acos((tr-1.0)/2.0);
magnitude = theta/(2.0*sin(theta));
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0;
}
thetaR = magnitude*Vector3(
rot(2,1)-rot(1,2),
rot(0,2)-rot(2,0),
rot(1,0)-rot(0,1));
}
if(H) *H = Rot3::LogmapDerivative(thetaR);
return thetaR;
return SO3::Logmap(R.rot_,H);
}
/* ************************************************************************* */

40
gtsam/geometry/Rot3Q.cpp
View File

@ -81,28 +81,13 @@ namespace gtsam {
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return QuaternionChart::Expmap(theta,w);
return Quaternion(Eigen::AngleAxis<double>(theta, w));
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse());
}
/* ************************************************************************* */
// TODO: Could we do this? It works in Rot3M but not here, probably because
// here we create an intermediate value by calling matrix()
@ -111,14 +96,6 @@ namespace gtsam {
return matrix().transpose();
}
/* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return inverse() * R2;
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
@ -131,18 +108,21 @@ namespace gtsam {
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if(H) *H = Rot3::LogmapDerivative(thetaR);
return QuaternionChart::Logmap(R.quaternion_);
return traits<Quaternion>::Logmap(R.quaternion_, H);
}
/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
return compose(Expmap(omega));
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Expmap(omega, H);
else throw std::runtime_error("Rot3::Retract: unknown mode");
}
/* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
return Logmap(between(t2));
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Logmap(R, H);
else throw std::runtime_error("Rot3::Local: unknown mode");
}
/* ************************************************************************* */

92
gtsam/geometry/SO3.cpp
View File

@ -20,9 +20,12 @@
#include <gtsam/base/concepts.h>
#include <cmath>
using namespace std;
namespace gtsam {
SO3 Rodrigues(const double& theta, const Vector3& axis) {
/* ************************************************************************* */
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
using std::cos;
using std::sin;
@ -46,27 +49,25 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
}
/// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Eigen::Ref<const Vector3>& omega,
SO3 SO3::Expmap(const Vector3& omega,
ChartJacobian H) {
if (H)
CONCEPT_NOT_IMPLEMENTED;
*H = ExpmapDerivative(omega);
if (omega.isZero())
return SO3::Identity();
return Identity();
else {
double angle = omega.norm();
return Rodrigues(angle, omega / angle);
return Rodrigues(omega / angle, angle);
}
}
/* ************************************************************************* */
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
using std::sqrt;
using std::sin;
if (H)
CONCEPT_NOT_IMPLEMENTED;
// note switch to base 1
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
@ -75,16 +76,18 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// Get trace(R)
double tr = R.trace();
Vector3 omega;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;
double tr_3 = tr - 3.0; // always negative
@ -96,9 +99,74 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
}
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}
if(H) *H = LogmapDerivative(omega);
return omega;
}
/* ************************************************************************* */
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm(); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = omega^, normalized by normx
const Matrix3 Y = skewSymmetric(omega) / theta;
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
#endif
}
/* ************************************************************************* */
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm();
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
}
/* ************************************************************************* */
} // end namespace gtsam

75
gtsam/geometry/SO3.h
View File

@ -30,15 +30,21 @@ namespace gtsam {
* We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it...
*/
class SO3: public Matrix3, public LieGroup<SO3,3> {
class GTSAM_EXPORT SO3: public Matrix3, public LieGroup<SO3, 3> {
protected:
public:
enum { dimension=3 };
enum {
dimension = 3
};
/// @name Constructors
/// @{
/// Constructor from AngleAxisd
SO3() : Matrix3(I_3x3) {
SO3() :
Matrix3(I_3x3) {
}
/// Constructor from Eigen Matrix
@ -52,6 +58,13 @@ public:
Matrix3(angleAxis) {
}
/// Static, named constructor TODO think about relation with above
static SO3 Rodrigues(const Vector3& axis, double theta);
/// @}
/// @name Testable
/// @{
void print(const std::string& s) const {
std::cout << s << *this << std::endl;
}
@ -60,31 +73,67 @@ public:
return equal_with_abs_tol(*this, R, tol);
}
static SO3 identity() { return I_3x3; }
SO3 inverse() const { return this->Matrix3::inverse(); }
/// @}
/// @name Group
/// @{
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega, ChartJacobian H = boost::none);
/// identity rotation for group operation
static SO3 identity() {
return I_3x3;
}
/// inverse of a rotation = transpose
SO3 inverse() const {
return this->Matrix3::inverse();
}
/// @}
/// @name Lie Group
/// @{
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
*/
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
Matrix3 AdjointMap() const { return *this; }
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& omega);
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& omega);
Matrix3 AdjointMap() const {
return *this;
}
// Chart at origin
struct ChartAtOrigin {
static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) {
return Expmap(v,H);
static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
return Expmap(omega, H);
}
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
return Logmap(R,H);
return Logmap(R, H);
}
};
using LieGroup<SO3,3>::inverse;
using LieGroup<SO3, 3>::inverse;
/// @}
};
template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
struct traits<SO3> : public internal::LieGroupTraits<SO3> {
};
template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
};
} // end namespace gtsam

2
gtsam/geometry/StereoCamera.h
View File

@ -149,4 +149,6 @@ private:
template<>
struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
template<>
struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {};
}

3
gtsam/geometry/StereoPoint2.h
View File

@ -176,4 +176,7 @@ namespace gtsam {
template<>
struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
template<>
struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
}

13
gtsam/geometry/Unit3.cpp
View File

@ -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();

49
gtsam/geometry/Unit3.h
View File

@ -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,23 +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> {
};
} // namespace gtsam

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

@ -0,0 +1,119 @@
/* ----------------------------------------------------------------------------
* 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/OrientedPlane3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
using namespace gtsam;
using namespace std;
using boost::none;
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,
none, 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, none, none), pose);
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1,
none);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
}
{
expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3>(
boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane);
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, 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);
}
/* ************************************************************************* */

17
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -51,8 +51,21 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */
TEST( PinholeCamera, constructor)
{
EXPECT(assert_equal( camera.calibration(), K));
EXPECT(assert_equal( camera.pose(), pose));
EXPECT(assert_equal( K, camera.calibration()));
EXPECT(assert_equal( pose, camera.pose()));
}
//******************************************************************************
TEST(PinholeCamera, Pose) {
Matrix actualH;
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
/* ************************************************************************* */

8
gtsam/geometry/tests/testPose2.cpp
View File

@ -145,7 +145,7 @@ TEST(Pose2, expmap0d) {
/* ************************************************************************* */
// test case for screw motion in the plane
namespace screw {
namespace screwPose2 {
double w=0.3;
Vector xi = (Vector(3) << 0.0, w, w).finished();
Rot2 expectedR = Rot2::fromAngle(w);
@ -155,9 +155,9 @@ namespace screw {
TEST(Pose2, expmap_c)
{
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
EXPECT(assert_equal(screwPose2::expected, expm<Pose2>(screwPose2::xi),1e-6));
EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),1e-6));
EXPECT(assert_equal(screwPose2::xi, Pose2::Logmap(screwPose2::expected),1e-6));
}
/* ************************************************************************* */

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

@ -109,7 +109,7 @@ TEST(Pose3, expmap_b)
/* ************************************************************************* */
// test case for screw motion in the plane
namespace screw {
namespace screwPose3 {
double a=0.3, c=cos(a), s=sin(a), w=0.3;
Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished();
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
@ -121,24 +121,24 @@ namespace screw {
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(Pose3, expmap_c_full)
{
EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screwPose3::expected, expm<Pose3>(screwPose3::xi),1e-6));
EXPECT(assert_equal(screwPose3::expected, Pose3::Expmap(screwPose3::xi),1e-6));
}
/* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose3, Adjoint_full)
{
Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse();
Vector xiprime = T.Adjoint(screw::xi);
Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse();
Vector xiprime = T.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse();
Vector xiprime2 = T2.Adjoint(screw::xi);
Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse();
Vector xiprime2 = T2.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse();
Vector xiprime3 = T3.Adjoint(screw::xi);
Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse();
Vector xiprime3 = T3.Adjoint(screwPose3::xi);
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
}
@ -634,9 +634,9 @@ TEST( Pose3, unicycle )
/* ************************************************************************* */
TEST( Pose3, adjointMap) {
Matrix res = Pose3::adjointMap(screw::xi);
Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2));
Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5));
Matrix res = Pose3::adjointMap(screwPose3::xi);
Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
Matrix Z3 = zeros(3,3);
Matrix6 expected;
expected << wh, Z3, vh, wh;
@ -704,13 +704,13 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
}
TEST( Pose3, adjoint) {
Vector expected = testDerivAdjoint(screw::xi, screw::xi);
Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi);
Matrix actualH;
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH);
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screw::xi, screw::xi, 1e-5);
testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5));
@ -758,7 +758,6 @@ TEST(Pose3 , Invariants) {
check_manifold_invariants(id,T3);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T3);
}
//******************************************************************************
@ -769,19 +768,22 @@ TEST(Pose3 , LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
}
//******************************************************************************
TEST(Pose3 , ChartDerivatives) {
Pose3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T3);
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);
}
}
/* ************************************************************************* */
int main(){ TestResult tr; return TestRegistry::runAllTests(tr);}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -17,6 +17,8 @@
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -38,11 +40,11 @@ TEST(Quaternion , Constructor) {
}
//******************************************************************************
TEST(Quaternion , Invariants) {
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
check_group_invariants(q1, q2);
check_manifold_invariants(q1, q2);
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));
}
//******************************************************************************
@ -53,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));
}
//******************************************************************************
@ -74,47 +76,62 @@ TEST(Quaternion , Compose) {
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1 * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Compose(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
Q actual = traits<Q>::Compose(q1, q2);
EXPECT(traits<Q>::Equals(expected, actual));
}
//******************************************************************************
Vector3 z_axis(0, 0, 1);
Q id(Eigen::AngleAxisd(0, z_axis));
Q R1(Eigen::AngleAxisd(1, z_axis));
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
//******************************************************************************
TEST(Quaternion , Between) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.2, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1.inverse() * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Between(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
Q actual = traits<Q>::Between(q1, q2);
EXPECT(traits<Q>::Equals(expected, actual));
}
//******************************************************************************
TEST(Quaternion , Inverse) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.1, z_axis));
Q expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH;
Q actual = traits<Q>::Inverse(q1, actualH);
EXPECT(traits<Q>::Equals(expected,actual));
Q actual = traits<Q>::Inverse(q1);
EXPECT(traits<Q>::Equals(expected, actual));
}
Matrix numericalH = numericalDerivative11(traits<Q>::Inverse, q1);
EXPECT(assert_equal(numericalH,actualH));
//******************************************************************************
TEST(Quaternion , Invariants) {
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);
}
//******************************************************************************
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);
}
//******************************************************************************
TEST(Quaternion , ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, R2);
CHECK_CHART_DERIVATIVES(R2, id);
CHECK_CHART_DERIVATIVES(R2, R1);
}
//******************************************************************************

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

@ -663,12 +663,13 @@ TEST(Rot3 , Invariants) {
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_group_invariants(T1,T2);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
check_manifold_invariants(T1,T2);
}
//******************************************************************************
@ -678,18 +679,20 @@ TEST(Rot3 , LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T1,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Rot3 , ChartDerivatives) {
Rot3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
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(T1,T2);
CHECK_CHART_DERIVATIVES(T2,T1);
}
}
/* ************************************************************************* */

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

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

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

@ -66,16 +66,15 @@ TEST(SO3 , Invariants) {
check_manifold_invariants(id,R1);
check_manifold_invariants(R2,id);
check_manifold_invariants(R2,R1);
}
//******************************************************************************
//TEST(SO3 , 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);
//}
TEST(SO3 , 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);
}
//******************************************************************************
TEST(SO3 , ChartDerivatives) {

6
gtsam/geometry/tests/testSerializationGeometry.cpp
View File

@ -59,7 +59,7 @@ static StereoCamera cam2(pose3, cal4ptr);
static StereoPoint2 spt(1.0, 2.0, 3.0);
/* ************************************************************************* */
TEST (Serialization, text_geometry) {
TEST_DISABLED (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
@ -84,7 +84,7 @@ TEST (Serialization, text_geometry) {
}
/* ************************************************************************* */
TEST (Serialization, xml_geometry) {
TEST_DISABLED (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
@ -108,7 +108,7 @@ TEST (Serialization, xml_geometry) {
}
/* ************************************************************************* */
TEST (Serialization, binary_geometry) {
TEST_DISABLED (Serialization, binary_geometry) {
EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));

66
gtsam/linear/HessianFactor.cpp
View File

@ -433,7 +433,8 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
}
/* ************************************************************************* */
void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) {
void HessianFactor::updateATA(const JacobianFactor& update,
const Scatter& scatter) {
// This function updates 'combined' with the information in 'update'.
// 'scatter' maps variables in the update factor to slots in the combined
@ -441,52 +442,47 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
gttic(updateATA);
if(update.rows() > 0)
{
// First build an array of slots
gttic(slots);
FastVector<DenseIndex> slots(update.size());
DenseIndex slot = 0;
BOOST_FOREACH(Key j, update) {
slots[slot] = scatter.at(j).slot;
++ slot;
}
gttoc(slots);
if (update.rows() > 0) {
gttic(whiten);
// Whiten the factor if it has a noise model
boost::optional<JacobianFactor> _whitenedFactor;
const JacobianFactor* whitenedFactor;
if(update.get_model())
{
if(update.get_model()->isConstrained())
throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");
const JacobianFactor* whitenedFactor = &update;
if (update.get_model()) {
if (update.get_model()->isConstrained())
throw invalid_argument(
"Cannot update HessianFactor from JacobianFactor with constrained noise model");
_whitenedFactor = update.whiten();
whitenedFactor = &(*_whitenedFactor);
}
else
{
whitenedFactor = &update;
}
gttoc(whiten);
const VerticalBlockMatrix& updateBlocks = whitenedFactor->matrixObject();
// A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below
const VerticalBlockMatrix& A = whitenedFactor->matrixObject();
// N is number of variables in HessianFactor, n in JacobianFactor
DenseIndex N = this->info_.nBlocks() - 1, n = A.nBlocks() - 1;
// First build an array of slots
gttic(slots);
FastVector<DenseIndex> slots(n + 1);
DenseIndex slot = 0;
BOOST_FOREACH(Key j, update)
slots[slot++] = scatter.at(j).slot;
slots[n] = N;
gttoc(slots);
// Apply updates to the upper triangle
gttic(update);
DenseIndex nrInfoBlocks = this->info_.nBlocks(), nrUpdateBlocks = updateBlocks.nBlocks();
for(DenseIndex j2 = 0; j2 < nrUpdateBlocks; ++j2) { // Horizontal block of Hessian
DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2];
assert(slot2 >= 0 && slot2 <= nrInfoBlocks);
for(DenseIndex j1 = 0; j1 <= j2; ++j1) { // Vertical block of Hessian
DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1];
assert(slot1 >= 0 && slot1 < nrInfoBlocks);
if(slot1 != slot2)
info_(slot1, slot2).knownOffDiagonal() += updateBlocks(j1).transpose() * updateBlocks(j2);
else
info_(slot1, slot2).selfadjointView().rankUpdate(updateBlocks(j1).transpose());
// Loop over blocks of A, including RHS with j==n
for (DenseIndex j = 0; j <= n; ++j) {
DenseIndex J = slots[j]; // get block in Hessian
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
DenseIndex I = slots[i]; // get block in Hessian
info_(I, J).knownOffDiagonal() += A(i).transpose() * A(j);
}
// Fill diagonal block with Aj'*Aj
info_(J, J).selfadjointView().rankUpdate(A(j).transpose());
}
gttoc(update);
}

1
gtsam/linear/NoiseModel.h
View File

@ -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> {};

5
gtsam/linear/PCGSolver.cpp
View File

@ -84,8 +84,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
// Make the result as Vector form
AtAx = vvAtAx.vector();
AtAx = vvAtAx.vector(keyInfo_.ordering());
}
/*****************************************************************************/
@ -96,7 +95,7 @@ void GaussianFactorGraphSystem::getb(Vector &b) const {
VectorValues vvb = gfg_.gradientAtZero();
// Make the result as Vector form
b = -vvb.vector();
b = -vvb.vector(keyInfo_.ordering());
}
/**********************************************************************************/

9
gtsam/linear/linearExceptions.cpp
View File

@ -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();
}

1
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -148,7 +148,6 @@ TEST( GaussianBayesNet, DeterminantTest )
}
/* ************************************************************************* */
typedef Eigen::Matrix<double,10,1> Vector10;
namespace {
double computeError(const GaussianBayesNet& gbn, const Vector10& values)
{

1
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -175,7 +175,6 @@ TEST(GaussianBayesTree, complicatedMarginal) {
}
/* ************************************************************************* */
typedef Eigen::Matrix<double, 10, 1> Vector10;
namespace {
double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();

4
gtsam/navigation/AHRSFactor.h
View File

@ -26,7 +26,7 @@
namespace gtsam {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
public:
/**
@ -36,7 +36,7 @@ public:
* Can be built incrementally so as to avoid costly integration at time of
* factor construction.
*/
class PreintegratedMeasurements : public PreintegratedRotation {
class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation {
friend class AHRSFactor;

20
gtsam/navigation/ImuFactor.cpp
View File

@ -94,15 +94,27 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc;
if(use2ndOrderIntegration()){
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
preintMeasCov_.block<3,3>(0,3) += temp;
preintMeasCov_.block<3,3>(3,0) += temp.transpose();
}
// F_test and G_test are given as output for testing purposes and are not needed by the factor
if(F_test){ // This in only for testing
(*F_test) << F;
}
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
if(!use2ndOrderIntegration())
F_pos_noiseacc = Z_3x3;
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
}
}

11
gtsam/navigation/PreintegrationBase.h
View File

@ -83,6 +83,7 @@ public:
integrationCovariance_(integrationErrorCovariance) {}
/// methods to access class variables
bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;}
const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;}
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
@ -149,8 +150,14 @@ public:
if(F){
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles;
if(use2ndOrderIntegration_)
F_pos_angles = 0.5 * F_vel_angles * deltaT;
else
F_pos_angles = Z_3x3;
// pos vel angle
*F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
*F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles;// angle
}
@ -353,7 +360,7 @@ public:
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
D_fR_fRrot * ( I_3x3 ), Z_3x3;
D_fR_fRrot, Z_3x3;
}
if(H4) {
H4->resize(9,3);

2
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -56,7 +56,7 @@ Vector updatePreintegratedMeasurementsTest(
if(!use2ndOrderIntegration){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
}
Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);

149
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -65,7 +65,7 @@ Vector updatePreintegratedPosVel(
if(!use2ndOrderIntegration_){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
}
Vector3 deltaVij_new = deltaVij_old + temp;
@ -90,9 +90,9 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
const bool use2ndOrderIntegration = false){
ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(),
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity());
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -107,8 +107,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
const list<double>& deltaTs){
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij();
}
@ -117,8 +116,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
const list<double>& deltaTs)
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij();
@ -128,10 +126,9 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
const list<double>& deltaTs){
return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
measuredAccs, measuredOmegas, deltaTs).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
@ -479,21 +476,21 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
@ -528,9 +525,10 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01);
}
bool use2ndOrderIntegration = false;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
@ -547,7 +545,126 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual);
bool use2ndOrderIntegration = false;
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
_1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle =
numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
// Compute expected f_rot wrt angles
Matrix dfr_dangle =
numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
Matrix FexpectedBottom3(3,9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3;
EXPECT(assert_equal(Fexpected, Factual));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6,3);
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
Matrix GexpectedTop6(6,9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle =
numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
deltaRij_old, _1, newDeltaT), newMeasuredOmega);
Matrix GexpectedBottom3(3,9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3,
Z_3x3, accNoiseVar*I_3x3, Z_3x3,
Z_3x3, Z_3x3, omegaNoiseVar*I_3x3;
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
(1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
{
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01);
}
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F

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

@ -0,0 +1,61 @@
/*
* OrientedPlane3Factor.cpp
*
* Created on: Jan 29, 2014
* Author: Natesh Srinivasan
*/
#include "OrientedPlane3Factor.h"
using namespace std;
namespace gtsam {
//***************************************************************************
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) 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;
}
}
}

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

@ -0,0 +1,91 @@
/*
* @file OrientedPlane3Factor.cpp
* @brief OrientedPlane3 Factor class
* @author Alex Trevor
* @date December 22, 2013
*/
#pragma once
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Factor to measure a planar landmark from a given pose
*/
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
Key poseKey_;
Key landmarkKey_;
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 Key& pose, const Key& landmark) :
Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_(
z) {
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}
/// print
virtual void print(const std::string& s = "OrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// evaluateError
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
virtual void print(const std::string& s = "OrientedPlane3DirectionPrior",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) 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

6
gtsam/slam/RegularHessianFactor.h
View File

@ -55,6 +55,12 @@ public:
typedef Eigen::Matrix<double, D, 1> DVector;
mutable std::vector<DVector> y;
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{
throw std::runtime_error(
"RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead");
}
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
// Create a vector of temporary y values, corresponding to rows i

1
gtsam/slam/dataset.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/inference/Symbol.h>

23
gtsam/slam/expressions.h
View File

@ -9,6 +9,7 @@
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
namespace gtsam {
@ -19,7 +20,7 @@ typedef Expression<Point2> Point2_;
typedef Expression<Rot2> Rot2_;
typedef Expression<Pose2> Pose2_;
Point2_ transform_to(const Pose2_& x, const Point2_& p) {
inline Point2_ transform_to(const Pose2_& x, const Point2_& p) {
return Point2_(x, &Pose2::transform_to, p);
}
@ -29,24 +30,36 @@ typedef Expression<Point3> Point3_;
typedef Expression<Rot3> Rot3_;
typedef Expression<Pose3> Pose3_;
Point3_ transform_to(const Pose3_& x, const Point3_& p) {
inline Point3_ transform_to(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transform_to, p);
}
// Projection
typedef Expression<Cal3_S2> Cal3_S2_;
typedef Expression<Cal3Bundler> Cal3Bundler_;
Point2_ project(const Point3_& p_cam) {
inline Point2_ project(const Point3_& p_cam) {
return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
}
Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
template <class CAMERA>
Point2 project4(const CAMERA& camera, const Point3& p,
OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) {
return camera.project2(p, Dcam, Dpoint);
}
template <class CAMERA>
Point2_ project2(const Expression<CAMERA>& camera_, const Point3_& p_) {
return Point2_(project4<CAMERA>, camera_, p_);
}
inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) {
return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
}
Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
return Point2_(project6, x, p, K);
}

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/slam/OrientedPlane3Factor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.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 (OrientedPlane3Factor, lm_translation_error) {
// Tests one pose, two measurements of the landmark that differ in range only.
// Normal along -x, 3m away
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
ISAM2 isam2;
Values new_values;
NonlinearFactorGraph new_graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
Vector sigmas(6);
sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
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);
Vector sigmas3(3);
sigmas3 << 0.1, 0.1, 0.1;
Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
OrientedPlane3Factor test_meas0(test_meas0_mean,
noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas0);
Vector test_meas1_mean(4);
test_meas1_mean << -1.0, 0.0, 0.0, 1.0;
OrientedPlane3Factor test_meas1(test_meas1_mean,
noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas1);
// Optimize
ISAM2Result result = isam2.update(new_graph, new_values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
lm_sym);
// Given two noisy measurements of equal weight, expect result between the two
OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
// *************************************************************************
TEST (OrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
ISAM2 isam2;
Values new_values;
NonlinearFactorGraph new_graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
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;
OrientedPlane3Factor test_meas0(test_meas0_mean,
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;
OrientedPlane3Factor test_meas1(test_meas1_mean,
noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym);
new_graph.add(test_meas1);
// Optimize
ISAM2Result result = isam2.update(new_graph, new_values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
lm_sym);
// Given two noisy measurements of equal weight, expect result between the two
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( OrientedPlane3DirectionPrior, 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 = 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);
}
/* ************************************************************************* */

4
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::Dim())) {
Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) {
assert(model->dim() == 1);
this->fillH();
}
@ -78,7 +78,7 @@ namespace gtsam {
/** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) {
assert((size_t)prior_.size() == mask.size());
assert(model->dim() == (size_t) prior.size());
this->fillH();

4
matlab/+gtsam/covarianceEllipse.m
View File

@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k)
% it is assumed x and y are the first two components of state x
% k is scaling for std deviations, defaults to 1 std
hold on
[e,s] = eig(P(1:2,1:2));
s1 = s(1,1);
s2 = s(2,2);
@ -32,4 +34,4 @@ h = line(ex,ey,'color',color);
y = c(2) + points(2,:);
end
end
end

8
matlab/+gtsam/covarianceEllipse3D.m
View File

@ -1,4 +1,4 @@
function covarianceEllipse3D(c,P)
function sc = covarianceEllipse3D(c,P,scale)
% covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
% Based on Maybeck Vol 1, page 366
% k=2.296 corresponds to 1 std, 68.26% of all probability
@ -6,10 +6,16 @@ function covarianceEllipse3D(c,P)
%
% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
hold on
[e,s] = svd(P);
k = 11.82;
radii = k*sqrt(diag(s));
if exist('scale', 'var')
radii = radii * scale;
end
% generate data for "unrotated" ellipsoid
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);

89
matlab/+gtsam/cylinderSampleProjection.m Normal file
View File

@ -0,0 +1,89 @@
function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders)
% Input:
% Output:
% visiblePoints: data{k} 3D Point in overal point clouds with index k
% Z{k} 2D measurements in overal point clouds with index k
% index {i}{j}
% i: the cylinder index;
% j: the point index on the cylinder;
%
% @Description: Project sampled points on cylinder to camera frame
% @Authors: Zhaoyang Lv
import gtsam.*
camera = SimpleCamera(pose, K);
%% memory allocation
cylinderNum = length(cylinders);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
% Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z <= 0
continue;
end
Z2d = camera.project(sampledPoint3);
% ignore points not visible in the scene
if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y
continue;
end
% ignore points occluded
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
continue;
else
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z2d;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end
end
end

93
matlab/+gtsam/cylinderSampleProjectionStereo.m Normal file
View File

@ -0,0 +1,93 @@
function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
import gtsam.*
%% memory allocation
cylinderNum = length(cylinders);
visiblePoints.data = cell(1);
visiblePoints.Z = cell(1);
visiblePoints.cylinderIdx = cell(1);
visiblePoints.overallIdx = cell(1);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
% For Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z < 0
continue;
end
% measurements
Z.du = K.fx() * K.baseline() / sampledPoint3local.z;
Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
Z.uR = Z.uL + Z.du;
Z.v = K.fy() / sampledPoint3local.z + K.py();
% ignore points not visible in the scene
if Z.uL < 0 || Z.uL >= imageSize.x || ...
Z.uR < 0 || Z.uR >= imageSize.x || ...
Z.v < 0 || Z.v >= imageSize.y
continue;
end
% too small disparity may call indeterminant system exception
if Z.du < 0.6
continue;
end
% ignore points occluded
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
continue;
else
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end
end
end

26
matlab/+gtsam/cylinderSampling.m Normal file
View File

@ -0,0 +1,26 @@
function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
%
% @author: Zhaoyang Lv
import gtsam.*
% calculate the cylinder area
area = 2 * pi * radius * height;
pointsNum = round(area * density);
points3 = cell(pointsNum, 1);
% sample the points
for i = 1:pointsNum
theta = 2 * pi * rand;
x = radius * cos(theta) + baseCentroid.x;
y = radius * sin(theta) + baseCentroid.y;
z = height * rand;
points3{i,1} = Point3([x,y,z]');
end
cylinder.area = area;
cylinder.radius = radius;
cylinder.height = height;
cylinder.Points = points3;
cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
end

10
matlab/+gtsam/plotCamera.m
View File

@ -1,18 +1,20 @@
function plotCamera(pose, axisLength)
hold on
C = pose.translation().vector();
R = pose.rotation().matrix();
xAxis = C+R(:,1)*axisLength;
L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r');
h_x = line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+R(:,2)*axisLength;
L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g');
h_y = line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+R(:,3)*axisLength;
L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b');
h_z = line(L(:,1),L(:,2),L(:,3),'Color','b');
axis equal
end
end

35
matlab/+gtsam/plotCylinderSamples.m Normal file
View File

@ -0,0 +1,35 @@
function plotCylinderSamples(cylinders, options, figID)
% plot the cylinders on the given field
% @author: Zhaoyang Lv
figure(figID);
holdstate = ishold;
hold on
num = size(cylinders, 1);
sampleDensity = 120;
for i = 1:num
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
Z = Z * cylinders{i}.height;
cylinderHandle = surf(X,Y,Z);
set(cylinderHandle, 'FaceAlpha', 0.5);
hold on
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
grid on
if ~holdstate
hold off
end
end

177
matlab/+gtsam/plotFlyingResults.m Normal file
View File

@ -0,0 +1,177 @@
function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
% plot the visible points on the cylinders and trajectories
%
% author: Zhaoyang Lv
import gtsam.*
figID = 1;
figure(figID);
set(gcf, 'Position', [80,1,1800,1000]);
%% plot all the cylinders and sampled points
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Height (m)');
h = cameratoolbar('Show');
if options.camera.IS_MONO
h_title = title('Quadrotor Flight Simulation with Monocular Camera');
else
h_title = title('Quadrotor Flight Simulation with Stereo Camera');
end
text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed))
view([30, 30]);
hlight = camlight('headlight');
lighting gouraud
if(options.writeVideo)
videoObj = VideoWriter('Camera_Flying_Example.avi');
videoObj.Quality = 100;
videoObj.FrameRate = options.camera.fps;
open(videoObj);
end
sampleDensity = 120;
cylinderNum = length(cylinders);
h_cylinder = cell(cylinderNum);
for i = 1:cylinderNum
hold on
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
Z = Z * cylinders{i}.height;
h_cylinder{i} = surf(X,Y,Z);
set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
h_cylinder{i}.AmbientStrength = 0.8;
end
%% plot trajectories and points
posesSize = length(poses);
pointSize = length(pts3d);
for i = 1:posesSize
if i > 1
hold on
plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
end
if exist('h_pose_cov', 'var')
delete(h_pose_cov);
end
%plotCamera(poses{i}, 3);
gRp = poses{i}.rotation().matrix(); % rotation from pose to global
C = poses{i}.translation().vector();
axisLength = 3;
xAxis = C+gRp(:,1)*axisLength;
L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+gRp(:,2)*axisLength;
L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+gRp(:,3)*axisLength;
L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b');
pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame
gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale);
if exist('h_point', 'var')
for j = 1:pointSize
delete(h_point{j});
end
end
if exist('h_point_cov', 'var')
for j = 1:pointSize
delete(h_point_cov{j});
end
end
h_point = cell(pointSize, 1);
h_point_cov = cell(pointSize, 1);
for j = 1:pointSize
if ~isempty(pts3d{j}.cov{i})
hold on
h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
pts3d{j}.cov{i}, options.plot.covarianceScale);
end
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
drawnow
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
end
if exist('h_pose_cov', 'var')
delete(h_pose_cov);
end
% wait for two seconds
pause(2);
%% change views angle
for i = 30 : i : 90
view([30, i]);
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
drawnow
end
% changing perspective
%% camera flying through video
camzoom(0.8);
for i = 1 : posesSize
hold on
campos([poses{i}.x, poses{i}.y, poses{i}.z]);
camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
camlight(hlight, 'headlight');
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
drawnow
end
%%close video
if(options.writeVideo)
close(videoObj);
end
end

113
matlab/+gtsam/points2DTrackMonocular.m Normal file
View File

@ -0,0 +1,113 @@
function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization
% @author: Zhaoyang Lv
import gtsam.*
%% create graph
graph = NonlinearFactorGraph;
%% create the noise factors
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
measurementNoiseSigma = 1.0;
measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
points3d = cell(0);
for i = 1:cylinderNum
cylinderPointsNum = length(cylinders{i}.Points);
pointsNum = pointsNum + cylinderPointsNum;
for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0);
points3d{end}.camConstraintIdx = cell(0);
points3d{end}.added = cell(0);
points3d{end}.visiblity = false;
points3d{end}.cov = cell(cameraPosesNum);
end
end
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end
pts3d = cell(cameraPosesNum, 1);
cameraPosesCov = cell(cameraPosesNum, 1);
marginals = Values;
for i = 1:cameraPosesNum
cameraPose = cameraPoses{i};
pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.camConstraintIdx{end+1} = i;
points3d{index}.added{end+1} = false;
if length(points3d{index}.Z) < 2
continue;
else
for k = 1:length(points3d{index}.Z)
if ~points3d{index}.added{k}
graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ...
measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ...
symbol('p', index), K) );
points3d{index}.added{k} = true;
end
end
end
points3d{index}.visiblity = true;
end
pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
initialEstimate.insert(symbol('x', i), pose_i);
marginals = Marginals(graph, initialEstimate);
for j = 1:pointsNum
if points3d{j}.visiblity
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j));
end
end
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i));
end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% Plot the result
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
%% get all the points track information
for i = 1:pointsNum
if ~points3d{i}.visiblity
continue;
end
pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
pts2dTracksMono.Z{end+1} = points3d{i}.Z;
if length(points3d{i}.Z) == 1
%pts2dTracksMono.cov{i} singular matrix
else
pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
end
end

101
matlab/+gtsam/points2DTrackStereo.m Normal file
View File

@ -0,0 +1,101 @@
function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization
%
% @author: Zhaoyang Lv
import gtsam.*
%% create graph
graph = NonlinearFactorGraph;
%% create the noise factors
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05);
cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
points3d = cell(0);
for i = 1:cylinderNum
cylinderPointsNum = length(cylinders{i}.Points);
pointsNum = pointsNum + length(cylinders{i}.Points);
for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0);
points3d{end}.cameraConstraint = cell(0);
points3d{end}.visiblity = false;
points3d{end}.cov = cell(cameraPosesNum);
end
end
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.05*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end
pts3d = cell(cameraPosesNum, 1);
cameraPosesCov = cell(cameraPosesNum, 1);
for i = 1:cameraPosesNum
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
if isempty(pts3d{i}.Z)
continue;
end
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.cameraConstraint{end+1} = i;
points3d{index}.visiblity = true;
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
stereoNoise, symbol('x', i), symbol('p', index), K));
end
pose_i = cameraPoses{i}.retract(poseNoiseSigmas);
initialEstimate.insert(symbol('x', i), pose_i);
%% linearize the graph
marginals = Marginals(graph, initialEstimate);
for j = 1:pointsNum
if points3d{j}.visiblity
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j));
end
end
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));
end
%% Plot the result
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
%% get all the 2d points track information
pts2dTracksStereo.pt3d = cell(0);
pts2dTracksStereo.Z = cell(0);
pts2dTracksStereo.cov = cell(0);
for i = 1:pointsNum
if ~points3d{i}.visiblity
continue;
end
pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
%
% %% plot the result with covariance ellipses
% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
end

179
matlab/gtsam_examples/CameraFlyingExample.m Normal file
View File

@ -0,0 +1,179 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief A camera flying example through a field of cylinder landmarks
% @author Zhaoyang Lv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
clf;
import gtsam.*
% test or run
options.enableTests = false;
%% cylinder options
% the number of cylinders in the field
options.cylinder.cylinderNum = 15; % pls be smaller than 20
% cylinder size
options.cylinder.radius = 3; % pls be smaller than 5
options.cylinder.height = 10;
% point density on cylinder
options.cylinder.pointDensity = 0.1;
%% camera options
% parameters set according to the stereo camera:
% http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
% set up monocular camera or stereo camera
options.camera.IS_MONO = false;
% the field of view of camera
options.camera.fov = 120;
% fps for image
options.camera.fps = 25;
% camera pixel resolution
options.camera.resolution = Point2(752, 480);
% camera horizon
options.camera.horizon = 60;
% camera baseline
options.camera.baseline = 0.05;
% camera focal length
options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
options.camera.fov);
% camera focal baseline
options.camera.fB = options.camera.f * options.camera.baseline;
% camera disparity
options.camera.disparity = options.camera.fB / options.camera.horizon;
% Monocular Camera Calibration
options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2);
% Stereo Camera Calibration
options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
% write video output
options.writeVideo = true;
% the testing field size (unit: meter)
options.fieldSize = Point2([100, 100]');
% camera flying speed (unit: meter / second)
options.speed = 20;
% camera flying height
options.height = 30;
%% ploting options
% display covariance scaling factor, default to be 1.
% The covariance visualization default models 99% of all probablity
options.plot.covarianceScale = 1;
% plot the trajectory covariance
options.plot.DISP_TRAJ_COV = true;
% plot points covariance
options.plot.POINTS_COV = true;
%% This is for tests
if options.enableTests
% test1: visibility test in monocular camera
cylinders{1}.centroid = Point3(30, 50, 5);
cylinders{2}.centroid = Point3(50, 50, 5);
cylinders{3}.centroid = Point3(70, 50, 5);
for i = 1:3
cylinders{i}.radius = 5;
cylinders{i}.height = 10;
cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
end
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK);
pose = camera.pose;
prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
options.camera.resolution, cylinders);
% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
pose, options.camera.resolution, cylinders);
end
%% generate a set of cylinders and point samples on cylinders
cylinderNum = options.cylinder.cylinderNum;
cylinders = cell(cylinderNum, 1);
baseCentroid = cell(cylinderNum, 1);
theta = 0;
i = 1;
while i <= cylinderNum
theta = theta + 2*pi/10;
x = 40 * rand * cos(theta) + options.fieldSize.x/2;
y = 20 * rand * sin(theta) + options.fieldSize.y/2;
baseCentroid{i} = Point2([x, y]');
% prevent two cylinders interact with each other
regenerate = false;
for j = 1:i-1
if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
regenerate = true;
break;
end
end
if regenerate
continue;
end
cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
options.cylinder.height, options.cylinder.pointDensity);
i = i+1;
end
%% generate ground truth camera trajectories: a line
KMono = Cal3_S2(525,525,0,320,240);
cameraPoses = cell(0);
theta = 0;
t = Point3(5, 5, options.height);
i = 0;
while 1
i = i+1;
distance = options.speed / options.camera.fps;
angle = 0.1*pi*(rand-0.5);
inc_t = Point3(distance * cos(angle), ...
distance * sin(angle), 0);
t = t.compose(inc_t);
if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
break;
end
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
% 15, 10]');
camera = SimpleCamera.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.camera.monoK);
cameraPoses{end+1} = camera.pose;
end
%% set up camera and get measurements
if options.camera.IS_MONO
% use Monocular Camera
pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
options.camera.resolution, cylinders);
else
% use Stereo Camera
pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ...
cameraPoses, options, cylinders);
end

7
matlab/gtsam_tests/testCal3Unified.m Normal file
View File

@ -0,0 +1,7 @@
% test Cal3Unified
import gtsam.*;
K = Cal3Unified;
EXPECT('fx',K.fx()==1);
EXPECT('fy',K.fy()==1);

21
matlab/gtsam_tests/test_gtsam.m
View File

@ -1,17 +1,25 @@
% Test runner script - runs each test
% display 'Starting: testPriorFactor'
% testPriorFactor
%% geometry
display 'Starting: testCal3Unified'
testCal3Unified
display 'Starting: testValues'
testValues
%% linear
display 'Starting: testKalmanFilter'
testKalmanFilter
display 'Starting: testJacobianFactor'
testJacobianFactor
display 'Starting: testKalmanFilter'
testKalmanFilter
%% nonlinear
display 'Starting: testValues'
testValues
%% SLAM
display 'Starting: testPriorFactor'
testPriorFactor
%% examples
display 'Starting: testLocalizationExample'
testLocalizationExample
@ -36,6 +44,7 @@ testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
%% MATLAB specific
display 'Starting: testUtilities'
testUtilities

1
matlab/unstable_examples/.gitignore vendored
View File

@ -1 +1,2 @@
*.m~
*.avi

43
tests/testPCGSolver.cpp
View File

@ -91,6 +91,49 @@ TEST( PCGSolver, llt ) {
}
/* ************************************************************************* */
// Test GaussianFactorGraphSystem::multiply and getb
TEST( GaussianFactorGraphSystem, multiply_getb)
{
// Create a Gaussian Factor Graph
GaussianFactorGraph simpleGFG;
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
// Create a dummy-preconditioner and a GaussianFactorGraphSystem
DummyPreconditioner dummyPreconditioner;
KeyInfo keyInfo(simpleGFG);
std::map<Key,Vector> lambda;
dummyPreconditioner.build(simpleGFG, keyInfo, lambda);
GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda);
// Prepare container for each variable
Vector initial, residual, preconditionedResidual, p, actualAp;
initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished();
// Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver
gfgs.residual(initial, residual); /* r = b-Ax */
gfgs.leftPrecondition(residual, preconditionedResidual); /* pr = L^{-1} (b-Ax) */
gfgs.rightPrecondition(preconditionedResidual, p); /* p = L^{-T} pr */
gfgs.multiply(p, actualAp); /* A p */
// Expected value of Ap for the first iteration of this example problem
Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
EXPECT(assert_equal(expectedAp, actualAp, 1e-3));
// Expected value of getb
Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
Vector actualb;
gfgs.getb(actualb);
EXPECT(assert_equal(expectedb, actualb, 1e-3));
}
/* ************************************************************************* */
// Test Dummy Preconditioner
TEST( PCGSolver, dummy )