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

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

Conflicts:
	gtsam/nonlinear/expressionTesting.h
	gtsam/nonlinear/factorTesting.h
release/4.3a0
Frank Dellaert 2015-07-15 23:20:14 -07:00
parent b01eb4e960 07577189d9
commit 8d4e54bc20
307 changed files with 13703 additions and 9777 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3370
.cproject
View File

File diff suppressed because it is too large Load Diff

11
CMakeLists.txt
View File

@ -158,6 +158,12 @@ else()
set(GTSAM_USE_TBB 0) # This will go into config.h
endif()
###############################################################################
# Prohibit Timing build mode in combination with TBB
if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing"))
message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
endif()
###############################################################################
# Find Google perftools
@ -173,6 +179,11 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
# --no-as-needed is required with gcc according to the MKL link advisor
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
endif()
else()
set(GTSAM_USE_EIGEN_MKL 0)
set(EIGEN_USE_MKL_ALL 0)

9
cmake/GtsamMatlabWrap.cmake
View File

@ -367,6 +367,11 @@ endfunction()
# should be installed to all build type toolboxes
function(install_matlab_scripts source_directory patterns)
set(patterns_args "")
set(exclude_patterns "")
if(NOT GTSAM_WRAP_SERIALIZATION)
set(exclude_patterns "testSerialization.m")
endif()
foreach(pattern ${patterns})
list(APPEND patterns_args PATTERN "${pattern}")
endforeach()
@ -381,10 +386,10 @@ function(install_matlab_scripts source_directory patterns)
# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endif()
endfunction()

4
cmake/GtsamTesting.cmake
View File

@ -195,7 +195,9 @@ 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})
if(NOT XCODE_VERSION)
add_dependencies(all.tests ${script_name})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")

1
examples/Data/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
*.txt

2
examples/PlanarSLAMExample.cpp
View File

@ -42,7 +42,7 @@
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.

2
examples/RangeISAMExample_plaza2.cpp
View File

@ -39,7 +39,7 @@
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own

2
examples/SFMExample.cpp
View File

@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.print("Initial Estimates:\n");

2
examples/SFMExampleExpressions.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
// Create perturbed initial
Values initial;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)

19
examples/SFMExample_SmartFactor.cpp
View File

@ -34,6 +34,9 @@ using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
@ -55,12 +58,12 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor());
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
SimpleCamera camera(poses[i], *K);
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
@ -68,7 +71,7 @@ int main(int argc, char* argv[]) {
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
smartfactor->add(measurement, i, measurementNoise, K);
smartfactor->add(measurement, i, measurementNoise);
}
// insert the smart factor in the graph
@ -77,24 +80,24 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results

19
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -30,6 +30,9 @@ using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
@ -51,16 +54,16 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor());
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
SimpleCamera camera(poses[i], *K);
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
smartfactor->add(measurement, i, measurementNoise, K);
smartfactor->add(measurement, i, measurementNoise);
}
// insert the smart factor in the graph
@ -69,18 +72,18 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));
// Create the initial estimate to the solution
Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.insert(i, Camera(poses[i].compose(delta),K));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.

144
examples/SFMExample_bal_COLAMD_METIS.cpp Normal file
View File

@ -0,0 +1,144 @@
/* ----------------------------------------------------------------------------
* 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 SFMExample.cpp
* @brief This file is to compare the ordering performance for COLAMD vs METIS.
* Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
* @author Frank Dellaert, Zhaoyang Lv
*/
// For an explanation of headers, see SFMExample.cpp
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/base/timing.h>
#include <vector>
using namespace std;
using namespace gtsam;
using symbol_shorthand::C;
using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
/* ************************************************************************* */
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
NonlinearFactorGraph graph;
// 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
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.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);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
try {
params_using_METIS.setVerbosity("ERROR");
gttic_(METIS_ORDERING);
params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
gttoc_(METIS_ORDERING);
params_using_COLAMD.setVerbosity("ERROR");
gttic_(COLAMD_ORDERING);
params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
gttoc_(COLAMD_ORDERING);
} catch (exception& e) {
cout << e.what();
}
// expect they have different ordering results
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
cout << "COLAMD and METIS produce the same ordering. "
<< "Problem here!!!" << endl;
}
/* Optimize the graph with METIS and COLAMD and time the results */
Values result_METIS, result_COLAMD;
try {
gttic_(OPTIMIZE_WITH_METIS);
LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
result_METIS = lm_METIS.optimize();
gttoc_(OPTIMIZE_WITH_METIS);
gttic_(OPTIMIZE_WITH_COLAMD);
LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
result_COLAMD = lm_COLAMD.optimize();
gttoc_(OPTIMIZE_WITH_COLAMD);
} catch (exception& e) {
cout << e.what();
}
{ // printing the result
cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
cout << "METIS final error: " << graph.error(result_METIS) << endl;
cout << endl << endl;
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") \
% mydata.number_tracks() % mydata.number_cameras() \
<< endl;
tictoc_print_();
}
return 0;
}
/* ************************************************************************* */

2
examples/SelfCalibrationExample.cpp
View File

@ -82,7 +82,7 @@ int main(int argc, char* argv[]) {
Values initialEstimate;
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));

5
examples/SolverComparer.cpp
View File

@ -32,7 +32,7 @@
*/
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
@ -44,6 +44,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
@ -575,7 +576,7 @@ void runStats()
{
cout << "Gathering statistics..." << endl;
GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
ofstream file;

1
examples/TimeTBB.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/global_includes.h>
#include <gtsam/base/Matrix.h>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <map>

2
examples/VisualISAM2Example.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale

2
examples/VisualISAMExample.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
}
// Intentionally initialize the variables off from the ground truth
Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose

93
gtsam.h
View File

@ -1,4 +1,5 @@
/**
* GTSAM Wrap Module Definition
*
* These are the current classes available through the matlab toolbox interface,
@ -156,7 +157,7 @@ virtual class Value {
size_t dim() const;
};
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
class LieScalar {
// Standard constructors
LieScalar();
@ -185,7 +186,7 @@ class LieScalar {
static Vector Logmap(const gtsam::LieScalar& p);
};
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieVector.h>
class LieVector {
// Standard constructors
LieVector();
@ -217,7 +218,7 @@ class LieVector {
void serialize() const;
};
#include <gtsam/base/LieMatrix_Deprecated.h>
#include <gtsam/base/deprecated/LieMatrix.h>
class LieMatrix {
// Standard constructors
LieMatrix();
@ -330,8 +331,6 @@ class StereoPoint2 {
gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::StereoPoint2 retract(Vector v) const;
Vector localCoordinates(const gtsam::StereoPoint2& p) const;
@ -440,7 +439,7 @@ class Rot3 {
static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft)
static gtsam::Rot3 ypr(double y, double p, double r);
static gtsam::Rot3 quaternion(double w, double x, double y, double z);
static gtsam::Rot3 rodriguez(Vector v);
static gtsam::Rot3 Rodrigues(Vector v);
// Testable
void print(string s) const;
@ -814,7 +813,7 @@ class CalibratedCamera {
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
// Standard Interface
gtsam::Pose3 pose() const;
@ -850,7 +849,7 @@ class PinholeCamera {
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@ -886,7 +885,7 @@ virtual class SimpleCamera {
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@ -1656,12 +1655,12 @@ char symbolChr(size_t key);
size_t symbolIndex(size_t key);
// Default keyformatter
void printKeyList (const gtsam::KeyList& keys);
void printKeyList (const gtsam::KeyList& keys, string s);
void printKeyVector(const gtsam::KeyVector& keys);
void printKeyVector(const gtsam::KeyVector& keys, string s);
void printKeySet (const gtsam::KeySet& keys);
void printKeySet (const gtsam::KeySet& keys, string s);
void PrintKeyList (const gtsam::KeyList& keys);
void PrintKeyList (const gtsam::KeyList& keys, string s);
void PrintKeyVector(const gtsam::KeyVector& keys);
void PrintKeyVector(const gtsam::KeyVector& keys, string s);
void PrintKeySet (const gtsam::KeySet& keys);
void PrintKeySet (const gtsam::KeySet& keys, string s);
#include <gtsam/inference/LabeledSymbol.h>
class LabeledSymbol {
@ -1778,7 +1777,7 @@ class Values {
void swap(gtsam::Values& values);
bool exists(size_t j) const;
gtsam::KeyList keys() const;
gtsam::KeyVector keys() const;
gtsam::VectorValues zeroVectors() const;
@ -1895,8 +1894,6 @@ class KeySet {
class KeyVector {
KeyVector();
KeyVector(const gtsam::KeyVector& other);
KeyVector(const gtsam::KeySet& other);
KeyVector(const gtsam::KeyList& other);
// Note: no print function
@ -2266,7 +2263,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
template<POSE, POINT>
virtual class RangeFactor : gtsam::NoiseModelFactor {
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
@ -2276,20 +2273,16 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
// Commented out by Frank Dec 2014: not poses!
// If needed, we need a RangeFactor that takes a camera, extracts the pose
// Should be easy with Expressions
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>
template<POSE, POINT, ROTATION>
#include <gtsam/sam/BearingFactor.h>
template<POSE, POINT, BEARING>
virtual class BearingFactor : gtsam::NoiseModelFactor {
BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
@ -2297,19 +2290,18 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
#include <gtsam/slam/BearingRangeFactor.h>
template<POSE, POINT, ROTATION>
#include <gtsam/sam/BearingRangeFactor.h>
template<POSE, POINT, BEARING, RANGE>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
pair<ROTATION, double> measured() const;
BearingRangeFactor(size_t poseKey, size_t pointKey,
const BEARING& measuredBearing, const RANGE& measuredRange,
const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
#include <gtsam/slam/ProjectionFactor.h>
@ -2356,18 +2348,31 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
void serialize() const;
};
#include <gtsam/slam/SmartProjectionFactor.h>
class SmartProjectionParams {
SmartProjectionParams();
// TODO(frank): make these work:
// void setLinearizationMode(LinearizationMode linMode);
// void setDegeneracyMode(DegeneracyMode degMode);
void setRankTolerance(double rankTol);
void setEnableEPI(bool enableEPI);
void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(double rankTol);
SmartProjectionPoseFactor();
SmartProjectionPoseFactor(const CALIBRATION* K);
SmartProjectionPoseFactor(const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
const gtsam::noiseModel::Base* noise_i);
// enabling serialization functionality
//void serialize() const;

6
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -58,10 +58,10 @@ add_subdirectory(ceres)
include(GeographicLib/cmake/FindGeographicLib.cmake)
# Set up the option to install GeographicLib
if(GEOGRAPHICLIB_FOUND)
set(install_geographiclib_default OFF)
else()
if(GEOGRAPHICLIB-NOTFOUND)
set(install_geographiclib_default ON)
else()
set(install_geographiclib_default OFF)
endif()
option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default})

3
gtsam/CMakeLists.txt
View File

@ -9,7 +9,10 @@ set (gtsam_subdirs
discrete
linear
nonlinear
sam
sfm
slam
smart
navigation
)

3
gtsam/base/CMakeLists.txt
View File

@ -5,5 +5,8 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base)
file(GLOB base_headers_tree "treeTraversal/*.h")
install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
file(GLOB deprecated_headers "deprecated/*.h")
install(FILES ${deprecated_headers} DESTINATION include/gtsam/base/deprecated)
# Build tests
add_subdirectory(tests)

5
gtsam/base/FastDefaultAllocator.h
View File

@ -17,8 +17,7 @@
*/
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/config.h> // Configuration from CMake
#if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL
# ifdef GTSAM_USE_TBB
@ -85,4 +84,4 @@ namespace gtsam
};
}
}
}

2
gtsam/base/FastMap.h
View File

@ -19,9 +19,9 @@
#pragma once
#include <gtsam/base/FastDefaultAllocator.h>
#include <map>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/map.hpp>
#include <map>
namespace gtsam {

123
gtsam/base/FastSet.h
View File

@ -18,26 +18,22 @@
#pragma once
#include <gtsam/base/FastDefaultAllocator.h>
#include <boost/mpl/has_xxx.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/set.hpp>
#include <set>
#include <iostream>
#include <string>
#include <cmath>
#include <gtsam/base/FastDefaultAllocator.h>
#include <gtsam/base/Testable.h>
BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
#include <functional>
#include <set>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
// This is used internally to allow this container to be Testable even when it
// contains non-testable elements.
template<typename VALUE, class ENABLE = void>
struct FastSetTestableHelper;
/**
* FastSet is a thin wrapper around std::set that uses the boost
* fast_pool_allocator instead of the default STL allocator. This is just a
@ -45,12 +41,16 @@ struct FastSetTestableHelper;
* we've seen that the fast_pool_allocator can lead to speedups of several %.
* @addtogroup base
*/
template<typename VALUE, class ENABLE = void>
class FastSet: public std::set<VALUE, std::less<VALUE>, typename internal::FastDefaultAllocator<VALUE>::type> {
template<typename VALUE>
class FastSet: public std::set<VALUE, std::less<VALUE>,
typename internal::FastDefaultAllocator<VALUE>::type> {
BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> ));
public:
typedef std::set<VALUE, std::less<VALUE>, typename internal::FastDefaultAllocator<VALUE>::type> Base;
typedef std::set<VALUE, std::less<VALUE>,
typename internal::FastDefaultAllocator<VALUE>::type> Base;
/** Default constructor */
FastSet() {
@ -59,23 +59,23 @@ public:
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
explicit FastSet(INPUTITERATOR first, INPUTITERATOR last) :
Base(first, last) {
Base(first, last) {
}
/** Constructor from a iterable container, passes through to base class */
template<typename INPUTCONTAINER>
explicit FastSet(const INPUTCONTAINER& container) :
Base(container.begin(), container.end()) {
Base(container.begin(), container.end()) {
}
/** Copy constructor from another FastSet */
FastSet(const FastSet<VALUE>& x) :
Base(x) {
Base(x) {
}
/** Copy constructor from the base set class */
FastSet(const Base& x) :
Base(x) {
Base(x) {
}
#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
@ -85,7 +85,7 @@ public:
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(x.size() > 0)
Base::insert(x.begin(), x.end());
Base::insert(x.begin(), x.end());
}
#endif
@ -95,17 +95,31 @@ public:
}
/** Handy 'exists' function */
bool exists(const VALUE& e) const { return this->find(e) != this->end(); }
bool exists(const VALUE& e) const {
return this->find(e) != this->end();
}
/** Print to implement Testable */
void print(const std::string& str = "") const { FastSetTestableHelper<VALUE>::print(*this, str); }
/** Print to implement Testable: pretty basic */
void print(const std::string& str = "") const {
for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it)
traits<VALUE>::Print(*it, str);
}
/** Check for equality within tolerance to implement Testable */
bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const { return FastSetTestableHelper<VALUE>::equals(*this, other, tol); }
bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const {
typename Base::const_iterator it1 = this->begin(), it2 = other.begin();
while (it1 != this->end()) {
if (it2 == other.end() || !traits<VALUE>::Equals(*it2, *it2, tol))
return false;
++it1;
++it2;
}
return true;
}
/** insert another set: handy for MATLAB access */
void merge(const FastSet& other) {
Base::insert(other.begin(),other.end());
Base::insert(other.begin(), other.end());
}
private:
@ -117,59 +131,4 @@ private:
}
};
// This is the default Testable interface for *non*Testable elements, which
// uses stream operators.
template<typename VALUE, class ENABLE>
struct FastSetTestableHelper {
typedef FastSet<VALUE> Set;
static void print(const Set& set, const std::string& str) {
std::cout << str << "\n";
for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it)
std::cout << " " << *it << "\n";
std::cout.flush();
}
static bool equals(const Set& set1, const Set& set2, double tol) {
typename Set::const_iterator it1 = set1.begin();
typename Set::const_iterator it2 = set2.begin();
while (it1 != set1.end()) {
if (it2 == set2.end() ||
fabs((double)(*it1) - (double)(*it2)) > tol)
return false;
++it1;
++it2;
}
return true;
}
};
// This is the Testable interface for Testable elements
template<typename VALUE>
struct FastSetTestableHelper<VALUE, typename boost::enable_if<has_print<VALUE> >::type> {
typedef FastSet<VALUE> Set;
static void print(const Set& set, const std::string& str) {
std::cout << str << "\n";
for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it)
it->print(" ");
std::cout.flush();
}
static bool equals(const Set& set1, const Set& set2, double tol) {
typename Set::const_iterator it1 = set1.begin();
typename Set::const_iterator it2 = set2.begin();
while (it1 != set1.end()) {
if (it2 == set2.end() ||
!it1->equals(*it2, tol))
return false;
++it1;
++it2;
}
return true;
}
};
}

46
gtsam/base/FastVector.h
View File

@ -18,12 +18,10 @@
#pragma once
#include <vector>
#include <gtsam/base/FastDefaultAllocator.h>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h>
#include <vector>
namespace gtsam {
@ -35,20 +33,27 @@ namespace gtsam {
* @addtogroup base
*/
template<typename VALUE>
class FastVector: public std::vector<VALUE, typename internal::FastDefaultVectorAllocator<VALUE>::type> {
class FastVector: public std::vector<VALUE,
typename internal::FastDefaultVectorAllocator<VALUE>::type> {
public:
typedef std::vector<VALUE, typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
typedef std::vector<VALUE,
typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
/** Default constructor */
FastVector() {}
FastVector() {
}
/** Constructor from size */
explicit FastVector(size_t size) : Base(size) {}
explicit FastVector(size_t size) :
Base(size) {
}
/** Constructor from size and initial values */
explicit FastVector(size_t size, const VALUE& initial) : Base(size, initial) {}
explicit FastVector(size_t size, const VALUE& initial) :
Base(size, initial) {
}
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
@ -56,33 +61,22 @@ public:
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(first != last)
if (first != last)
Base::assign(first, last);
}
/** Copy constructor from a FastList */
FastVector(const FastList<VALUE>& x) {
if(x.size() > 0)
Base::assign(x.begin(), x.end());
}
/** Copy constructor from a FastSet */
FastVector(const FastSet<VALUE>& x) {
if(x.size() > 0)
Base::assign(x.begin(), x.end());
}
/** Copy constructor from the base class */
FastVector(const Base& x) : Base(x) {}
FastVector(const Base& x) :
Base(x) {
}
/** Copy constructor from a standard STL container */
template<typename ALLOCATOR>
FastVector(const std::vector<VALUE, ALLOCATOR>& x)
{
FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(x.size() > 0)
if (x.size() > 0)
Base::assign(x.begin(), x.end());
}

5
gtsam/base/GenericValue.h
View File

@ -194,6 +194,11 @@ public:
};
// traits
template <typename ValueType>
struct traits<GenericValue<ValueType> >
: public Testable<GenericValue<ValueType> > {};
// define Value::cast here since now GenericValue has been declared
template<typename ValueType>
const ValueType& Value::cast() const {

130
gtsam/base/Group.h
View File

@ -13,16 +13,20 @@
* @file Group.h
*
* @brief Concept check class for variable types with Group properties
* @date Nov 5, 2011
* @date November, 2011
* @author Alex Cunningham
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/base/Testable.h>
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <boost/static_assert.hpp>
#include <utility>
namespace gtsam {
@ -83,21 +87,119 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
&& traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
}
/// Macro to add group traits, additive flavor
#define GTSAM_ADDITIVE_GROUP(GROUP) \
typedef additive_group_tag group_flavor; \
static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \
static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \
static GROUP Inverse(const GROUP &g) { return -g;}
namespace internal {
/// Macro to add group traits, multiplicative flavor
#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \
typedef additive_group_tag group_flavor; \
static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \
static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \
static GROUP Inverse(const GROUP &g) { return g.inverse();}
/// A helper class that implements the traits interface for multiplicative groups.
/// Assumes existence of identity, operator* and inverse method
template<class Class>
struct MultiplicativeGroupTraits {
typedef group_tag structure_category;
typedef multiplicative_group_tag group_flavor;
static Class Identity() { return Class::identity(); }
static Class Compose(const Class &g, const Class & h) { return g * h;}
static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
static Class Inverse(const Class &g) { return g.inverse();}
};
} // \ namespace gtsam
/// Both multiplicative group traits and Testable
template<class Class>
struct MultiplicativeGroup : MultiplicativeGroupTraits<Class>, Testable<Class> {};
/// A helper class that implements the traits interface for additive groups.
/// Assumes existence of identity and three additive operators
template<class Class>
struct AdditiveGroupTraits {
typedef group_tag structure_category;
typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity(); }
static Class Compose(const Class &g, const Class & h) { return g + h;}
static Class Between(const Class &g, const Class & h) { return h - g;}
static Class Inverse(const Class &g) { return -g;}
};
/// Both additive group traits and Testable
template<class Class>
struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
} // namespace internal
/// compose multiple times
template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
compose_pow(const G& g, size_t n) {
if (n == 0) return traits<G>::Identity();
else if (n == 1) return g;
else return traits<G>::Compose(compose_pow(g, n - 1), g);
}
/// Template to construct the direct product of two arbitrary groups
/// Assumes nothing except group structure and Testable from G and H
template<typename G, typename H>
class DirectProduct: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>));
BOOST_CONCEPT_ASSERT((IsGroup<H>));
public:
/// Default constructor yields identity
DirectProduct():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
// Construct from two subgroup elements
DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
// identity
static DirectProduct identity() { return DirectProduct(); }
DirectProduct operator*(const DirectProduct& other) const {
return DirectProduct(traits<G>::Compose(this->first, other.first),
traits<H>::Compose(this->second, other.second));
}
DirectProduct inverse() const {
return DirectProduct(this->first.inverse(), this->second.inverse());
}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename G, typename H>
struct traits<DirectProduct<G, H> > :
internal::MultiplicativeGroupTraits<DirectProduct<G, H> > {};
/// Template to construct the direct sum of two additive groups
/// Assumes existence of three additive operators for both groups
template<typename G, typename H>
class DirectSum: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>)); // TODO(frank): check additive
BOOST_CONCEPT_ASSERT((IsGroup<H>)); // TODO(frank): check additive
const G& g() const { return this->first; }
const H& h() const { return this->second;}
public:
/// Default constructor yields identity
DirectSum():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
// Construct from two subgroup elements
DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
// identity
static DirectSum identity() { return DirectSum(); }
DirectSum operator+(const DirectSum& other) const {
return DirectSum(g()+other.g(), h()+other.h());
}
DirectSum operator-(const DirectSum& other) const {
return DirectSum(g()-other.g(), h()-other.h());
}
DirectSum operator-() const {
return DirectSum(- g(), - h());
}
};
// Define direct sums to be a model of the Additive Group concept
template<typename G, typename H>
struct traits<DirectSum<G, H> > :
internal::AdditiveGroupTraits<DirectSum<G, H> > {};
} // namespace gtsam
/**
* Macros for using the IsGroup

11
gtsam/base/Lie.h
View File

@ -136,8 +136,10 @@ namespace internal {
/// A helper class that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// template<> struct traits<Class> : public internal::LieGroupTraits<Class> {};
/// Assumes existence of: identity, dimension, localCoordinates, retract,
/// and additionally Logmap, Expmap, compose, between, and inverse
template<class Class>
struct LieGroupTraits : Testable<Class> {
struct LieGroupTraits {
typedef lie_group_tag structure_category;
/// @name Group
@ -167,12 +169,10 @@ struct LieGroupTraits : Testable<Class> {
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return origin.retract(v, Horigin, Hv);
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
return Class::Logmap(m, Hm);
}
@ -195,11 +195,12 @@ struct LieGroupTraits : Testable<Class> {
ChartJacobian H = boost::none) {
return m.inverse(H);
}
/// @}
};
/// Both LieGroupTraits and Testable
template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
} // \ namepsace internal
/**

27
gtsam/base/LieMatrix.cpp
View File

@ -1,27 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 LieMatrix.cpp
* @brief A wrapper around Matrix providing Lie compatibility
* @author Richard Roberts and Alex Cunningham
*/
#include <gtsam/base/LieMatrix_Deprecated.h>
namespace gtsam {
/* ************************************************************************* */
void LieMatrix::print(const std::string& name) const {
gtsam::print(matrix(), name);
}
}

4
gtsam/base/LieMatrix.h
View File

@ -11,7 +11,7 @@
/**
* @file LieMatrix.h
* @brief External deprecation warning, see LieMatrix_Deprecated.h for details
* @brief External deprecation warning, see deprecated/LieMatrix.h for details
* @author Paul Drews
*/
@ -23,4 +23,4 @@
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#endif
#include "gtsam/base/LieMatrix_Deprecated.h"
#include "gtsam/base/deprecated/LieMatrix.h"

17
gtsam/base/LieScalar.cpp
View File

@ -1,17 +0,0 @@
/*
* LieScalar.cpp
*
* Created on: Apr 12, 2013
* Author: thduynguyen
*/
#include <gtsam/base/LieScalar_Deprecated.h>
namespace gtsam {
void LieScalar::print(const std::string& name) const {
std::cout << name << ": " << d_ << std::endl;
}
}

4
gtsam/base/LieScalar.h
View File

@ -11,7 +11,7 @@
/**
* @file LieScalar.h
* @brief External deprecation warning, see LieScalar_Deprecated.h for details
* @brief External deprecation warning, see deprecated/LieScalar.h for details
* @author Kai Ni
*/
@ -23,4 +23,4 @@
#warning "LieScalar.h is deprecated. Please use double/float instead."
#endif
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>

40
gtsam/base/LieVector.cpp
View File

@ -1,40 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 LieVector.cpp
* @brief Implementations for LieVector functions
* @author Alex Cunningham
*/
#include <gtsam/base/LieVector_Deprecated.h>
#include <cstdarg>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
LieVector::LieVector(size_t m, const double* const data)
: Vector(m)
{
for(size_t i = 0; i < m; i++)
(*this)(i) = data[i];
}
/* ************************************************************************* */
void LieVector::print(const std::string& name) const {
gtsam::print(vector(), name);
}
// Does not compile because LieVector is not fixed size.
// GTSAM_CONCEPT_LIE_INST(LieVector)
} // \namespace gtsam

4
gtsam/base/LieVector.h
View File

@ -11,7 +11,7 @@
/**
* @file LieVector.h
* @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details.
* @brief Deprecation warning for LieVector, see deprecated/LieVector.h for details.
* @author Paul Drews
*/
@ -23,4 +23,4 @@
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#endif
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieVector.h>

58
gtsam/base/Manifold.h
View File

@ -46,7 +46,7 @@ struct manifold_tag {};
* There may be multiple possible retractions for a given manifold, which can be chosen
* between depending on the computational complexity. The important criteria for
* the creation for the retract and localCoordinates functions is that they be
* inverse operations. The new notion of a Chart guarantees that.
* inverse operations.
*
*/
@ -90,9 +90,9 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
/// A helper that implements the traits interface for GTSAM manifolds.
/// To use this for your class type, define:
/// template<> struct traits<Class> : public Manifold<Class> { };
/// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
template<class Class>
struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
@ -116,6 +116,9 @@ struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
}
};
/// Both ManifoldTraits and Testable
template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
} // \ namespace internal
/// Check invariants for Manifold type
@ -135,7 +138,7 @@ class IsManifold {
public:
typedef typename traits<T>::structure_category structure_category_tag;
static const size_t dim = traits<T>::dimension;
static const int dim = traits<T>::dimension;
typedef typename traits<T>::ManifoldType ManifoldType;
typedef typename traits<T>::TangentVector TangentVector;
@ -165,6 +168,53 @@ struct FixedDimension {
"FixedDimension instantiated for dymanically-sized type.");
};
/// Helper class to construct the product manifold of two other manifolds, M1 and M2
/// Assumes nothing except manifold structure from M1 and M2
template<typename M1, typename M2>
class ProductManifold: public std::pair<M1, M2> {
BOOST_CONCEPT_ASSERT((IsManifold<M1>));
BOOST_CONCEPT_ASSERT((IsManifold<M2>));
protected:
enum { dimension1 = traits<M1>::dimension };
enum { dimension2 = traits<M2>::dimension };
public:
enum { dimension = dimension1 + dimension2 };
inline static size_t Dim() { return dimension;}
inline size_t dim() const { return dimension;}
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
/// Default constructor needs default constructors to be defined
ProductManifold():std::pair<M1,M2>(M1(),M2()) {}
// Construct from two original manifold values
ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
/// Retract delta to manifold
ProductManifold retract(const TangentVector& xi) const {
M1 m1 = traits<M1>::Retract(this->first, xi.template head<dimension1>());
M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
return ProductManifold(m1,m2);
}
/// Compute the coordinates in the tangent space
TangentVector localCoordinates(const ProductManifold& other) const {
typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first, other.first);
typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
TangentVector v;
v << v1, v2;
return v;
}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename M1, typename M2>
struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
};
} // \ namespace gtsam
///**

2
gtsam/base/Matrix.h
View File

@ -22,6 +22,8 @@
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/config.h> // Configuration from CMake
#include <boost/math/special_functions/fpclassify.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>

12
gtsam/base/OptionalJacobian.h
View File

@ -18,7 +18,7 @@
*/
#pragma once
#include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense>
#ifndef OPTIONALJACOBIAN_NOBOOST
@ -171,6 +171,16 @@ public:
// forward declare
template <typename T> struct traits;
/**
* @brief: meta-function to generate Jacobian
* @param T return type
* @param A argument type
*/
template <class T, class A>
struct MakeJacobian {
typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
};
/**
* @brief: meta-function to generate JacobianTA optional reference
* Used mainly by Expressions

197
gtsam/base/ProductLieGroup.h Normal file
View File

@ -0,0 +1,197 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------1------------------------------------------- */
/**
* @file ProductLieGroup.h
* @date May, 2015
* @author Frank Dellaert
* @brief Group product of two Lie Groups
*/
#pragma once
#include <gtsam/base/Lie.h>
#include <utility> // pair
namespace gtsam {
/// Template to construct the product Lie group of two other Lie groups
/// Assumes Lie group structure for G and H
template<typename G, typename H>
class ProductLieGroup: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
typedef std::pair<G, H> Base;
protected:
enum {dimension1 = traits<G>::dimension};
enum {dimension2 = traits<H>::dimension};
public:
/// Default constructor yields identity
ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
// Construct from two subgroup elements
ProductLieGroup(const G& g, const H& h):Base(g,h) {}
// Construct from base
ProductLieGroup(const Base& base):Base(base) {}
/// @name Group
/// @{
typedef multiplicative_group_tag group_flavor;
static ProductLieGroup identity() {return ProductLieGroup();}
ProductLieGroup operator*(const ProductLieGroup& other) const {
return ProductLieGroup(traits<G>::Compose(this->first,other.first),
traits<H>::Compose(this->second,other.second));
}
ProductLieGroup inverse() const {
return ProductLieGroup(this->first.inverse(), this->second.inverse());
}
ProductLieGroup compose(const ProductLieGroup& g) const {
return (*this) * g;
}
ProductLieGroup between(const ProductLieGroup& g) const {
return this->inverse() * g;
}
/// @}
/// @name Manifold
/// @{
enum {dimension = dimension1 + dimension2};
inline static size_t Dim() {return dimension;}
inline size_t dim() const {return dimension;}
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static ProductLieGroup Retract(const TangentVector& v) {
return ProductLieGroup::ChartAtOrigin::Retract(v);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g) {
return ProductLieGroup::ChartAtOrigin::Local(g);
}
ProductLieGroup retract(const TangentVector& v) const {
return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
}
TangentVector localCoordinates(const ProductLieGroup& g) const {
return ProductLieGroup::ChartAtOrigin::Local(between(g));
}
/// @}
/// @name Lie Group
/// @{
protected:
typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
public:
ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
if (H1) {
H1->setZero();
H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
if (H2) *H2 = Jacobian::Identity();
return ProductLieGroup(g,h);
}
ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
if (H1) {
H1->setZero();
H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
if (H2) *H2 = Jacobian::Identity();
return ProductLieGroup(g,h);
}
ProductLieGroup inverse(ChartJacobian D) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
if (D) {
D->setZero();
D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
return ProductLieGroup(g,h);
}
static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet");
G g = traits<G>::Expmap(v.template head<dimension1>());
H h = traits<H>::Expmap(v.template tail<dimension2>());
return ProductLieGroup(g,h);
}
static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet");
typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first);
typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second);
TangentVector v;
v << v1, v2;
return v;
}
struct ChartAtOrigin {
static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
return Logmap(m, Hm);
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
return Expmap(v, Hv);
}
};
ProductLieGroup expmap(const TangentVector& v) const {
return compose(ProductLieGroup::Expmap(v));
}
TangentVector logmap(const ProductLieGroup& g) const {
return ProductLieGroup::Logmap(between(g));
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Local(g,H1);
}
ProductLieGroup retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
ProductLieGroup h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v;
return h;
}
TangentVector localCoordinates(const ProductLieGroup& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ProductLieGroup h = between(g,H1,H2);
Jacobian D_v_h;
TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2);
return v;
}
/// @}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename G, typename H>
struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
ProductLieGroup<G, H> > {
};
} // namespace gtsam

15
gtsam/base/SymmetricBlockMatrix.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/ThreadsafeException.h>
namespace gtsam {
@ -61,13 +62,13 @@ VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(
// Split conditional
// Create one big conditionals with many frontal variables.
gttic(Construct_conditional);
const size_t varDim = offset(nFrontals);
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
Ab.full() = matrix_.topRows(varDim);
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
gttoc(Construct_conditional);
// Create one big conditionals with many frontal variables.
gttic(Construct_conditional);
const size_t varDim = offset(nFrontals);
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
Ab.full() = matrix_.topRows(varDim);
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
gttoc(Construct_conditional);
gttic(Remaining_factor);
// Take lower-right block of Ab_ to get the remaining factor

23
gtsam/base/SymmetricBlockMatrix.h
View File

@ -17,9 +17,21 @@
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/SymmetricBlockMatrixBlockExpr.h>
#include <gtsam/base/types.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cassert>
#include <stdexcept>
#include <vector>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
@ -247,13 +259,8 @@ namespace gtsam {
}
};
/* ************************************************************************* */
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{
public:
CholeskyFailed() throw() {}
virtual ~CholeskyFailed() throw() {}
};
/// Foward declare exception class
class CholeskyFailed;
}

11
gtsam/base/TestableAssertions.h
View File

@ -17,13 +17,14 @@
#pragma once
#include <vector>
#include <map>
#include <iostream>
#include <gtsam/base/Testable.h>
#include <gtsam/global_includes.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/Testable.h>
#include <map>
#include <iostream>
#include <vector>
namespace gtsam {

155
gtsam/base/ThreadsafeException.h Normal file
View File

@ -0,0 +1,155 @@
/* ----------------------------------------------------------------------------
* 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 ThreadSafeException.h
* @brief Base exception type that uses tbb_exception if GTSAM is compiled with TBB
* @author Richard Roberts
* @date Aug 21, 2010
* @addtogroup base
*/
#pragma once
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/optional/optional.hpp>
#include <string>
#include <typeinfo>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb_allocator.h>
#include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#include <iostream>
#endif
namespace gtsam {
/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
template<class DERIVED>
class ThreadsafeException:
#ifdef GTSAM_USE_TBB
public tbb::tbb_exception
#else
public std::exception
#endif
{
#ifdef GTSAM_USE_TBB
private:
typedef tbb::tbb_exception Base;
protected:
typedef std::basic_string<char, std::char_traits<char>,
tbb::tbb_allocator<char> > String;
#else
private:
typedef std::exception Base;
protected:
typedef std::string String;
#endif
protected:
bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> description_; ///< Optional description
/// Default constructor is protected - may only be created from derived classes
ThreadsafeException() :
dynamic_(false) {
}
/// Copy constructor is protected - may only be created from derived classes
ThreadsafeException(const ThreadsafeException& other) :
Base(other), dynamic_(false) {
}
/// Construct with description string
ThreadsafeException(const std::string& description) :
dynamic_(false), description_(
String(description.begin(), description.end())) {
}
/// Default destructor doesn't have the throw()
virtual ~ThreadsafeException() throw () {
}
public:
// Implement functions for tbb_exception
#ifdef GTSAM_USE_TBB
virtual tbb::tbb_exception* move() throw () {
void* cloneMemory = scalable_malloc(sizeof(DERIVED));
if (!cloneMemory) {
std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
exit(-1);
}
DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
clone->dynamic_ = true;
return clone;
}
virtual void destroy() throw () {
if (dynamic_) {
DERIVED* derivedPtr = static_cast<DERIVED*>(this);
derivedPtr->~DERIVED();
scalable_free(derivedPtr);
}
}
virtual void throw_self() {
throw *static_cast<DERIVED*>(this);
}
virtual const char* name() const throw () {
return typeid(DERIVED).name();
}
#endif
virtual const char* what() const throw () {
return description_ ? description_->c_str() : "";
}
};
/// Thread-safe runtime error exception
class RuntimeErrorThreadsafe: public ThreadsafeException<RuntimeErrorThreadsafe> {
public:
/// Construct with a string describing the exception
RuntimeErrorThreadsafe(const std::string& description) :
ThreadsafeException<RuntimeErrorThreadsafe>(description) {
}
};
/// Thread-safe out of range exception
class OutOfRangeThreadsafe: public ThreadsafeException<OutOfRangeThreadsafe> {
public:
/// Construct with a string describing the exception
OutOfRangeThreadsafe(const std::string& description) :
ThreadsafeException<OutOfRangeThreadsafe>(description) {
}
};
/// Thread-safe invalid argument exception
class InvalidArgumentThreadsafe: public ThreadsafeException<
InvalidArgumentThreadsafe> {
public:
/// Construct with a string describing the exception
InvalidArgumentThreadsafe(const std::string& description) :
ThreadsafeException<InvalidArgumentThreadsafe>(description) {
}
};
/// Indicate Cholesky factorization failure
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{
public:
CholeskyFailed() throw() {}
virtual ~CholeskyFailed() throw() {}
};
} // namespace gtsam

15
gtsam/base/VectorSpace.h
View File

@ -20,7 +20,7 @@ template<typename T> struct traits;
namespace internal {
/// VectorSpace Implementation for Fixed sizes
/// VectorSpaceTraits Implementation for Fixed sizes
template<class Class, int N>
struct VectorSpaceImpl {
@ -83,7 +83,7 @@ struct VectorSpaceImpl {
/// @}
};
/// VectorSpace implementation for dynamic types.
/// VectorSpaceTraits implementation for dynamic types.
template<class Class>
struct VectorSpaceImpl<Class,Eigen::Dynamic> {
@ -159,11 +159,11 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @}
};
/// A helper that implements the traits interface for GTSAM lie groups.
/// A helper that implements the traits interface for GTSAM vector space types.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public VectorSpace<Type> { };
/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { };
template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
typedef vector_space_tag structure_category;
@ -178,9 +178,12 @@ struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
enum { dimension = Class::dimension};
typedef Class ManifoldType;
/// @}
};
/// Both VectorSpaceTRaits and Testable
template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
/// A helper that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public ScalarTraits<Type> { };

2
gtsam/base/debug.cpp
View File

@ -17,6 +17,8 @@
*/
#include <gtsam/base/debug.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef GTSAM_USE_TBB
#include <tbb/mutex.h>
#endif

5
gtsam/base/LieMatrix_Deprecated.h → gtsam/base/deprecated/LieMatrix.h
View File

@ -60,8 +60,9 @@ struct LieMatrix : public Matrix {
/// @{
/** print @param s optional string naming the object */
GTSAM_EXPORT void print(const std::string& name="") const;
GTSAM_EXPORT void print(const std::string& name = "") const {
gtsam::print(matrix(), name);
}
/** equality up to tolerance */
inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);

6
gtsam/base/LieScalar_Deprecated.h → gtsam/base/deprecated/LieScalar.h
View File

@ -48,8 +48,10 @@ namespace gtsam {
/// @name Testable
/// @{
void print(const std::string& name="") const;
bool equals(const LieScalar& expected, double tol=1e-5) const {
void print(const std::string& name = "") const {
std::cout << name << ": " << d_ << std::endl;
}
bool equals(const LieScalar& expected, double tol = 1e-5) const {
return fabs(expected.d_ - d_) <= tol;
}
/// @}

9
gtsam/base/LieVector_Deprecated.h → gtsam/base/deprecated/LieVector.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <cstdarg>
namespace gtsam {
@ -50,11 +51,15 @@ struct LieVector : public Vector {
LieVector(double d) : Vector((Vector(1) << d).finished()) {}
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);
GTSAM_EXPORT LieVector(size_t m, const double* const data) : Vector(m) {
for (size_t i = 0; i < m; i++) (*this)(i) = data[i];
}
/// @name Testable
/// @{
GTSAM_EXPORT void print(const std::string& name="") const;
GTSAM_EXPORT void print(const std::string& name="") const {
gtsam::print(vector(), name);
}
bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}

18
gtsam/base/serializationTestHelpers.h
View File

@ -49,15 +49,15 @@ bool equality(const T& input = T()) {
return input==output;
}
// This version requires equals
// This version requires Testable
template<class T>
bool equalsObj(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input.equals(output);
return assert_equal(input, output);
}
// De-referenced version for pointers
// De-referenced version for pointers, requires equals method
template<class T>
bool equalsDereferenced(const T& input) {
T output;
@ -84,15 +84,15 @@ bool equalityXML(const T& input = T()) {
return input==output;
}
// This version requires equals
// This version requires Testable
template<class T>
bool equalsXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input.equals(output);
return assert_equal(input, output);
}
// This version is for pointers
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
@ -119,15 +119,15 @@ bool equalityBinary(const T& input = T()) {
return input==output;
}
// This version requires equals
// This version requires Testable
template<class T>
bool equalsBinary(const T& input = T()) {
T output;
roundtripBinary<T>(input,output);
return input.equals(output);
return assert_equal(input, output);
}
// This version is for pointers
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedBinary(const T& input = T()) {
T output;

9
gtsam/base/tests/testFastContainers.cpp
View File

@ -7,13 +7,14 @@
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/set.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <CppUnitLite/TestHarness.h>
using namespace boost::assign;
using namespace gtsam;
@ -21,7 +22,7 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( testFastContainers, KeySet ) {
FastVector<size_t> init_vector;
FastVector<Key> init_vector;
init_vector += 2, 3, 4, 5;
FastSet<size_t> actSet(init_vector);

144
gtsam/base/tests/testGroup.cpp Normal file
View File

@ -0,0 +1,144 @@
/* ----------------------------------------------------------------------------
* 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 testGroup.cpp
* @brief Unit tests for groups
* @author Frank Dellaert
**/
#include <gtsam/base/Group.h>
#include <gtsam/base/Testable.h>
#include <Eigen/Core>
#include <iostream>
#include <boost/foreach.hpp>
namespace gtsam {
/// Symmetric group
template<int N>
class Symmetric: private Eigen::PermutationMatrix<N> {
Symmetric(const Eigen::PermutationMatrix<N>& P) :
Eigen::PermutationMatrix<N>(P) {
}
public:
static Symmetric identity() { return Symmetric(); }
Symmetric() {
Eigen::PermutationMatrix<N>::setIdentity();
}
static Symmetric Transposition(int i, int j) {
Symmetric g;
return g.applyTranspositionOnTheRight(i, j);
}
Symmetric operator*(const Symmetric& other) const {
return Eigen::PermutationMatrix<N>::operator*(other);
}
bool operator==(const Symmetric& other) const {
for (size_t i = 0; i < N; i++)
if (this->indices()[i] != other.indices()[i])
return false;
return true;
}
Symmetric inverse() const {
return Eigen::PermutationMatrix<N>(Eigen::PermutationMatrix<N>::inverse());
}
friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) {
for (size_t i = 0; i < N; i++)
os << m.indices()[i] << " ";
return os;
}
void print(const std::string& s = "") const {
std::cout << s << *this << std::endl;
}
bool equals(const Symmetric<N>& other, double tol = 0) const {
return this->indices() == other.indices();
}
};
/// Define permutation group traits to be a model of the Multiplicative Group concept
template<int N>
struct traits<Symmetric<N> > : internal::MultiplicativeGroupTraits<Symmetric<N> >,
Testable<Symmetric<N> > {
};
} // namespace gtsam
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
typedef Symmetric<2> S2;
TEST(Group, S2) {
S2 e, s1 = S2::Transposition(0, 1);
BOOST_CONCEPT_ASSERT((IsGroup<S2>));
EXPECT(check_group_invariants(e, s1));
}
//******************************************************************************
typedef Symmetric<3> S3;
TEST(Group, S3) {
S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
BOOST_CONCEPT_ASSERT((IsGroup<S3>));
EXPECT(check_group_invariants(e, s1));
EXPECT(assert_equal(s1, s1 * e));
EXPECT(assert_equal(s1, e * s1));
EXPECT(assert_equal(e, s1 * s1));
S3 g = s1 * s2; // 1 2 0
EXPECT(assert_equal(s1, g * s2));
EXPECT(assert_equal(e, compose_pow(g, 0)));
EXPECT(assert_equal(g, compose_pow(g, 1)));
EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup
}
//******************************************************************************
// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon,
// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon)
namespace gtsam {
typedef DirectProduct<S2, S3> Dih6;
std::ostream &operator<<(std::ostream &os, const Dih6& m) {
os << "( " << m.first << ", " << m.second << ")";
return os;
}
// Provide traits with Testable
template<>
struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
static void Print(const Dih6& m, const string& s = "") {
cout << s << m << endl;
}
static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) {
return m1 == m2;
}
};
} // namespace gtsam
TEST(Group, Dih6) {
Dih6 e, g(S2::Transposition(0, 1),
S3::Transposition(0, 1) * S3::Transposition(1, 2));
BOOST_CONCEPT_ASSERT((IsGroup<Dih6>));
EXPECT(check_group_invariants(e, g));
EXPECT(assert_equal(e, compose_pow(g, 0)));
EXPECT(assert_equal(g, compose_pow(g, 1)));
EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

3
gtsam/base/tests/testLieMatrix.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieMatrix_Deprecated.h>
#include <gtsam/base/deprecated/LieMatrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>

7
gtsam/base/tests/testLieScalar.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
@ -37,8 +36,8 @@ TEST(LieScalar , Concept) {
//******************************************************************************
TEST(LieScalar , Invariants) {
LieScalar lie1(2), lie2(3);
check_group_invariants(lie1, lie2);
check_manifold_invariants(lie1, lie2);
CHECK(check_group_invariants(lie1, lie2));
CHECK(check_manifold_invariants(lie1, lie2));
}
/* ************************************************************************* */

3
gtsam/base/tests/testLieVector.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieVector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>

10
gtsam/base/tests/testSerializationBase.cpp
View File

@ -16,6 +16,8 @@
* @date Feb 7, 2012
*/
#include <gtsam/inference/Key.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/FastList.h>
@ -60,10 +62,10 @@ TEST (Serialization, FastMap) {
/* ************************************************************************* */
TEST (Serialization, FastSet) {
FastSet<double> set;
set.insert(1.0);
set.insert(2.0);
set.insert(3.0);
KeySet set;
set.insert(1);
set.insert(2);
set.insert(3);
EXPECT(equality(set));
EXPECT(equalityXML(set));

3
gtsam/base/tests/testTestableAssertions.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam/base/TestableAssertions.h>
using namespace gtsam;

61
gtsam/base/timing.cpp
View File

@ -16,24 +16,28 @@
* @date Oct 5, 2010
*/
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <stdlib.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <boost/algorithm/string/replace.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <cmath>
#include <cstddef>
#include <cassert>
#include <iomanip>
#include <iostream>
#include <map>
#include <stdexcept>
#include <utility>
namespace gtsam {
namespace internal {
GTSAM_EXPORT boost::shared_ptr<TimingOutline> timingRoot(
GTSAM_EXPORT boost::shared_ptr<TimingOutline> gTimingRoot(
new TimingOutline("Total", getTicTocID("Total")));
GTSAM_EXPORT boost::weak_ptr<TimingOutline> timingCurrent(timingRoot);
GTSAM_EXPORT boost::weak_ptr<TimingOutline> gCurrentTimer(gTimingRoot);
/* ************************************************************************* */
// Implementation of TimingOutline
@ -50,8 +54,8 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) {
}
/* ************************************************************************* */
TimingOutline::TimingOutline(const std::string& label, size_t myId) :
myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
TimingOutline::TimingOutline(const std::string& label, size_t id) :
id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
0), lastChildOrder_(0), label_(label) {
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
timer_.stop();
@ -153,7 +157,7 @@ const boost::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
}
/* ************************************************************************* */
void TimingOutline::ticInternal() {
void TimingOutline::tic() {
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
assert(timer_.is_stopped());
timer_.start();
@ -169,7 +173,7 @@ void TimingOutline::ticInternal() {
}
/* ************************************************************************* */
void TimingOutline::tocInternal() {
void TimingOutline::toc() {
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
assert(!timer_.is_stopped());
@ -212,7 +216,6 @@ void TimingOutline::finishedIteration() {
}
/* ************************************************************************* */
// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements
size_t getTicTocID(const char *descriptionC) {
const std::string description(descriptionC);
// Global (static) map from strings to ID numbers and current next ID number
@ -232,37 +235,33 @@ size_t getTicTocID(const char *descriptionC) {
}
/* ************************************************************************* */
void ticInternal(size_t id, const char *labelC) {
void tic(size_t id, const char *labelC) {
const std::string label(labelC);
if (ISDEBUG("timing-verbose"))
std::cout << "gttic_(" << id << ", " << label << ")" << std::endl;
boost::shared_ptr<TimingOutline> node = //
timingCurrent.lock()->child(id, label, timingCurrent);
timingCurrent = node;
node->ticInternal();
gCurrentTimer.lock()->child(id, label, gCurrentTimer);
gCurrentTimer = node;
node->tic();
}
/* ************************************************************************* */
void tocInternal(size_t id, const char *label) {
if (ISDEBUG("timing-verbose"))
std::cout << "gttoc(" << id << ", " << label << ")" << std::endl;
boost::shared_ptr<TimingOutline> current(timingCurrent.lock());
if (id != current->myId_) {
timingRoot->print();
void toc(size_t id, const char *label) {
boost::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
if (id != current->id_) {
gTimingRoot->print();
throw std::invalid_argument(
(boost::format(
"gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".")
% label % current->label_).str());
}
if (!current->parent_.lock()) {
timingRoot->print();
gTimingRoot->print();
throw std::invalid_argument(
(boost::format(
"gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root")
% label).str());
}
current->tocInternal();
timingCurrent = current->parent_;
current->toc();
gCurrentTimer = current->parent_;
}
} // namespace internal

72
gtsam/base/timing.h
View File

@ -17,12 +17,16 @@
*/
#pragma once
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/version.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/dllexport.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/smart_ptr/weak_ptr.hpp>
#include <boost/version.hpp>
#include <cstddef>
#include <string>
// This file contains the GTSAM timing instrumentation library, a low-overhead method for
// learning at a medium-fine level how much time various components of an algorithm take
@ -125,16 +129,21 @@
namespace gtsam {
namespace internal {
// Generate/retrieve a unique global ID number that will be used to look up tic/toc statements
GTSAM_EXPORT size_t getTicTocID(const char *description);
GTSAM_EXPORT void ticInternal(size_t id, const char *label);
GTSAM_EXPORT void tocInternal(size_t id, const char *label);
// Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method
GTSAM_EXPORT void tic(size_t id, const char *label);
// Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer
GTSAM_EXPORT void toc(size_t id, const char *label);
/**
* Timing Entry, arranged in a tree
*/
class GTSAM_EXPORT TimingOutline {
protected:
size_t myId_;
size_t id_;
size_t t_;
size_t tWall_;
double t2_ ; ///< cache the \sum t_i^2
@ -176,29 +185,38 @@ namespace gtsam {
void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
const boost::shared_ptr<TimingOutline>&
child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
void ticInternal();
void tocInternal();
void tic();
void toc();
void finishedIteration();
GTSAM_EXPORT friend void tocInternal(size_t id, const char *label);
GTSAM_EXPORT friend void toc(size_t id, const char *label);
}; // \TimingOutline
/**
* No documentation
* Small class that calls internal::tic at construction, and internol::toc when destroyed
*/
class AutoTicToc {
private:
private:
size_t id_;
const char *label_;
const char* label_;
bool isSet_;
public:
AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); }
void stop() { tocInternal(id_, label_); isSet_ = false; }
~AutoTicToc() { if(isSet_) stop(); }
public:
AutoTicToc(size_t id, const char* label)
: id_(id), label_(label), isSet_(true) {
tic(id_, label_);
}
void stop() {
toc(id_, label_);
isSet_ = false;
}
~AutoTicToc() {
if (isSet_) stop();
}
};
GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> timingRoot;
GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> timingCurrent;
GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> gTimingRoot;
GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> gCurrentTimer;
}
// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined)
@ -210,7 +228,7 @@ namespace gtsam {
// tic
#define gttic_(label) \
static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label)
::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label)
// toc
#define gttoc_(label) \
@ -228,26 +246,26 @@ namespace gtsam {
// indicate iteration is finished
inline void tictoc_finishedIteration_() {
::gtsam::internal::timingRoot->finishedIteration(); }
::gtsam::internal::gTimingRoot->finishedIteration(); }
// print
inline void tictoc_print_() {
::gtsam::internal::timingRoot->print(); }
::gtsam::internal::gTimingRoot->print(); }
// print mean and standard deviation
inline void tictoc_print2_() {
::gtsam::internal::timingRoot->print2(); }
::gtsam::internal::gTimingRoot->print2(); }
// get a node by label and assign it to variable
#define tictoc_getNode(variable, label) \
static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \
const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \
::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent);
::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer);
// reset
inline void tictoc_reset_() {
::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total")));
::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; }
::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total")));
::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; }
#ifdef ENABLE_TIMING
#define gttic(label) gttic_(label)

396
gtsam/base/treeTraversal-inst.h
View File

@ -1,19 +1,19 @@
/* ----------------------------------------------------------------------------
* 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)
* 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
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
* -------------------------------------------------------------------------- */
/**
* @file treeTraversal-inst.h
* @author Richard Roberts
* @date April 9, 2013
*/
* @file treeTraversal-inst.h
* @author Richard Roberts
* @date April 9, 2013
*/
#pragma once
#include <gtsam/base/treeTraversal/parallelTraversalTasks.h>
@ -22,6 +22,7 @@
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <stack>
#include <vector>
@ -33,192 +34,197 @@
namespace gtsam {
/** Internal functions used for traversing trees */
namespace treeTraversal {
/** Internal functions used for traversing trees */
namespace treeTraversal {
/* ************************************************************************* */
namespace {
// Internal node used in DFS preorder stack
template<typename NODE, typename DATA>
struct TraversalNode {
bool expanded;
const boost::shared_ptr<NODE>& treeNode;
DATA& parentData;
typename FastList<DATA>::iterator dataPointer;
TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
expanded(false), treeNode(_treeNode), parentData(_parentData) {}
};
// Do nothing - default argument for post-visitor for tree traversal
struct no_op {
template<typename NODE, typename DATA>
void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {}
};
}
/** Traverse a forest depth-first with pre-order and post-order visits.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
* to pass to the children. The returned \c DATA object will be copy-constructed only
* upon returning to store internally, thus may be modified by visiting the children.
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
* @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting
* its children, and will be passed, by reference, the \c DATA object returned by the
* call to \c visitorPre (the \c DATA object may be modified by visiting the children).
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost)
{
// Typedefs
typedef typename FOREST::Node Node;
typedef boost::shared_ptr<Node> sharedNode;
// Depth first traversal stack
typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
typedef FastList<TraversalNode> Stack;
Stack stack;
FastList<DATA> dataList; // List to store node data as it is returned from the pre-order visitor
// Add roots to stack (insert such that they are visited and processed in order
{
typename Stack::iterator insertLocation = stack.begin();
BOOST_FOREACH(const sharedNode& root, forest.roots())
stack.insert(insertLocation, TraversalNode(root, rootData));
}
// Traverse
while(!stack.empty())
{
// Get next node
TraversalNode& node = stack.front();
if(node.expanded) {
// If already expanded, then the data stored in the node is no longer needed, so visit
// then delete it.
(void) visitorPost(node.treeNode, *node.dataPointer);
dataList.erase(node.dataPointer);
stack.pop_front();
} else {
// If not already visited, visit the node and add its children (use reverse iterators so
// children are processed in the order they appear)
node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData));
typename Stack::iterator insertLocation = stack.begin();
BOOST_FOREACH(const sharedNode& child, node.treeNode->children)
stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
node.expanded = true;
}
}
assert(dataList.empty());
}
/** Traverse a forest depth-first, with a pre-order visit but no post-order visit.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
* to pass to the children. The returned \c DATA object will be copy-constructed only
* upon returning to store internally, thus may be modified by visiting the children.
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre)
{
no_op visitorPost;
DepthFirstForest(forest, rootData, visitorPre, visitorPost);
}
/** Traverse a forest depth-first with pre-order and post-order visits.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
* to pass to the children. The returned \c DATA object will be copy-constructed only
* upon returning to store internally, thus may be modified by visiting the children.
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
* @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting
* its children, and will be passed, by reference, the \c DATA object returned by the
* call to \c visitorPre (the \c DATA object may be modified by visiting the children).
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
int problemSizeThreshold = 10)
{
#ifdef GTSAM_USE_TBB
// Typedefs
typedef typename FOREST::Node Node;
typedef boost::shared_ptr<Node> sharedNode;
tbb::task::spawn_root_and_wait(internal::CreateRootTask<Node>(
forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold));
#else
DepthFirstForest(forest, rootData, visitorPre, visitorPost);
#endif
}
/* ************************************************************************* */
/** Traversal function for CloneForest */
namespace {
template<typename NODE>
boost::shared_ptr<NODE>
CloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
{
// Clone the current node and add it to its cloned parent
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
clone->children.clear();
parentPointer->children.push_back(clone);
return clone;
}
}
/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child
* pointers for a clone of the original tree.
* @param forest The forest of trees to clone. The method \c forest.roots() should exist and
* return a collection of shared pointers to \c FOREST::Node.
* @return The new collection of roots. */
template<class FOREST>
FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(const FOREST& forest)
{
typedef typename FOREST::Node Node;
boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end());
}
/* ************************************************************************* */
/** Traversal function for PrintForest */
namespace {
struct PrintForestVisitorPre
{
const KeyFormatter& formatter;
PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {}
template<typename NODE> std::string operator()(const boost::shared_ptr<NODE>& node, const std::string& parentString)
{
// Print the current node
node->print(parentString + "-", formatter);
// Increment the indentation
return parentString + "| ";
}
};
}
/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter.
* To print each node, this function calls the \c print function of the tree nodes. */
template<class FOREST>
void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) {
PrintForestVisitorPre visitor(keyFormatter);
DepthFirstForest(forest, str, visitor);
}
/* ************************************************************************* */
namespace {
// Internal node used in DFS preorder stack
template<typename NODE, typename DATA>
struct TraversalNode {
bool expanded;
const boost::shared_ptr<NODE>& treeNode;
DATA& parentData;
typename FastList<DATA>::iterator dataPointer;
TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
expanded(false), treeNode(_treeNode), parentData(_parentData) {
}
};
// Do nothing - default argument for post-visitor for tree traversal
struct no_op {
template<typename NODE, typename DATA>
void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {
}
};
}
/** Traverse a forest depth-first with pre-order and post-order visits.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
* to pass to the children. The returned \c DATA object will be copy-constructed only
* upon returning to store internally, thus may be modified by visiting the children.
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
* @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting
* its children, and will be passed, by reference, the \c DATA object returned by the
* call to \c visitorPre (the \c DATA object may be modified by visiting the children).
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE,
typename VISITOR_POST>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
VISITOR_POST& visitorPost) {
// Typedefs
typedef typename FOREST::Node Node;
typedef boost::shared_ptr<Node> sharedNode;
// Depth first traversal stack
typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
typedef FastList<TraversalNode> Stack;
Stack stack;
FastList<DATA> dataList; // List to store node data as it is returned from the pre-order visitor
// Add roots to stack (insert such that they are visited and processed in order
{
typename Stack::iterator insertLocation = stack.begin();
BOOST_FOREACH(const sharedNode& root, forest.roots())
stack.insert(insertLocation, TraversalNode(root, rootData));
}
// Traverse
while (!stack.empty()) {
// Get next node
TraversalNode& node = stack.front();
if (node.expanded) {
// If already expanded, then the data stored in the node is no longer needed, so visit
// then delete it.
(void) visitorPost(node.treeNode, *node.dataPointer);
dataList.erase(node.dataPointer);
stack.pop_front();
} else {
// If not already visited, visit the node and add its children (use reverse iterators so
// children are processed in the order they appear)
node.dataPointer = dataList.insert(dataList.end(),
visitorPre(node.treeNode, node.parentData));
typename Stack::iterator insertLocation = stack.begin();
BOOST_FOREACH(const sharedNode& child, node.treeNode->children)
stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
node.expanded = true;
}
}
assert(dataList.empty());
}
/** Traverse a forest depth-first, with a pre-order visit but no post-order visit.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
* to pass to the children. The returned \c DATA object will be copy-constructed only
* upon returning to store internally, thus may be modified by visiting the children.
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
no_op visitorPost;
DepthFirstForest(forest, rootData, visitorPre, visitorPost);
}
/** Traverse a forest depth-first with pre-order and post-order visits.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
* to pass to the children. The returned \c DATA object will be copy-constructed only
* upon returning to store internally, thus may be modified by visiting the children.
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
* @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting
* its children, and will be passed, by reference, the \c DATA object returned by the
* call to \c visitorPre (the \c DATA object may be modified by visiting the children).
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE,
typename VISITOR_POST>
void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
int problemSizeThreshold = 10) {
#ifdef GTSAM_USE_TBB
// Typedefs
typedef typename FOREST::Node Node;
typedef boost::shared_ptr<Node> sharedNode;
tbb::task::spawn_root_and_wait(
internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
visitorPost, problemSizeThreshold));
#else
DepthFirstForest(forest, rootData, visitorPre, visitorPost);
#endif
}
/* ************************************************************************* */
/** Traversal function for CloneForest */
namespace {
template<typename NODE>
boost::shared_ptr<NODE> CloneForestVisitorPre(
const boost::shared_ptr<NODE>& node,
const boost::shared_ptr<NODE>& parentPointer) {
// Clone the current node and add it to its cloned parent
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
clone->children.clear();
parentPointer->children.push_back(clone);
return clone;
}
}
/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child
* pointers for a clone of the original tree.
* @param forest The forest of trees to clone. The method \c forest.roots() should exist and
* return a collection of shared pointers to \c FOREST::Node.
* @return The new collection of roots. */
template<class FOREST>
FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(
const FOREST& forest) {
typedef typename FOREST::Node Node;
boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(),
rootContainer->children.end());
}
/* ************************************************************************* */
/** Traversal function for PrintForest */
namespace {
struct PrintForestVisitorPre {
const KeyFormatter& formatter;
PrintForestVisitorPre(const KeyFormatter& formatter) :
formatter(formatter) {
}
template<typename NODE> std::string operator()(
const boost::shared_ptr<NODE>& node, const std::string& parentString) {
// Print the current node
node->print(parentString + "-", formatter);
// Increment the indentation
return parentString + "| ";
}
};
}
/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter.
* To print each node, this function calls the \c print function of the tree nodes. */
template<class FOREST>
void PrintForest(const FOREST& forest, std::string str,
const KeyFormatter& keyFormatter) {
PrintForestVisitorPre visitor(keyFormatter);
DepthFirstForest(forest, str, visitor);
}
}
}

1
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -24,6 +24,7 @@
#ifdef GTSAM_USE_TBB
# include <tbb/tbb.h>
# include <tbb/scalable_allocator.h>
# undef max // TBB seems to include windows.h and we don't want these macros
# undef min
# undef ERROR

35
gtsam/base/types.cpp
View File

@ -1,35 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 types.h
* @brief Typedefs for easier changing of types
* @author Richard Roberts
* @date Aug 21, 2010
* @addtogroup base
*/
#include <gtsam/base/types.h>
#include <gtsam/inference/Symbol.h>
#include <boost/lexical_cast.hpp>
namespace gtsam {
/* ************************************************************************* */
std::string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);
if(asSymbol.chr() > 0)
return (std::string)asSymbol;
else
return boost::lexical_cast<std::string>(key);
}
}

120
gtsam/base/types.h
View File

@ -20,19 +20,16 @@
#pragma once
#include <gtsam/dllexport.h>
#include <gtsam/config.h>
#include <boost/function/function1.hpp>
#include <boost/concept/assert.hpp>
#include <boost/range/concepts.hpp>
#include <boost/optional.hpp>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <cstddef>
#include <string>
#include <ostream>
#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
#include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#include <iostream>
#endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP
@ -58,22 +55,9 @@ namespace gtsam {
/// Integer nonlinear key type
typedef size_t Key;
/// Typedef for a function to format a key, i.e. to convert it to a string
typedef boost::function<std::string(Key)> KeyFormatter;
// Helper function for DefaultKeyFormatter
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
/// a nonlinear 'print' function. Automatically detects plain integer keys
/// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
/// The index type for Eigen objects
typedef ptrdiff_t DenseIndex;
/* ************************************************************************* */
/**
* Helper class that uses templates to select between two types based on
@ -148,104 +132,6 @@ namespace gtsam {
return ListOfOneContainer<T>(element);
}
/* ************************************************************************* */
/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
template<class DERIVED>
class ThreadsafeException :
#ifdef GTSAM_USE_TBB
public tbb::tbb_exception
#else
public std::exception
#endif
{
#ifdef GTSAM_USE_TBB
private:
typedef tbb::tbb_exception Base;
protected:
typedef std::basic_string<char, std::char_traits<char>, tbb::tbb_allocator<char> > String;
#else
private:
typedef std::exception Base;
protected:
typedef std::string String;
#endif
protected:
bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> description_; ///< Optional description
/// Default constructor is protected - may only be created from derived classes
ThreadsafeException() : dynamic_(false) {}
/// Copy constructor is protected - may only be created from derived classes
ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {}
/// Construct with description string
ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {}
/// Default destructor doesn't have the throw()
virtual ~ThreadsafeException() throw () {}
public:
// Implement functions for tbb_exception
#ifdef GTSAM_USE_TBB
virtual tbb::tbb_exception* move() throw () {
void* cloneMemory = scalable_malloc(sizeof(DERIVED));
if (!cloneMemory) {
std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
exit(-1);
}
DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
clone->dynamic_ = true;
return clone;
}
virtual void destroy() throw() {
if (dynamic_) {
DERIVED* derivedPtr = static_cast<DERIVED*>(this);
derivedPtr->~DERIVED();
scalable_free(derivedPtr);
}
}
virtual void throw_self() {
throw *static_cast<DERIVED*>(this); }
virtual const char* name() const throw() {
return typeid(DERIVED).name(); }
#endif
virtual const char* what() const throw() {
return description_ ? description_->c_str() : ""; }
};
/* ************************************************************************* */
/// Threadsafe runtime error exception
class RuntimeErrorThreadsafe : public ThreadsafeException<RuntimeErrorThreadsafe>
{
public:
/// Construct with a string describing the exception
RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException<RuntimeErrorThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe runtime error exception
class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe>
{
public:
/// Construct with a string describing the exception
OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException<OutOfRangeThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe invalid argument exception
class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe>
{
public:
/// Construct with a string describing the exception
InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException<InvalidArgumentThreadsafe>(description) {}
};
/* ************************************************************************* */
#ifdef __clang__
# pragma clang diagnostic push

4
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -39,8 +39,8 @@ namespace gtsam {
}
/* ************************************************************************* */
FastSet<Key> DiscreteFactorGraph::keys() const {
FastSet<Key> keys;
KeySet DiscreteFactorGraph::keys() const {
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end());
return keys;

2
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -120,7 +120,7 @@ public:
}
/** Return the set of variables involved in the factors (set union) */
FastSet<Key> keys() const;
KeySet keys() const;
/** return product of all factors as a single factor */
DecisionTreeFactor product() const;

1
gtsam/discrete/Potentials.h
View File

@ -19,6 +19,7 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/inference/Key.h>
#include <boost/shared_ptr.hpp>
#include <set>

133
gtsam/geometry/BearingRange.h Normal file
View File

@ -0,0 +1,133 @@
/* ----------------------------------------------------------------------------
* 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 BearingRange.h
* @date July, 2015
* @author Frank Dellaert
* @brief Bearing-Range product
*/
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/concept/assert.hpp>
namespace gtsam {
// Forward declaration of Bearing functor which should be of A1*A2 -> return_type
// For example Bearing<Pose3,Point3>(pose,point), defined in Pose3.h will return Unit3
// At time of writing only Pose2 and Pose3 specialize this functor.
template <typename A1, typename A2>
struct Bearing;
// Forward declaration of Range functor which should be of A1*A2 -> return_type
// For example Range<Pose2,Pose2>(T1,T2), defined in Pose2.h will return double
// At time of writing Pose2, Pose3, and several Camera variants specialize this for several types
template <typename A1, typename A2>
struct Range;
/**
* Bearing-Range product for a particular A1,A2 combination will use the functors above to create
* a similar functor of type A1*A2 -> pair<Bearing::return_type,Range::return_type>
* For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double>
* and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
*/
template <typename A1, typename A2>
struct BearingRange
: public ProductManifold<typename Bearing<A1, A2>::result_type,
typename Range<A1, A2>::result_type> {
typedef typename Bearing<A1, A2>::result_type B;
typedef typename Range<A1, A2>::result_type R;
typedef ProductManifold<B, R> Base;
BearingRange() {}
BearingRange(const ProductManifold<B, R>& br) : Base(br) {}
BearingRange(const B& b, const R& r) : Base(b, r) {}
/// Prediction function that stacks measurements
static BearingRange Measure(
const A1& a1, const A2& a2,
OptionalJacobian<Base::dimension, traits<A1>::dimension> H1 = boost::none,
OptionalJacobian<Base::dimension, traits<A2>::dimension> H2 =
boost::none) {
typename MakeJacobian<B, A1>::type HB1;
typename MakeJacobian<B, A2>::type HB2;
typename MakeJacobian<R, A1>::type HR1;
typename MakeJacobian<R, A2>::type HR2;
B b = Bearing<A1, A2>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
R r = Range<A1, A2>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
if (H1) *H1 << HB1, HR1;
if (H2) *H2 << HB2, HR2;
return BearingRange(b, r);
}
void print(const std::string& str = "") const {
std::cout << str;
traits<B>::Print(this->first, "bearing ");
traits<R>::Print(this->second, "range ");
}
bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
return traits<B>::Equals(this->first, m2.first, tol) &&
traits<R>::Equals(this->second, m2.second, tol);
}
private:
/// Serialization function
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("bearing", this->first);
ar& boost::serialization::make_nvp("range", this->second);
}
friend class boost::serialization::access;
};
// Declare this to be both Testable and a Manifold
template <typename A1, typename A2>
struct traits<BearingRange<A1, A2> >
: Testable<BearingRange<A1, A2> >,
internal::ManifoldTraits<BearingRange<A1, A2> > {};
// Helper class for to implement Range traits for classes with a bearing method
// For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say
// template <> struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
// where the third argument is used to indicate the return type
template <class A1, typename A2, class RT>
struct HasBearing {
typedef RT result_type;
RT operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
return a1.bearing(a2, H1, H2);
}
};
// Similar helper class for to implement Range traits for classes with a range method
// For classes with overloaded range methods, such as SimpleCamera, this can even be templated:
// template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
template <class A1, typename A2, class RT>
struct HasRange {
typedef RT result_type;
RT operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
return a1.range(a2, H1, H2);
}
};
} // namespace gtsam

1
gtsam/geometry/Cal3Bundler.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {

1
gtsam/geometry/Cal3DS2.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {

1
gtsam/geometry/Cal3Unified.h
View File

@ -23,7 +23,6 @@
#pragma once
#include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {

19
gtsam/geometry/Cal3_S2.cpp
View File

@ -83,10 +83,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p) const {
const double u = p.x(), v = p.y();
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
(1 / fy_) * (v - v0_));
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
OptionalJacobian<2,2> Dp) const {
const double u = p.x(), v = p.y();
double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1/ fx_, inv_fy = 1/fy_;
double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
inv_fy_delta_v);
if(Dcal)
*Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(),
-inv_fx, inv_fx_s_inv_fy,
0, -inv_fy * point.y(), 0, 0, -inv_fy;
if(Dp)
*Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
}
/* ************************************************************************* */

5
gtsam/geometry/Cal3_S2.h
View File

@ -156,9 +156,12 @@ public:
/**
* convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert homogeneous image coordinates to intrinsic coordinates

50
gtsam/geometry/CalibratedCamera.cpp
View File

@ -23,7 +23,6 @@ using namespace std;
namespace gtsam {
#ifndef PINHOLEBASE_LINKING_FIX
/* ************************************************************************* */
Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb
@ -86,8 +85,7 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
}
/* ************************************************************************* */
Point2 PinholeBase::project_to_camera(const Point3& pc,
OptionalJacobian<2, 3> Dpoint) {
Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) {
double d = 1.0 / pc.z();
const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint)
@ -95,10 +93,22 @@ Point2 PinholeBase::project_to_camera(const Point3& pc,
return Point2(u, v);
}
/* ************************************************************************* */
Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
if (Dpoint) {
Matrix32 Dpoint3_pc;
Matrix23 Duv_point3;
Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3);
*Dpoint = Duv_point3 * Dpoint3_pc;
return uv;
} else
return Project(pc.point3());
}
/* ************************************************************************* */
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw);
const Point2 pn = project_to_camera(pc);
const Point2 pn = Project(pc);
return make_pair(pn, pc.z() > 0);
}
@ -110,9 +120,9 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0)
throw CheiralityException();
throw CheiralityException();
#endif
const Point2 pn = project_to_camera(q);
const Point2 pn = Project(q);
if (Dpose || Dpoint) {
const double d = 1.0 / q.z();
@ -124,14 +134,38 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
return pn;
}
/* ************************************************************************* */
Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint) const {
// world to camera coordinate
Matrix23 Dpc_rot;
Matrix2 Dpc_point;
const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
Dpose ? &Dpc_point : 0);
// camera to normalized image coordinate
Matrix2 Dpn_pc;
const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
// chain the Jacobian matrices
if (Dpose) {
// only rotation is important
Matrix26 Dpc_pose;
Dpc_pose.setZero();
Dpc_pose.leftCols<3>() = Dpc_rot;
*Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6
}
if (Dpoint)
*Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p,
const double depth) {
return Point3(p.x() * depth, p.y() * depth, depth);
}
#endif
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
return CalibratedCamera(LevelPose(pose2, height));

211
gtsam/geometry/CalibratedCamera.h
View File

@ -18,11 +18,14 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Pose3.h>
#define PINHOLEBASE_LINKING_FIX
#ifdef PINHOLEBASE_LINKING_FIX
#include <gtsam/geometry/Pose2.h>
#endif
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
class Point2;
@ -42,12 +45,22 @@ public:
*/
class GTSAM_EXPORT PinholeBase {
public:
/** Pose Concept requirements */
typedef Rot3 Rotation;
typedef Point3 Translation;
/**
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Point2 Measurement;
private:
Pose3 pose_; ///< 3D pose of camera
#ifndef PINHOLEBASE_LINKING_FIX
protected:
/// @name Derivatives
@ -135,6 +148,16 @@ public:
return pose_;
}
/// get rotation
const Rot3& rotation() const {
return pose_.rotation();
}
/// get translation
const Point3& translation() const {
return pose_.translation();
}
/// return pose, with derivative
const Pose3& getPose(OptionalJacobian<6, 6> H) const;
@ -147,14 +170,21 @@ public:
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Point2 project_to_camera(const Point3& pc, //
static Point2 Project(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none);
/**
* Project from 3D point at infinity in camera coordinates into image
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Point2 Project(const Unit3& pc, //
OptionalJacobian<2, 2> Dpoint = boost::none);
/// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const;
/**
* Project point into the image
/** Project point into the image
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
@ -162,135 +192,32 @@ public:
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** Project point at infinity into the image
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Point2 project2(const Unit3& point,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
static Point3 backproject_from_camera(const Point2& p, const double depth);
#else
/// @}
/// @name Advanced interface
/// @{
public:
PinholeBase() {
/**
* Return the start and end indices (inclusive) of the translation component of the
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
}
explicit PinholeBase(const Pose3& pose) :
pose_(pose) {
}
explicit PinholeBase(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
const Pose3& pose() const {
return pose_;
}
/* ************************************************************************* */
static Matrix26 Dpose(const Point2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
return Dpn_pose;
}
/* ************************************************************************* */
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point;
Dpn_point << //
Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
/**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
Dpn_point *= d;
return Dpn_point;
}
/* ************************************************************************* */
static Pose3 LevelPose(const Pose2& pose2, double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
return Pose3(wRc, t);
}
/* ************************************************************************* */
static Pose3 LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector) {
Point3 zc = target - eye;
zc = zc / zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
xc = xc / xc.norm();
Point3 yc = zc.cross(xc);
return Pose3(Rot3(xc, yc, zc), eye);
}
/* ************************************************************************* */
bool equals(const PinholeBase &camera, double tol=1e-9) const {
return pose_.equals(camera.pose(), tol);
}
/* ************************************************************************* */
void print(const std::string& s) const {
pose_.print(s + ".pose");
}
/* ************************************************************************* */
const Pose3& getPose(OptionalJacobian<6, 6> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
}
return pose_;
}
/* ************************************************************************* */
static Point2 project_to_camera(const Point3& pc,
OptionalJacobian<2, 3> Dpoint = boost::none) {
double d = 1.0 / pc.z();
const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v);
}
/* ************************************************************************* */
std::pair<Point2, bool> projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw);
const Point2 pn = project_to_camera(pc);
return std::make_pair(pn, pc.z() > 0);
}
/* ************************************************************************* */
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0)
throw CheiralityException();
#endif
const Point2 pn = project_to_camera(q);
if (Dpose || Dpoint) {
const double d = 1.0 / q.z();
if (Dpose)
*Dpose = PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt);
}
return pn;
}
/* ************************************************************************* */
static Point3 backproject_from_camera(const Point2& p,
const double depth) {
return Point3(p.x() * depth, p.y() * depth, depth);
}
#endif
/// @}
private:
@ -392,7 +319,7 @@ public:
}
/// @}
/// @name Transformations and mesaurement functions
/// @name Transformations and measurement functions
/// @{
/**
@ -458,14 +385,16 @@ private:
/// @}
};
template<>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
};
// manifold traits
template <>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
template<>
struct traits<const CalibratedCamera> : public internal::Manifold<
CalibratedCamera> {
};
template <>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
}
// range traits, used in RangeFactor
template <typename T>
struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
} // namespace gtsam

266
gtsam/geometry/CameraSet.h
View File

@ -21,13 +21,14 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
#include <gtsam/base/Testable.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastMap.h>
#include <vector>
namespace gtsam {
/**
* @brief A set of cameras, all with their own calibration
* Assumes that a camera is laid out as 6 Pose3 parameters then calibration
*/
template<class CAMERA>
class CameraSet: public std::vector<CAMERA> {
@ -40,28 +41,46 @@ protected:
*/
typedef typename CAMERA::Measurement Z;
static const int D = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
/// Make a vector of re-projection errors
static Vector ErrorVector(const std::vector<Z>& predicted,
const std::vector<Z>& measured) {
// Check size
size_t m = predicted.size();
if (measured.size() != m)
throw std::runtime_error("CameraSet::errors: size mismatch");
// Project and fill error vector
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
Z e = predicted[i] - measured[i];
b.segment<ZDim>(row) = e.vector();
}
return b;
}
public:
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
typedef std::pair<Key, MatrixZD> FBlock; // Fblocks
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::vector<MatrixZD> FBlocks;
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "") const {
virtual void print(const std::string& s = "") const {
std::cout << s << "CameraSet, cameras = \n";
for (size_t k = 0; k < this->size(); ++k)
this->at(k).print();
this->at(k).print(s);
}
/// equals
virtual bool equals(const CameraSet& p, double tol = 1e-9) const {
bool equals(const CameraSet& p, double tol = 1e-9) const {
if (this->size() != p.size())
return false;
bool camerasAreEqual = true;
@ -74,31 +93,227 @@ public:
}
/**
* Project a point, with derivatives in this, point, and calibration
* Project a point (possibly Unit3 at infinity), with derivatives
* Note that F is a sparse block-diagonal matrix, so instead of a large dense
* matrix this function returns the diagonal blocks.
* throws CheiralityException
*/
std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F =
boost::none, boost::optional<Matrix&> E = boost::none,
boost::optional<Matrix&> H = boost::none) const {
template<class POINT>
std::vector<Z> project2(const POINT& point, //
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
size_t nrCameras = this->size();
if (F) F->resize(ZDim * nrCameras, 6);
if (E) E->resize(ZDim * nrCameras, 3);
if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6);
std::vector<Z> z(nrCameras);
static const int N = FixedDimension<POINT>::value;
for (size_t i = 0; i < nrCameras; i++) {
Eigen::Matrix<double, ZDim, 6> Fi;
Eigen::Matrix<double, ZDim, 3> Ei;
Eigen::Matrix<double, ZDim, Dim - 6> Hi;
z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
// Allocate derivatives
if (E)
E->resize(ZDim * m, N);
if (Fs)
Fs->resize(m);
// Project and fill derivatives
for (size_t i = 0; i < m; i++) {
MatrixZD Fi;
Eigen::Matrix<double, ZDim, N> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
if (Fs)
(*Fs)[i] = Fi;
if (E)
E->block<ZDim, N>(ZDim * i, 0) = Ei;
}
return z;
}
/// Calculate vector [project2(point)-z] of re-projection errors
template<class POINT>
Vector reprojectionError(const POINT& point, const std::vector<Z>& measured,
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
return ErrorVector(project2(point, Fs, E), measured);
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* Fixed size version
*/
template<int N> // N = 2 or 3
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i];
const Eigen::Matrix<double, 2, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian(i, m) = Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i, i) = Fi.transpose()
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi);
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i, j) = -Fi.transpose()
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj);
}
} // end of for over cameras
augmentedHessian(m, m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
/// Computes Point Covariance P, with lambda parameter
template<int N> // N = 2 or 3
static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
const Matrix& E, double lambda, bool diagonalDamping = false) {
Matrix EtE = E.transpose() * E;
if (diagonalDamping) { // diagonal of the hessian
EtE.diagonal() += lambda * EtE.diagonal();
} else {
DenseIndex n = E.cols();
EtE += lambda * Eigen::MatrixXd::Identity(n, n);
}
P = (EtE).inverse();
}
/// Computes Point Covariance P, with lambda parameter, dynamic version
static Matrix PointCov(const Matrix& E, const double lambda = 0.0,
bool diagonalDamping = false) {
if (E.cols() == 2) {
Matrix2 P2;
ComputePointCovariance(P2, E, lambda, diagonalDamping);
return P2;
} else {
Matrix3 P3;
ComputePointCovariance(P3, E, lambda, diagonalDamping);
return P3;
}
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* Dynamic version
*/
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
const Matrix& E, const Vector& b, const double lambda = 0.0,
bool diagonalDamping = false) {
SymmetricBlockMatrix augmentedHessian;
if (E.cols() == 2) {
Matrix2 P;
ComputePointCovariance(P, E, lambda, diagonalDamping);
augmentedHessian = SchurComplement(Fblocks, E, P, b);
} else {
Matrix3 P;
ComputePointCovariance(P, E, lambda, diagonalDamping);
augmentedHessian = SchurComplement(Fblocks, E, P, b);
}
return augmentedHessian;
}
/**
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
template<int N> // N = 2 or 3
static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
const Eigen::Matrix<double, N, N>& P, const Vector& b,
const FastVector<Key>& allKeys, const FastVector<Key>& keys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
assert(keys.size()==Fs.size());
assert(keys.size()<=allKeys.size());
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// Schur complement trick
// G = F' * F - F' * E * P * E' * F
// g = F' * (b - E * P * E' * b)
Eigen::Matrix<double, D, D> matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
// a single point is observed in m cameras
size_t m = Fs.size(); // cameras observing current point
size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
assert(allKeys.size()==M);
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera in the current factor
const MatrixZD& Fi = Fs[i];
const Eigen::Matrix<double, 2, N> Ei_P = E.template block<ZDim, N>(
ZDim * i, 0) * P;
// D = (DxZDim) * (ZDim)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i = this->keys_[i];
DenseIndex aug_i = KeySlotMap.at(keys[i]);
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal()
+ Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i, aug_i);
// add contribution of current factor
augmentedHessian(aug_i, aug_i) = matrixBlock
+ (Fi.transpose()
* (Fi - Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
DenseIndex aug_j = KeySlotMap.at(keys[j]);
// (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, aug_j) =
augmentedHessian(aug_i, aug_j).knownOffDiagonal()
- Fi.transpose()
* (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() * Fj);
}
} // end of for over cameras
augmentedHessian(M, M)(0, 0) += b.squaredNorm();
}
private:
/// Serialization function
@ -109,6 +324,9 @@ private:
}
};
template<class CAMERA>
const int CameraSet<CAMERA>::D;
template<class CAMERA>
const int CameraSet<CAMERA>::ZDim;

29
gtsam/geometry/Cyclic.h
View File

@ -16,10 +16,8 @@
**/
#include <gtsam/base/Group.h>
#include <cstddef>
#include <string>
#include <iostream>
#include <assert.h>
#include <gtsam/base/Testable.h>
#include <iostream> // for cout :-(
namespace gtsam {
@ -33,10 +31,11 @@ public:
i_(i) {
assert(i < N);
}
/// Idenity element
static Cyclic Identity() {
return Cyclic(0);
/// Default constructor yields identity
Cyclic():i_(0) {
}
static Cyclic identity() { return Cyclic();}
/// Cast to size_t
operator size_t() const {
return i_;
@ -63,20 +62,10 @@ public:
}
};
/// Define cyclic group traits to be a model of the Group concept
/// Define cyclic group to be a model of the Additive Group concept
template<size_t N>
struct traits<Cyclic<N> > {
typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic<N>)
static Cyclic<N> Identity() {
return Cyclic<N>::Identity();
}
static bool Equals(const Cyclic<N>& a, const Cyclic<N>& b,
double tol = 1e-9) {
return a.equals(b, tol);
}
static void Print(const Cyclic<N>& c, const std::string &s = "") {
c.print(s);
}
struct traits<Cyclic<N> > : internal::AdditiveGroupTraits<Cyclic<N> >, //
Testable<Cyclic<N> > {
};
} // \namespace gtsam

54
gtsam/geometry/EssentialMatrix.cpp
View File

@ -13,64 +13,46 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
OptionalJacobian<5, 6> H) {
const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation();
const Rot3& aRb = aPb.rotation();
const Point3& aTb = aPb.translation();
if (!H) {
// just make a direction out of translation and create E
Unit3 direction(_1T2_);
return EssentialMatrix(_1R2_, direction);
Unit3 direction(aTb);
return EssentialMatrix(aRb, direction);
} else {
// Calculate the 5*6 Jacobian H = D_E_1P2
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
// First get 2*3 derivative from Unit3::FromPoint3
Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2);
*H << I_3x3, Z_3x3, //
Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
return EssentialMatrix(_1R2_, direction);
Matrix23::Zero(), D_direction_1T2 * aRb.matrix();
return EssentialMatrix(aRb, direction);
}
}
/* ************************************************************************* */
void EssentialMatrix::print(const string& s) const {
cout << s;
aRb_.print("R:\n");
aTb_.print("d: ");
}
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
assert(xi.size() == 5);
Vector3 omega(sub(xi, 0, 3));
Vector2 z(sub(xi, 3, 5));
Rot3 R = aRb_.retract(omega);
Unit3 t = aTb_.retract(z);
return EssentialMatrix(R, t);
}
/* ************************************************************************* */
Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
Vector5 v;
v << aRb_.localCoordinates(other.aRb_),
aTb_.localCoordinates(other.aTb_);
return v;
rotation().print("R:\n");
direction().print("d: ");
}
/* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
OptionalJacobian<3, 3> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3());
Pose3 pose(rotation(), direction().point3());
Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix35 H;
H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis();
*DE = H;
}
return q;
@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
Rot3 c1Rc2 = rotation().conjugate(cRb);
if (!HE && !HR) {
// Rotate translation direction and return
Unit3 c1Tc2 = cRb * aTb_;
Unit3 c1Tc2 = cRb * direction();
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
Matrix23 D_c1Tc2_cRb; // 2*3
Matrix2 D_c1Tc2_aTb; // 2*2
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE)
*HE << cRb.matrix(), Matrix32::Zero(), //
Matrix23::Zero(), D_c1Tc2_aTb;
@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
if (H) {
// See math.lyx
Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
* direction().basis();
*H << HR, HD;
}
return dot(vA, E_ * vB);

55
gtsam/geometry/EssentialMatrix.h
View File

@ -10,6 +10,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Manifold.h>
#include <iosfwd>
namespace gtsam {
@ -20,17 +21,18 @@ namespace gtsam {
* but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
*/
class GTSAM_EXPORT EssentialMatrix {
class GTSAM_EXPORT EssentialMatrix : private ProductManifold<Rot3, Unit3> {
private:
Rot3 aRb_; ///< Rotation between a and b
Unit3 aTb_; ///< translation direction from a to b
typedef ProductManifold<Rot3, Unit3> Base;
Matrix3 E_; ///< Essential matrix
public:
/// Construct from Base
EssentialMatrix(const Base& base) :
Base(base), E_(direction().skew() * rotation().matrix()) {
}
enum { dimension = 5 };
public:
/// Static function to convert Point2 to homogeneous coordinates
static Vector3 Homogeneous(const Point2& p) {
@ -42,12 +44,12 @@ public:
/// Default constructor
EssentialMatrix() :
aTb_(1, 0, 0), E_(aTb_.skew()) {
Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
}
/// Construct from rotation and translation
EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
}
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
@ -72,7 +74,8 @@ public:
/// assert equality up to a tolerance
bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
return rotation().equals(other.rotation(), tol)
&& direction().equals(other.direction(), tol);
}
/// @}
@ -80,22 +83,19 @@ public:
/// @name Manifold
/// @{
/// Dimensionality of tangent space = 5 DOF
inline static size_t Dim() {
return 5;
}
/// Return the dimensionality of the tangent space
size_t dim() const {
return 5;
}
using Base::dimension;
using Base::dim;
using Base::Dim;
/// Retract delta to manifold
EssentialMatrix retract(const Vector& xi) const;
EssentialMatrix retract(const TangentVector& v) const {
return Base::retract(v);
}
/// Compute the coordinates in the tangent space
Vector5 localCoordinates(const EssentialMatrix& other) const;
TangentVector localCoordinates(const EssentialMatrix& other) const {
return Base::localCoordinates(other);
}
/// @}
/// @name Essential matrix methods
@ -103,12 +103,12 @@ public:
/// Rotation
inline const Rot3& rotation() const {
return aRb_;
return this->first;
}
/// Direction
inline const Unit3& direction() const {
return aTb_;
return this->second;
}
/// Return 3*3 matrix representation
@ -118,12 +118,12 @@ public:
/// Return epipole in image_a , as Unit3 to allow for infinity
inline const Unit3& epipole_a() const {
return aTb_; // == direction()
return direction();
}
/// Return epipole in image_b, as Unit3 to allow for infinity
inline Unit3 epipole_b() const {
return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
return rotation().unrotate(direction());
}
/**
@ -180,8 +180,8 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(aRb_);
ar & BOOST_SERIALIZATION_NVP(aTb_);
ar & BOOST_SERIALIZATION_NVP(first);
ar & BOOST_SERIALIZATION_NVP(second);
ar & boost::serialization::make_nvp("E11", E_(0,0));
ar & boost::serialization::make_nvp("E12", E_(0,1));
@ -195,7 +195,6 @@ private:
}
/// @}
};
template<>

70
gtsam/geometry/OrientedPlane3.cpp
View File

@ -13,6 +13,7 @@
* @file OrientedPlane3.cpp
* @date Dec 19, 2013
* @author Alex Trevor
* @author Zhaoyang Lv
* @brief A plane, represented by a normal direction and perpendicular distance
*/
@ -25,79 +26,54 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
/// The print fuction
void OrientedPlane3::print(const std::string& s) const {
Vector coeffs = planeCoefficients();
void OrientedPlane3::print(const string& s) const {
Vector4 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);
OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp,
OptionalJacobian<3, 6> Hr) const {
Matrix23 D_rotated_plane;
Matrix22 D_rotated_pose;
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
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);
Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
if (Hr) {
*Hr = gtsam::zeros(3, 6);
(*Hr).block<2, 3>(0, 0) = n_hr;
(*Hr).block<1, 3>(2, 3) = unit_vec;
*Hr = zeros(3, 6);
Hr->block<2, 3>(0, 0) = D_rotated_plane;
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;
Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp;
(*Hp)(2, 2) = 1;
}
return (transformed_plane);
return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
Vector3 OrientedPlane3::error(const 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);
return Vector3(n_error(0), n_error(1), d_ - plane.d_);
}
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& 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);
return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));
}
/* ************************************************************************* */
Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
Vector2 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;
return Vector3(n_local(0), n_local(1), -d_local);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::planeCoefficients() const {
Vector unit_vec = n_.unitVector();
Vector3 a;
a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
return a;
}
/* ************************************************************************* */
}

83
gtsam/geometry/OrientedPlane3.h
View File

@ -14,6 +14,7 @@
* @date Dec 19, 2013
* @author Alex Trevor
* @author Frank Dellaert
* @author Zhaoyang Lv
* @brief An infinite plane, represented by a normal direction and perpendicular distance
*/
@ -22,46 +23,56 @@
#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> {
/**
* @brief Represents an infinite plane in 3D, which is composed of a planar normal and its
* perpendicular distance to the origin.
* Currently it provides a transform of the plane, and a norm 1 differencing of two planes.
* Refer to Trevor12iros for more math details.
*/
class GTSAM_EXPORT OrientedPlane3 {
private:
Unit3 n_; /// The direction of the planar normal
double d_; /// The perpendicular distance to this plane
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) {
n_(), d_(0.0) {
}
/// Construct from a Unit3 and a distance
OrientedPlane3(const Unit3& s, double d) :
n_(s), d_(d) {
n_(s), d_(d) {
}
/// Construct from a vector of plane coefficients
OrientedPlane3(const Vector4& vec) :
n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
}
/// Construct from a, b, c, d
/// Construct from four numbers of plane coeffcients (a, b, c, d)
OrientedPlane3(double a, double b, double c, double d) {
Point3 p(a, b, c);
n_ = Unit3(p);
d_ = d;
}
/// @}
/// @name Testable
/// @{
/// The print function
void print(const std::string& s = std::string()) const;
@ -70,13 +81,38 @@ public:
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;
/** Transforms a plane to the specified pose
* @param xr a transformation in current coordiante
* @param Hp optional Jacobian wrpt the destination plane
* @param Hr optional jacobian wrpt the pose transformation
* @return the transformed plane
*/
OrientedPlane3 transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
/**
* @deprecated the static method has wrong Jacobian order,
* please use the member method transform()
* @param The raw plane
* @param xr a transformation in current coordiante
* @param Hr optional jacobian wrpt the pose transformation
* @param Hp optional Jacobian wrpt the destination plane
* @return the transformed plane
*/
static OrientedPlane3 Transform(const OrientedPlane3& plane,
const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
OptionalJacobian<3, 3> Hp = boost::none) {
return plane.transform(xr, Hp, Hr);
}
/** Computes the error between two planes.
* The error is a norm 1 difference in tangent space.
* @param the other plane
*/
Vector3 error(const OrientedPlane3& plane) const;
/// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() {
@ -94,22 +130,31 @@ public:
/// The local coordinates function
Vector3 localCoordinates(const OrientedPlane3& s) const;
/// Returns the plane coefficients (a, b, c, d)
Vector3 planeCoefficients() const;
/// Returns the plane coefficients
inline Vector4 planeCoefficients() const {
Vector3 unit_vec = n_.unitVector();
return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
}
/// Return the normal
inline Unit3 normal() const {
return n_;
}
/// Return the perpendicular distance to the origin
inline double distance() const {
return d_;
}
/// @}
};
template<> struct traits<OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
OrientedPlane3> {
};
template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
OrientedPlane3> {
};
} // namespace gtsam

159
gtsam/geometry/PinholeCamera.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/PinholePose.h>
#include <gtsam/geometry/BearingRange.h>
namespace gtsam {
@ -31,14 +32,6 @@ namespace gtsam {
template<typename Calibration>
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
public:
/**
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Point2 Measurement;
private:
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
@ -153,7 +146,7 @@ public:
const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
H->template block<6, 6>(0, 0) = I_6x6;
}
return Base::pose();
}
@ -184,16 +177,15 @@ public:
if ((size_t) d.size() == 6)
return PinholeCamera(this->pose().retract(d), calibration());
else
return PinholeCamera(this->pose().retract(d.head(6)),
return PinholeCamera(this->pose().retract(d.head<6>()),
calibration().retract(d.tail(calibration().dim())));
}
/// return canonical coordinate
VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d;
// TODO: why does d.head<6>() not compile??
d.head(6) = this->pose().localCoordinates(T2.pose());
d.tail(DimK) = calibration().localCoordinates(T2.calibration());
d.template head<6>() = this->pose().localCoordinates(T2.pose());
d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
return d;
}
@ -208,101 +200,34 @@ public:
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
/** Templated projection of a 3D point or a point at infinity into the image
* @param pw either a Point3 or a Unit3, in world coordinates
*/
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
return pi;
}
/** project a point at infinity from world coordinate to the image
* @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf)
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
return K_.uncalibrate(pn);
}
// world to camera coordinate
Matrix3 Dpc_rot, Dpc_point;
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
Matrix36 Dpc_pose;
Dpc_pose.setZero();
Dpc_pose.leftCols<3>() = Dpc_rot;
// camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3
const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix2 Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose)
*Dpose = Dpi_pc * Dpc_pose;
if (Dpoint)
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
return pi;
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
*/
Point2 project2(
const Point3& pw, //
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
// project to normalized coordinates
template<class POINT>
Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
// We just call 3-derivative version in Base
Matrix26 Dpose;
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
Dcamera || Dpoint ? &Dpi_pn : 0);
// If needed, calculate derivatives
Eigen::Matrix<double, 2, DimK> Dcal;
Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
Dcamera ? &Dcal : 0);
if (Dcamera)
*Dcamera << Dpi_pn * Dpose, Dcal;
if (Dpoint)
*Dpoint = Dpi_pn * (*Dpoint);
*Dcamera << Dpose, Dcal;
return pi;
}
/// project a 3D point from world coordinates into the image
Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
/// project a point at infinity from world coordinates into the image
Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
@ -339,24 +264,22 @@ public:
template<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
boost::optional<Matrix&> Dother = boost::none) const {
OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
Matrix16 Dcamera_, Dother_;
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
if (Dcamera) {
Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero();
Dother->block(0, 0, 1, 6) = Dother_;
Dother->template block<1, 6>(0, 0) = Dother_;
}
return result;
}
/**
* Calculate range to another camera
* Calculate range to a calibrated camera
* @param camera Other camera
* @return range (double)
*/
@ -380,14 +303,18 @@ private:
};
template<typename Calibration>
struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
// manifold traits
template<typename Calibration>
struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
template <typename Calibration>
struct traits<PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
} // \ gtsam
template <typename Calibration>
struct traits<const PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
// range traits, used in RangeFactor
template <typename Calibration, typename T>
struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
} // \ gtsam

123
gtsam/geometry/PinholePose.h
View File

@ -30,14 +30,19 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
template<typename CALIBRATION>
class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
private:
public :
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
typedef Calibration CalibrationType;
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<CALIBRATION>::value;
public:
typedef CALIBRATION CalibrationType;
/// @name Standard Constructors
/// @{
@ -67,7 +72,7 @@ public :
}
/// return calibration
virtual const Calibration& calibration() const = 0;
virtual const CALIBRATION& calibration() const = 0;
/// @}
/// @name Transformations and measurement functions
@ -80,27 +85,65 @@ public :
return pn;
}
/** project a point from world coordinate to the image, fixed Jacobians
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinates
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
Point2 project(const Point3& pw) const {
const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** project a point from world coordinate to the image
* @param pw is a point at infinity in the world coordinates
*/
Point2 project(const Unit3& pw) const {
const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** Templated projection of a point (possibly at infinity) from world coordinate to the image
* @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
template <class POINT>
Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
OptionalJacobian<2, DimK> Dcal) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, boost::none,
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose) *Dpose = Dpi_pn * (*Dpose);
if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
return pi;
}
/// project a 3D point from world coordinates into the image
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// project a point at infinity from world coordinates into the image
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = calibration().calibrate(p);
@ -108,9 +151,9 @@ public :
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
Point3 backprojectPointAtInfinity(const Point2& p) const {
Unit3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p);
const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose().rotation().rotate(pc);
}
@ -178,13 +221,13 @@ private:
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class GTSAM_EXPORT PinholePose: public PinholeBaseK<Calibration> {
template<typename CALIBRATION>
class GTSAM_EXPORT PinholePose: public PinholeBaseK<CALIBRATION> {
private:
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
boost::shared_ptr<Calibration> K_; ///< shared pointer to fixed calibration
typedef PinholeBaseK<CALIBRATION> Base; ///< base class has 3D pose as private member
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to fixed calibration
public:
@ -201,11 +244,11 @@ public:
/** constructor with pose, uses default calibration */
explicit PinholePose(const Pose3& pose) :
Base(pose), K_(new Calibration()) {
Base(pose), K_(new CALIBRATION()) {
}
/** constructor with pose and calibration */
PinholePose(const Pose3& pose, const boost::shared_ptr<Calibration>& K) :
PinholePose(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& K) :
Base(pose), K_(K) {
}
@ -220,14 +263,14 @@ public:
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static PinholePose Level(const boost::shared_ptr<Calibration>& K,
static PinholePose Level(const boost::shared_ptr<CALIBRATION>& K,
const Pose2& pose2, double height) {
return PinholePose(Base::LevelPose(pose2, height), K);
}
/// PinholePose::level with default calibration
static PinholePose Level(const Pose2& pose2, double height) {
return PinholePose::Level(boost::make_shared<Calibration>(), pose2, height);
return PinholePose::Level(boost::make_shared<CALIBRATION>(), pose2, height);
}
/**
@ -240,8 +283,8 @@ public:
* @param K optional calibration parameter
*/
static PinholePose Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const boost::shared_ptr<Calibration>& K =
boost::make_shared<Calibration>()) {
const Point3& upVector, const boost::shared_ptr<CALIBRATION>& K =
boost::make_shared<CALIBRATION>()) {
return PinholePose(Base::LookatPose(eye, target, upVector), K);
}
@ -251,12 +294,12 @@ public:
/// Init from 6D vector
explicit PinholePose(const Vector &v) :
Base(v), K_(new Calibration()) {
Base(v), K_(new CALIBRATION()) {
}
/// Init from Vector and calibration
PinholePose(const Vector &v, const Vector &K) :
Base(v), K_(new Calibration(K)) {
Base(v), K_(new CALIBRATION(K)) {
}
/// @}
@ -286,10 +329,26 @@ public:
}
/// return calibration
virtual const Calibration& calibration() const {
virtual const CALIBRATION& calibration() const {
return *K_;
}
/** project a point from world coordinate to the image, 2 derivatives only
* @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose)
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}
/// project2 version for point at infinity
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}
/// @}
/// @name Manifold
/// @{
@ -336,14 +395,14 @@ private:
};
// end of class PinholePose
template<typename Calibration>
struct traits<PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
template<typename CALIBRATION>
struct traits<PinholePose<CALIBRATION> > : public internal::Manifold<
PinholePose<CALIBRATION> > {
};
template<typename Calibration>
struct traits<const PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
template<typename CALIBRATION>
struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
PinholePose<CALIBRATION> > {
};
} // \ gtsam

85
gtsam/geometry/PinholeSet.h Normal file
View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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 PinholeSet.h
* @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/triangulation.h>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
namespace gtsam {
/**
* PinholeSet: triangulates point and keeps an estimate of it around.
*/
template<class CAMERA>
class PinholeSet: public CameraSet<CAMERA> {
private:
typedef CameraSet<CAMERA> Base;
typedef PinholeSet<CAMERA> This;
protected:
public:
/** Virtual destructor */
virtual ~PinholeSet() {
}
/// @name Testable
/// @{
/// print
virtual void print(const std::string& s = "") const {
Base::print(s);
}
/// equals
bool equals(const PinholeSet& p, double tol = 1e-9) const {
return Base::equals(p, tol); // TODO all flags
}
/// @}
/// triangulateSafe
TriangulationResult triangulateSafe(
const std::vector<Point2>& measured,
const TriangulationParameters& params) const {
return gtsam::triangulateSafe(*this, measured, params);
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
template<class CAMERA>
struct traits<PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
};
template<class CAMERA>
struct traits<const PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
};
} // \ namespace gtsam

2
gtsam/geometry/Point2.h
View File

@ -148,7 +148,7 @@ public:
/// @{
/// equality
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;}
/// get x
double x() const {return x_;}

18
gtsam/geometry/Point3.h
View File

@ -22,6 +22,7 @@
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
@ -199,4 +200,19 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
}
template <typename A1, typename A2>
struct Range;
template <>
struct Range<Point3, Point3> {
typedef double result_type;
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return p.distance(q, H1, H2);
}
};
} // namespace gtsam

4
gtsam/geometry/Pose2.cpp
View File

@ -59,7 +59,7 @@ bool Pose2::equals(const Pose2& q, double tol) const {
}
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) {
Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) {
if (H) *H = Pose2::ExpmapDerivative(xi);
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
@ -130,7 +130,7 @@ Matrix3 Pose2::AdjointMap() const {
}
/* ************************************************************************* */
Matrix3 Pose2::adjointMap(const Vector& v) {
Matrix3 Pose2::adjointMap(const Vector3& v) {
// See Chirikjian12book2, vol.2, pg. 36
Matrix3 ad = zeros(3,3);
ad(0,1) = -v[2];

24
gtsam/geometry/Pose2.h
View File

@ -20,9 +20,11 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
namespace gtsam {
@ -118,7 +120,7 @@ public:
/// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none);
static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
@ -128,15 +130,14 @@ public:
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
*/
Matrix3 AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {
assert(xi.size() == 3);
inline Vector3 Adjoint(const Vector3& xi) const {
return AdjointMap()*xi;
}
/**
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
*/
static Matrix3 adjointMap(const Vector& v);
static Matrix3 adjointMap(const Vector3& v);
/**
* wedge for SE(2):
@ -290,11 +291,18 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
template<>
struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
template<>
struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
template <>
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
// bearing and range traits, used in RangeFactor
template <typename T>
struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
template <typename T>
struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
} // namespace gtsam

104
gtsam/geometry/Pose3.cpp
View File

@ -32,7 +32,7 @@ GTSAM_CONCEPT_POSE_INST(Pose3);
/* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_(
Point3(pose2.x(), pose2.y(), 0)) {
}
@ -111,25 +111,21 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) {
if (H) {
*H = ExpmapDerivative(xi);
}
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) *H = ExpmapDerivative(xi);
// get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
double theta = w.norm();
if (theta < 1e-10) {
static const Rot3 I;
return Pose3(I, v);
} else {
Point3 n(w / theta); // axis unit vector
Rot3 R = Rot3::rodriguez(n.vector(), theta);
double vn = n.dot(v); // translation parallel to n
Point3 n_cross_v = n.cross(v); // points towards axis
Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n;
Rot3 R = Rot3::Expmap(omega.vector());
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
double omega_v = omega.dot(v); // translation parallel to axis
Point3 omega_cross_v = omega.cross(v); // points towards axis
Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2;
return Pose3(R, t);
} else {
return Pose3(R, v);
}
}
@ -318,35 +314,47 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
/* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const {
OptionalJacobian<1, 3> H2) const {
Matrix36 D_local_pose;
Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) {
return transform_to(point).norm();
return local.norm();
} else {
Matrix36 D1;
Matrix3 D2;
Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
n = sqrt(d2);
Matrix13 D_result_d;
D_result_d << x / n, y / n, z / n;
if (H1) *H1 = D_result_d * D1;
if (H2) *H2 = D_result_d * D2;
return n;
Matrix13 D_r_local;
const double r = local.norm(D_r_local);
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;
}
}
/* ************************************************************************* */
double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
OptionalJacobian<1,6> H2) const {
Matrix13 D2;
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) {
Matrix13 H2_ = D2 * pose.rotation().matrix();
*H2 << Matrix13::Zero(), H2_;
}
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 6> H2) const {
Matrix13 D_local_point;
double r = range(pose.translation(), H1, H2 ? &D_local_point : 0);
if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
return r;
}
/* ************************************************************************* */
Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
OptionalJacobian<2, 3> H2) const {
Matrix36 D_local_pose;
Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) {
return Unit3(local);
} else {
Matrix23 D_b_local;
Unit3 b = Unit3::FromPoint3(local, D_b_local);
if (H1) *H1 = D_b_local * D_local_pose;
if (H2) *H2 = D_b_local * D_local_point;
return b;
}
}
/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
const size_t n = pairs.size();
@ -354,25 +362,25 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
return boost::none; // we need at least three pairs
// calculate centroids
Vector cp = zero(3), cq = zero(3);
BOOST_FOREACH(const Point3Pair& pair, pairs){
cp += pair.first.vector();
cq += pair.second.vector();
}
Vector3 cp = Vector3::Zero(), cq = Vector3::Zero();
BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first.vector();
cq += pair.second.vector();
}
double f = 1.0 / n;
cp *= f;
cq *= f;
// Add to form H matrix
Matrix3 H = Eigen::Matrix3d::Zero();
BOOST_FOREACH(const Point3Pair& pair, pairs){
Vector dp = pair.first.vector() - cp;
Vector dq = pair.second.vector() - cq;
H += dp * dq.transpose();
}
Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) {
Vector3 dp = pair.first.vector() - cp;
Vector3 dq = pair.second.vector() - cq;
H += dp * dq.transpose();
}
// Compute SVD
Matrix U,V;
Matrix U, V;
Vector S;
svd(H, U, S, V);

29
gtsam/geometry/Pose3.h
View File

@ -19,6 +19,7 @@
#include <gtsam/config.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie.h>
@ -110,7 +111,7 @@ public:
/// @{
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none);
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none);
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
@ -125,7 +126,7 @@ public:
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
*/
Vector Adjoint(const Vector& xi_b) const {
Vector6 Adjoint(const Vector6& xi_b) const {
return AdjointMap() * xi_b;
} /// FIXME Not tested - marked as incorrect
@ -262,6 +263,14 @@ public:
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const;
/**
* Calculate bearing to a landmark
* @param point 3D location of landmark
* @return bearing (Unit3)
*/
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
OptionalJacobian<2, 3> H2 = boost::none) const;
/// @}
/// @name Advanced Interface
/// @{
@ -323,11 +332,17 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;
template<>
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
template <>
struct traits<Pose3> : public internal::LieGroup<Pose3> {};
template<>
struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
// bearing and range traits, used in RangeFactor
template <>
struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
} // namespace gtsam
template <typename T>
struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
} // namespace gtsam

31
gtsam/geometry/Quaternion.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
#include <limits>
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -73,14 +74,22 @@ struct traits<QUATERNION_TYPE> {
return g.inverse();
}
/// Exponential map, simply be converting omega to axis/angle representation
/// Exponential map, using the inlined code from Eigen's conversion from axis/angle
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) {
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));
ChartJacobian H = boost::none) {
using std::cos;
using std::sin;
if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
_Scalar theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
_Scalar theta = std::sqrt(theta2);
_Scalar ha = _Scalar(0.5) * theta;
Vector3 vec = (sin(ha) / theta) * omega;
return Q(cos(ha), vec.x(), vec.y(), vec.z());
} else {
// first order approximation sin(theta/2)/theta = 0.5
Vector3 vec = _Scalar(0.5) * omega;
return Q(1.0, vec.x(), vec.y(), vec.z());
}
}
@ -93,9 +102,9 @@ struct traits<QUATERNION_TYPE> {
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10;
Vector3 omega;
TangentVector omega;
const double qw = q.w();
const _Scalar qw = q.w();
// See Quaternion-Logmap.nb in doc for Taylor expansions
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
@ -107,7 +116,7 @@ struct traits<QUATERNION_TYPE> {
omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
_Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
@ -116,7 +125,7 @@ struct traits<QUATERNION_TYPE> {
omega = (angle / s) * q.vec();
}
if(H) *H = SO3::LogmapDerivative(omega);
if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
return omega;
}

4
gtsam/geometry/Rot2.h
View File

@ -208,9 +208,9 @@ namespace gtsam {
};
template<>
struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
struct traits<Rot2> : public internal::LieGroup<Rot2> {};
template<>
struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
} // gtsam

25
gtsam/geometry/Rot3.cpp
View File

@ -34,29 +34,12 @@ void Rot3::print(const std::string& s) const {
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
return rodriguez((Vector)w.vector(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Unit3& w, double theta) {
return rodriguez(w.point3(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::Random(boost::mt19937 & rng) {
Rot3 Rot3::Random(boost::mt19937& rng) {
// TODO allow any engine without including all of boost :-(
Unit3 w = Unit3::Random(rng);
boost::uniform_real<double> randomAngle(-M_PI,M_PI);
Unit3 axis = Unit3::Random(rng);
boost::uniform_real<double> randomAngle(-M_PI, M_PI);
double angle = randomAngle(rng);
return rodriguez(w,angle);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w) {
double t = w.norm();
if (t < 1e-10) return Rot3();
return rodriguez(w/t, t);
return AxisAngle(axis, angle);
}
/* ************************************************************************* */

84
gtsam/geometry/Rot3.h
View File

@ -22,8 +22,9 @@
#pragma once
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
@ -140,7 +141,7 @@ namespace gtsam {
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf.
* Assumes vehicle coordinate frame X forward, Y right, Z down.
*/
static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion coefficients */
static Rot3 quaternion(double w, double x, double y, double z) {
@ -149,45 +150,58 @@ namespace gtsam {
}
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
* Convert from axis/angle representation
* @param axis is the rotation axis, unit length
* @param angle rotation angle
* @return incremental rotation
*/
static Rot3 rodriguez(const Vector3& w, double theta);
static Rot3 AxisAngle(const Vector3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return SO3::AxisAngle(axis,angle);
#endif
}
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
* Convert from axis/angle representation
* @param axisw is the rotation axis, unit length
* @param angle rotation angle
* @return incremental rotation
*/
static Rot3 rodriguez(const Point3& w, double theta);
static Rot3 AxisAngle(const Point3& axis, double angle) {
return AxisAngle(axis.vector(),angle);
}
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis
* @param theta rotation angle
* @return incremental rotation matrix
* Convert from axis/angle representation
* @param axis is the rotation axis
* @param angle rotation angle
* @return incremental rotation
*/
static Rot3 rodriguez(const Unit3& w, double theta);
static Rot3 AxisAngle(const Unit3& axis, double angle) {
return AxisAngle(axis.unitVector(),angle);
}
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
* Rodrigues' formula to compute an incremental rotation
* @param w a vector of incremental roll,pitch,yaw
* @return incremental rotation
*/
static Rot3 rodriguez(const Vector3& v);
static Rot3 Rodrigues(const Vector3& w) {
return Rot3::Expmap(w);
}
/**
* Rodriguez' formula to compute an incremental rotation matrix
* Rodrigues' formula to compute an incremental rotation
* @param wx Incremental roll (about X)
* @param wy Incremental pitch (about Y)
* @param wz Incremental yaw (about Z)
* @return incremental rotation matrix
* @return incremental rotation
*/
static Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector3(wx, wy, wz));}
static Rot3 Rodrigues(double wx, double wy, double wz) {
return Rodrigues(Vector3(wx, wy, wz));
}
/// @}
/// @name Testable
@ -272,11 +286,15 @@ namespace gtsam {
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) {
if(H) *H = Rot3::ExpmapDerivative(v);
if (zero(v)) return Rot3(); else return rodriguez(v);
#ifdef GTSAM_USE_QUATERNIONS
return traits<Quaternion>::Expmap(v);
#else
return traits<SO3>::Expmap(v);
#endif
}
/**
@ -422,6 +440,14 @@ namespace gtsam {
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
/// @}
/// @name Deprecated
/// @{
static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }
static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); }
/// @}
private:
@ -463,9 +489,9 @@ namespace gtsam {
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
template<>
struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
}

5
gtsam/geometry/Rot3M.cpp
View File

@ -117,11 +117,6 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return SO3::Rodrigues(w,theta);
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_));

4
gtsam/geometry/Rot3Q.cpp
View File

@ -83,10 +83,6 @@ namespace gtsam {
Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return Quaternion(Eigen::AngleAxis<double>(theta, w));
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);

94
gtsam/geometry/SO3.cpp
View File

@ -19,47 +19,69 @@
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <cmath>
using namespace std;
#include <limits>
namespace gtsam {
/* ************************************************************************* */
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
using std::cos;
using std::sin;
// get components of axis \omega
double wx = axis(0), wy = axis(1), wz = axis(2);
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz;
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;
// Near zero, we just have I + skew(omega)
static SO3 FirstOrder(const Vector3& omega) {
Matrix3 R;
R << c + C00, -swz + C01, swy + C02, //
swz + C01, c + C11, -swx + C12, //
-swy + C02, swx + C12, c + C22;
R(0, 0) = 1.;
R(1, 0) = omega.z();
R(2, 0) = -omega.y();
R(0, 1) = -omega.z();
R(1, 1) = 1.;
R(2, 1) = omega.x();
R(0, 2) = omega.y();
R(1, 2) = -omega.x();
R(2, 2) = 1.;
return R;
}
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
if (theta*theta > std::numeric_limits<double>::epsilon()) {
using std::cos;
using std::sin;
// get components of axis \omega, where is a unit vector
const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
wz_sintheta = wz * sintheta;
const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
const double C22 = c_1 * wz * wz;
Matrix3 R;
R(0, 0) = costheta + C00;
R(1, 0) = wz_sintheta + C01;
R(2, 0) = -wy_sintheta + C02;
R(0, 1) = -wz_sintheta + C01;
R(1, 1) = costheta + C11;
R(2, 1) = wx_sintheta + C12;
R(0, 2) = wy_sintheta + C02;
R(1, 2) = -wx_sintheta + C12;
R(2, 2) = costheta + C22;
return R;
} else {
return FirstOrder(axis*theta);
}
}
/// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Vector3& omega,
ChartJacobian H) {
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) *H = ExpmapDerivative(omega);
if (H)
*H = ExpmapDerivative(omega);
if (omega.isZero())
return Identity();
else {
double angle = omega.norm();
return Rodrigues(omega / angle, angle);
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
double theta = std::sqrt(theta2);
return AxisAngle(omega / theta, theta);
} else {
return FirstOrder(omega);
}
}
@ -111,8 +133,9 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm(); // rotation angle
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
@ -142,8 +165,9 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm();
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);

8
gtsam/geometry/SO3.h
View File

@ -59,7 +59,7 @@ public:
}
/// Static, named constructor TODO think about relation with above
static SO3 Rodrigues(const Vector3& axis, double theta);
static SO3 AxisAngle(const Vector3& axis, double theta);
/// @}
/// @name Testable
@ -93,7 +93,7 @@ public:
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
@ -129,11 +129,11 @@ public:
};
template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {
struct traits<SO3> : public internal::LieGroup<SO3> {
};
template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
struct traits<const SO3> : public internal::LieGroup<SO3> {
};
} // end namespace gtsam

23
gtsam/geometry/SimpleCamera.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
@ -125,14 +126,18 @@ public:
};
template<>
struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {
};
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
template<>
struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {
};
// manifold traits
template <>
struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {};
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
}
template <>
struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
// range traits, used in RangeFactor
template <typename T>
struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
} // namespace gtsam

38
gtsam/geometry/StereoCamera.cpp
View File

@ -91,8 +91,42 @@ namespace gtsam {
double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
double X = Z * (measured[0] - K_->px()) / K_->fx();
double Y = Z * (measured[2] - K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
Point3 point = leftCamPose_.transform_from(Point3(X, Y, Z));
return point;
}
/* ************************************************************************* */
Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2) const {
const Cal3_S2Stereo& K = *K_;
const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline();
double uL = z.uL(), uR = z.uR(), v = z.v();
double disparity = uL - uR;
double local_z = b * fx / disparity;
const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z);
if(H1 || H2) {
double z_partial_uR = local_z/disparity;
double x_partial_uR = local.x()/disparity;
double y_partial_uR = local.y()/disparity;
Matrix3 D_local_z;
D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0,
-y_partial_uR, y_partial_uR, local.z() / fy,
-z_partial_uR, z_partial_uR, 0;
Matrix3 D_point_local;
const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local);
if(H2) {
*H2 = D_point_local * D_local_z;
}
return point;
}
return leftCamPose_.transform_from(local);
}
}

8
gtsam/geometry/StereoCamera.h
View File

@ -137,6 +137,14 @@ public:
/// back-project a measurement
Point3 backproject(const StereoPoint2& z) const;
/** Back-project the 2D point and compute optional derivatives
* @param H1 derivative with respect to pose
* @param H2 derivative with respect to point
*/
Point3 backproject2(const StereoPoint2& z,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// @}
/// @name Deprecated
/// @{

230
gtsam/geometry/StereoPoint2.h
View File

@ -18,165 +18,145 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
/**
* A 2D stereo point, v will be same for rectified images
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT StereoPoint2 {
public:
static const size_t dimension = 3;
private:
double uL_, uR_, v_;
/**
* A 2D stereo point, v will be same for rectified images
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT StereoPoint2 {
private:
public:
double uL_, uR_, v_;
/// @name Standard Constructors
/// @{
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
/** default constructor */
StereoPoint2() :
/** default constructor */
StereoPoint2() :
uL_(0), uR_(0), v_(0) {
}
}
/** constructor */
StereoPoint2(double uL, double uR, double v) :
/** constructor */
StereoPoint2(double uL, double uR, double v) :
uL_(uL), uR_(uR), v_(v) {
}
}
/// @}
/// @name Testable
/// @{
/// construct from 3D vector
StereoPoint2(const Vector3& v) :
uL_(v(0)), uR_(v(1)), v_(v(2)) {}
/** print */
void print(const std::string& s="") const;
/// @}
/// @name Testable
/// @{
/** equals */
bool equals(const StereoPoint2& q, double tol=1e-9) const {
return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_
- q.v_) < tol);
}
/** print */
void print(const std::string& s = "") const;
/// @}
/// @name Group
/// @{
/** equals */
bool equals(const StereoPoint2& q, double tol = 1e-9) const {
return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol
&& fabs(v_ - q.v_) < tol);
}
/// identity
inline static StereoPoint2 identity() { return StereoPoint2(); }
/// @}
/// @name Group
/// @{
/// inverse
inline StereoPoint2 inverse() const {
return StereoPoint2()- (*this);
}
/// identity
inline static StereoPoint2 identity() {
return StereoPoint2();
}
/// "Compose", just adds the coordinates of two points.
inline StereoPoint2 compose(const StereoPoint2& p1) const {
return *this + p1;
}
/// inverse
StereoPoint2 operator-() const {
return StereoPoint2(-uL_, -uR_, -v_);
}
/// add two stereo points
StereoPoint2 operator+(const StereoPoint2& b) const {
return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
}
/// add
inline StereoPoint2 operator +(const StereoPoint2& b) const {
return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
}
/// subtract two stereo points
StereoPoint2 operator-(const StereoPoint2& b) const {
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
}
/// @}
/// @name Manifold
/// @{
/// subtract
inline StereoPoint2 operator -(const StereoPoint2& b) const {
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
}
/// dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
/// @}
/// @name Standard Interface
/// @{
/// return dimensionality of tangent space, DOF = 3
inline size_t dim() const { return dimension; }
/// equality
inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;}
/// Updates a with tangent space delta
inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
/// get uL
inline double uL() const {return uL_;}
/// Returns inverse retraction
inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
/// get uR
inline double uR() const {return uR_;}
/// @}
/// @name Lie Group
/// @{
/// get v
inline double v() const {return v_;}
/** Exponential map around identity - just create a Point2 from a vector */
static inline StereoPoint2 Expmap(const Vector& d) {
return StereoPoint2(d(0), d(1), d(2));
}
/** convert to vector */
Vector3 vector() const {
return Vector3(uL_, uR_, v_);
}
/** Log map around identity - just return the Point2 as a vector */
static inline Vector Logmap(const StereoPoint2& p) {
return p.vector();
}
/** convenient function to get a Point2 from the left image */
Point2 point2() const {
return Point2(uL_, v_);
}
/** The difference between another point and this point */
inline StereoPoint2 between(const StereoPoint2& p2) const {
return p2 - *this;
}
/** convenient function to get a Point2 from the right image */
Point2 right() const {
return Point2(uR_, v_);
}
/// @}
/// @name Standard Interface
/// @{
/// @name Deprecated
/// @{
inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);}
inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;}
inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; }
inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); }
static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); }
/// @}
/// get uL
inline double uL() const {return uL_;}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
/// get uR
inline double uR() const {return uR_;}
private:
/// get v
inline double v() const {return v_;}
/// @}
/// @name Advanced Interface
/// @{
/** convert to vector */
Vector3 vector() const {
return Vector3(uL_, uR_, v_);
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(uL_);
ar & BOOST_SERIALIZATION_NVP(uR_);
ar & BOOST_SERIALIZATION_NVP(v_);
}
/** convenient function to get a Point2 from the left image */
Point2 point2() const {
return Point2(uL_, v_);
}
/// @}
/** convenient function to get a Point2 from the right image */
Point2 right() const {
return Point2(uR_, v_);
}
};
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
template<>
struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(uL_);
ar & BOOST_SERIALIZATION_NVP(uR_);
ar & BOOST_SERIALIZATION_NVP(v_);
}
/// @}
};
template<>
struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
template<>
struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
template<>
struct traits<const StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
}

90
gtsam/geometry/Unit3.cpp
View File

@ -15,12 +15,13 @@
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @author Zhaoyang Lv
* @brief The Unit3 class - basically a point on a unit sphere
*/
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef __clang__
# pragma clang diagnostic push
@ -37,6 +38,7 @@
#include <boost/random/variate_generator.hpp>
#include <iostream>
#include <limits>
using namespace std;
@ -61,12 +63,10 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
boost::uniform_on_sphere<double> randomDirection(3);
// This variate_generator object is required for versions of boost somewhere
// around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
generator(rng, randomDirection);
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
rng, randomDirection);
vector<double> d = generator();
Unit3 result;
result.p_ = Point3(d[0], d[1], d[2]);
return result;
return Unit3(d[0], d[1], d[2]);
}
#ifdef GTSAM_USE_TBB
@ -80,30 +80,27 @@ const Matrix32& Unit3::basis() const {
#endif
// Return cached version if exists
if (B_)
return *B_;
if (B_) return *B_;
// Get the axis of rotation with the minimum projected length of the point
Point3 axis;
Vector3 axis;
double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
if ((mx <= my) && (mx <= mz))
axis = Point3(1.0, 0.0, 0.0);
axis = Vector3(1.0, 0.0, 0.0);
else if ((my <= mx) && (my <= mz))
axis = Point3(0.0, 1.0, 0.0);
axis = Vector3(0.0, 1.0, 0.0);
else if ((mz <= mx) && (mz <= my))
axis = Point3(0.0, 0.0, 1.0);
axis = Vector3(0.0, 0.0, 1.0);
else
assert(false);
// Create the two basis vectors
Point3 b1 = p_.cross(axis);
b1 = b1 / b1.norm();
Point3 b2 = p_.cross(b1);
b2 = b2 / b2.norm();
Vector3 b1 = p_.cross(axis).normalized();
Vector3 b2 = p_.cross(b1).normalized();
// Create the basis matrix
B_.reset(Matrix32());
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
(*B_) << b1, b2;
return *B_;
}
@ -121,10 +118,9 @@ Matrix3 Unit3::skew() const {
/* ************************************************************************* */
Vector2 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();
Vector2 xi = basis().transpose() * q.p_;
if (H)
*H = Bt * q.basis();
*H = basis().transpose() * q.basis();
return xi;
}
@ -140,47 +136,39 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
/* ************************************************************************* */
Unit3 Unit3::retract(const Vector2& v) const {
// Get the vector form of the point and the basis matrix
Vector3 p = p_.vector();
Matrix32 B = basis();
// Compute the 3D xi_hat vector
Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
Vector3 xi_hat = basis() * v;
double theta = xi_hat.norm();
double xi_hat_norm = xi_hat.norm();
// Avoid nan
if (xi_hat_norm == 0.0) {
if (v.norm() == 0.0)
return Unit3(point3());
else
return Unit3(-point3());
// Treat case of very small v differently
if (theta < std::numeric_limits<double>::epsilon()) {
return Unit3(cos(theta) * p_ + xi_hat);
}
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
Vector3 exp_p_xi_hat =
cos(theta) * p_ + xi_hat * (sin(theta) / theta);
return Unit3(exp_p_xi_hat);
}
/* ************************************************************************* */
Vector2 Unit3::localCoordinates(const Unit3& y) const {
Vector3 p = p_.vector(), q = y.p_.vector();
double dot = p.dot(q);
// Check for special cases
if (std::abs(dot - 1.0) < 1e-16)
return Vector2(0, 0);
else if (std::abs(dot + 1.0) < 1e-16)
return Vector2(M_PI, 0);
else {
Vector2 Unit3::localCoordinates(const Unit3& other) const {
const double x = p_.dot(other.p_);
// Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
// Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
// We treat the special case 1 and -1 below
const double x2 = x * x;
const double z = 1 - x2;
double y;
if (z < std::numeric_limits<double>::epsilon()) {
if (x > 0) // first order expansion at x=1
y = 1.0 - (x - 1.0) / 3.0;
else // cop out
return Vector2(M_PI, 0.0);
} else {
// no special case
double theta = acos(dot);
Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
return basis().transpose() * result_hat;
y = acos(x) / sqrt(z);
}
return basis().transpose() * y * (other.p_ - x * p_);
}
/* ************************************************************************* */

Some files were not shown because too many files have changed in this diff Show More