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

Generating config.h file in CMake with quaternion mode flag, dataset paths. Also added CMake option to use system-installed Eigen, which works by generating a global eigen include file containing the corresponding include paths.

release/4.3a0
Richard Roberts 2013-04-25 15:57:15 +00:00
parent 5f3238634d
commit eeef9eab32
49 changed files with 542 additions and 457 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CMakeLists.txt
View File

@ -83,12 +83,6 @@ if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!") message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!")
endif() endif()
# Add the Quaternion Build Flag if requested
if (GTSAM_USE_QUATERNIONS)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
endif(GTSAM_USE_QUATERNIONS)
# Flags to determine whether tests and examples are build during 'make install' # Flags to determine whether tests and examples are build during 'make install'
# Note that these remove the targets from the 'all' # Note that these remove the targets from the 'all'
option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON) option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON)
@ -141,6 +135,7 @@ include_directories(
gtsam/3rdparty/UFconfig gtsam/3rdparty/UFconfig
gtsam/3rdparty/CCOLAMD/Include gtsam/3rdparty/CCOLAMD/Include
${CMAKE_SOURCE_DIR} ${CMAKE_SOURCE_DIR}
${CMAKE_BINARY_DIR} # So we can include generated config header files
CppUnitLite CppUnitLite
${Boost_INCLUDE_DIR}) ${Boost_INCLUDE_DIR})
link_directories(${Boost_LIBRARY_DIRS}) link_directories(${Boost_LIBRARY_DIRS})

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

@ -2,16 +2,39 @@
install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD) install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD)
install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig) install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig)
# install Eigen - only the headers # Option for using system Eigen or GTSAM-bundled Eigen
install(DIRECTORY Eigen/Eigen option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
DESTINATION include/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h")
file(GLOB eigen_dir_headers_all "Eigen/Eigen/*")
# ensure that Eigen folders without extensions get added # Switch for using system Eigen or GTSAM-bundled Eigen
foreach(eigen_dir ${eigen_dir_headers_all}) if(GTSAM_USE_SYSTEM_EIGEN)
get_filename_component(filename ${eigen_dir} NAME) # Use generic Eigen include paths e.g. <Eigen/Core>
if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) set(GTSAM_EIGEN_INCLUDE_PREFIX "")
install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen)
endif() find_package(Eigen3 REQUIRED)
endforeach(eigen_dir) include_directories(EIGEN3_INCLUDE_DIR)
else()
# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND)
endif()
# install Eigen - only the headers in our 3rdparty directory
install(DIRECTORY Eigen/Eigen
DESTINATION include/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h")
file(GLOB eigen_dir_headers_all "Eigen/Eigen/*")
# ensure that Eigen folders without extensions get added
foreach(eigen_dir ${eigen_dir_headers_all})
get_filename_component(filename ${eigen_dir} NAME)
if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn")))
install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen)
endif()
endforeach(eigen_dir)
endif()
# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen
configure_file(gtsam_eigen_includes.h.in gtsam_eigen_includes.h)

24
gtsam/3rdparty/gtsam_eigen_includes.h.in vendored Normal file
View File

@ -0,0 +1,24 @@
/* ----------------------------------------------------------------------------
* 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 gtsam_eigen_includes.h
* @brief File to include the Eigen headers that we use - generated by CMake
* @author Richard Roberts
*/
#pragma once
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry>

3
gtsam/CMakeLists.txt
View File

@ -128,6 +128,9 @@ set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp"
APPEND PROPERTY COMPILE_DEFINITIONS APPEND PROPERTY COMPILE_DEFINITIONS
"SOURCE_TREE_DATASET_DIR=\"${CMAKE_SOURCE_DIR}/examples/Data\"" "SOURCE_TREE_DATASET_DIR=\"${CMAKE_SOURCE_DIR}/examples/Data\""
"INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"") "INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"")
# Generate config file
configure_file(config.h.in config.h)
# Create the matlab toolbox for the gtsam library # Create the matlab toolbox for the gtsam library
if (GTSAM_INSTALL_MATLAB_TOOLBOX) if (GTSAM_INSTALL_MATLAB_TOOLBOX)

2
gtsam/base/DSFVector.h
View File

@ -23,7 +23,7 @@
#include <set> #include <set>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
namespace gtsam { namespace gtsam {

2
gtsam/base/LieScalar.h
View File

@ -17,7 +17,7 @@
#pragma once #pragma once
#include <gtsam/base/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>

5
gtsam/base/Matrix.cpp
View File

@ -15,15 +15,12 @@
* @author Christian Potthast * @author Christian Potthast
*/ */
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <gtsam/3rdparty/Eigen/Eigen/SVD>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>

1
gtsam/base/Matrix.h
View File

@ -23,7 +23,6 @@
#pragma once #pragma once
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/3rdparty/Eigen/Eigen/QR>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp> #include <boost/math/special_functions/fpclassify.hpp>

2
gtsam/base/TestableAssertions.h
View File

@ -22,7 +22,7 @@
#include <iostream> #include <iostream>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
namespace gtsam { namespace gtsam {

1
gtsam/base/Vector.cpp
View File

@ -29,7 +29,6 @@
#include <cstdio> #include <cstdio>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/types.h>
//#ifdef WIN32 //#ifdef WIN32
//#include <Windows.h> //#include <Windows.h>

4
gtsam/base/Vector.h
View File

@ -23,8 +23,8 @@
#include <list> #include <list>
#include <vector> #include <vector>
#include <iostream> #include <iostream>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/3rdparty/Eigen/Eigen/Dense> #include <gtsam/3rdparty/gtsam_eigen_includes.h>
namespace gtsam { namespace gtsam {

3
gtsam/base/cholesky.cpp
View File

@ -20,9 +20,6 @@
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/3rdparty/Eigen/Eigen/Core>
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <cmath> #include <cmath>

2
gtsam/base/debug.h
View File

@ -17,7 +17,7 @@
*/ */
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <string> #include <string>

2
gtsam/base/timing.h
View File

@ -21,7 +21,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp> #include <boost/weak_ptr.hpp>
#include <boost/version.hpp> #include <boost/version.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
// Automatically use the new Boost timers if version is recent enough. // Automatically use the new Boost timers if version is recent enough.

2
gtsam/base/types.h
View File

@ -19,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/base/dllexport.h> #include <gtsam/dllexport.h>
#include <cstddef> #include <cstddef>

27
gtsam/config.h.in Normal file
View File

@ -0,0 +1,27 @@
/* ----------------------------------------------------------------------------
* 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 config.h
* @brief Settings and paths configured with CMake
* @author Richard Roberts
*/
#pragma once
// Paths to example datasets distributed with GTSAM
namespace gtsam {
static const char* SourceTreeDatasetDir = "@CMAKE_SOURCE_DIR@/examples/Data";
static const char* InstalledDatasetDir = "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data";
}
// Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices)
#cmakedefine GTSAM_USE_QUATERNIONS

2
gtsam/discrete/DiscreteKey.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <map> #include <map>
#include <string> #include <string>

1
gtsam/discrete/Potentials.h
View File

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

0
gtsam/base/dllexport.h → gtsam/dllexport.h
View File

17
gtsam/geometry/Rot3.h
View File

@ -21,9 +21,11 @@
#pragma once #pragma once
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
// You can override the default coordinate mode using this flag // You can override the default coordinate mode using this flag
#ifndef ROT3_DEFAULT_COORDINATES_MODE #ifndef ROT3_DEFAULT_COORDINATES_MODE
#ifdef GTSAM_DEFAULT_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
// Exponential map is very cheap for quaternions // Exponential map is very cheap for quaternions
#define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
#else #else
@ -35,7 +37,6 @@
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
namespace gtsam { namespace gtsam {
@ -46,7 +47,7 @@ namespace gtsam {
/** /**
* @brief A 3D rotation represented as a rotation matrix if the preprocessor * @brief A 3D rotation represented as a rotation matrix if the preprocessor
* symbol GTSAM_DEFAULT_QUATERNIONS is not defined, or as a quaternion if it * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
* is defined. * is defined.
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
@ -56,7 +57,7 @@ namespace gtsam {
static const size_t dimension = 3; static const size_t dimension = 3;
private: private:
#ifdef GTSAM_DEFAULT_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
/** Internal Eigen Quaternion */ /** Internal Eigen Quaternion */
Quaternion quaternion_; Quaternion quaternion_;
#else #else
@ -222,18 +223,18 @@ namespace gtsam {
* exponential map, but this can be expensive to compute. The following Enum is used * exponential map, but this can be expensive to compute. The following Enum is used
* to indicate which method should be used. The default * to indicate which method should be used. The default
* is determined by ROT3_DEFAULT_COORDINATES_MODE, which may be set at compile time, * is determined by ROT3_DEFAULT_COORDINATES_MODE, which may be set at compile time,
* and itself defaults to Rot3::CAYLEY, or if GTSAM_DEFAULT_QUATERNIONS is defined, * and itself defaults to Rot3::CAYLEY, or if GTSAM_USE_QUATERNIONS is defined,
* to Rot3::EXPMAP. * to Rot3::EXPMAP.
*/ */
enum CoordinatesMode { enum CoordinatesMode {
EXPMAP, ///< Use the Lie group exponential map to retract EXPMAP, ///< Use the Lie group exponential map to retract
#ifndef GTSAM_DEFAULT_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
CAYLEY, ///< Retract and localCoordinates using the Cayley transform. CAYLEY, ///< Retract and localCoordinates using the Cayley transform.
SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only). SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only).
#endif #endif
}; };
#ifndef GTSAM_DEFAULT_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
/// Retraction from R^3 to Rot3 manifold using the Cayley transform /// Retraction from R^3 to Rot3 manifold using the Cayley transform
Rot3 retractCayley(const Vector& omega) const; Rot3 retractCayley(const Vector& omega) const;
#endif #endif
@ -362,7 +363,7 @@ namespace gtsam {
{ {
ar & boost::serialization::make_nvp("Rot3", ar & boost::serialization::make_nvp("Rot3",
boost::serialization::base_object<Value>(*this)); boost::serialization::base_object<Value>(*this));
#ifndef GTSAM_DEFAULT_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
ar & boost::serialization::make_nvp("rot11", rot_(0,0)); ar & boost::serialization::make_nvp("rot11", rot_(0,0));
ar & boost::serialization::make_nvp("rot12", rot_(0,1)); ar & boost::serialization::make_nvp("rot12", rot_(0,1));
ar & boost::serialization::make_nvp("rot13", rot_(0,2)); ar & boost::serialization::make_nvp("rot13", rot_(0,2));

768
gtsam/geometry/Rot3M.cpp
View File

@ -1,385 +1,387 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * 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 Rot3M.cpp * @file Rot3M.cpp
* @brief Rotation (internal: 3*3 matrix representation*) * @brief Rotation (internal: 3*3 matrix representation*)
* @author Alireza Fathi * @author Alireza Fathi
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
*/ */
#ifndef GTSAM_DEFAULT_QUATERNIONS #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#include <gtsam/geometry/Rot3.h> #ifndef GTSAM_USE_QUATERNIONS
#include <boost/math/constants/constants.hpp>
#include <cmath> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
using namespace std; #include <cmath>
namespace gtsam { using namespace std;
static const Matrix3 I3 = Matrix3::Identity(); namespace gtsam {
/* ************************************************************************* */ static const Matrix3 I3 = Matrix3::Identity();
Rot3::Rot3() : rot_(Matrix3::Identity()) {}
/* ************************************************************************* */
/* ************************************************************************* */ Rot3::Rot3() : rot_(Matrix3::Identity()) {}
Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) {
rot_.col(0) = r1.vector(); /* ************************************************************************* */
rot_.col(1) = r2.vector(); Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) {
rot_.col(2) = r3.vector(); rot_.col(0) = r1.vector();
} rot_.col(1) = r2.vector();
rot_.col(2) = r3.vector();
/* ************************************************************************* */ }
Rot3::Rot3(double R11, double R12, double R13,
double R21, double R22, double R23, /* ************************************************************************* */
double R31, double R32, double R33) { Rot3::Rot3(double R11, double R12, double R13,
rot_ << R11, R12, R13, double R21, double R22, double R23,
R21, R22, R23, double R31, double R32, double R33) {
R31, R32, R33; rot_ << R11, R12, R13,
} R21, R22, R23,
R31, R32, R33;
/* ************************************************************************* */ }
Rot3::Rot3(const Matrix3& R) {
rot_ = R; /* ************************************************************************* */
} Rot3::Rot3(const Matrix3& R) {
rot_ = R;
/* ************************************************************************* */ }
Rot3::Rot3(const Matrix& R) {
/* ************************************************************************* */
Rot3::Rot3(const Matrix& R) {
if (R.rows()!=3 || R.cols()!=3) if (R.rows()!=3 || R.cols()!=3)
throw invalid_argument("Rot3 constructor expects 3*3 matrix"); throw invalid_argument("Rot3 constructor expects 3*3 matrix");
rot_ = R; rot_ = R;
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//Rot3::Rot3(const Matrix3& R) : rot_(R) {} //Rot3::Rot3(const Matrix3& R) : rot_(R) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rx(double t) { Rot3 Rot3::Rx(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
1, 0, 0, 1, 0, 0,
0, ct,-st, 0, ct,-st,
0, st, ct); 0, st, ct);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Ry(double t) { Rot3 Rot3::Ry(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
ct, 0, st, ct, 0, st,
0, 1, 0, 0, 1, 0,
-st, 0, ct); -st, 0, ct);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rz(double t) { Rot3 Rot3::Rz(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
ct,-st, 0, ct,-st, 0,
st, ct, 0, st, ct, 0,
0, 0, 1); 0, 0, 1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Considerably faster than composing matrices above ! // Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) { Rot3 Rot3::RzRyRx(double x, double y, double z) {
double cx=cos(x),sx=sin(x); double cx=cos(x),sx=sin(x);
double cy=cos(y),sy=sin(y); double cy=cos(y),sy=sin(y);
double cz=cos(z),sz=sin(z); double cz=cos(z),sz=sin(z);
double ss_ = sx * sy; double ss_ = sx * sy;
double cs_ = cx * sy; double cs_ = cx * sy;
double sc_ = sx * cy; double sc_ = sx * cy;
double cc_ = cx * cy; double cc_ = cx * cy;
double c_s = cx * sz; double c_s = cx * sz;
double s_s = sx * sz; double s_s = sx * sz;
double _cs = cy * sz; double _cs = cy * sz;
double _cc = cy * cz; double _cc = cy * cz;
double s_c = sx * cz; double s_c = sx * cz;
double c_c = cx * cz; double c_c = cx * cz;
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
return Rot3( return Rot3(
_cc,- c_s + ssc, s_s + csc, _cc,- c_s + ssc, s_s + csc,
_cs, c_c + sss, -s_c + css, _cs, c_c + sss, -s_c + css,
-sy, sc_, cc_ -sy, sc_, cc_
); );
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) { Rot3 Rot3::rodriguez(const Vector& w, double theta) {
// get components of axis \omega // get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2); double wx = w(0), wy=w(1), wz=w(2);
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG #ifndef NDEBUG
double l_n = wwTxx + wwTyy + wwTzz; double l_n = wwTxx + wwTyy + wwTzz;
if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif #endif
double c = cos(theta), s = sin(theta), c_1 = 1 - c; double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double swx = wx * s, swy = wy * s, swz = wz * s; 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 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 C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz; double C22 = c_1*wwTzz;
return Rot3( return Rot3(
c + C00, -swz + C01, swy + C02, c + C00, -swz + C01, swy + C02,
swz + C01, c + C11, -swx + C12, swz + C01, c + C11, -swx + C12,
-swy + C02, swx + C12, c + C22); -swy + C02, swx + C12, c + C22);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) { Rot3 Rot3::rodriguez(const Vector& w) {
double t = w.norm(); double t = w.norm();
if (t < 1e-10) return Rot3(); if (t < 1e-10) return Rot3();
return rodriguez(w/t, t); return rodriguez(w/t, t);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const { bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol); return equal_with_abs_tol(matrix(), R.matrix(), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2, Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose(); if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3; if (H2) *H2 = I3;
return *this * R2; return *this * R2;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const { return rotate(p); } Point3 Rot3::operator*(const Point3& p) const { return rotate(p); }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const { Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -rot_; if (H1) *H1 = -rot_;
return Rot3(Matrix3(rot_.transpose())); return Rot3(Matrix3(rot_.transpose()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2, Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_); if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3; if (H2) *H2 = I3;
return Rot3(Matrix3(rot_.transpose()*R2.rot_)); return Rot3(Matrix3(rot_.transpose()*R2.rot_));
//return between_default(*this, R2); //return between_default(*this, R2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const { Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_)); return Rot3(Matrix3(rot_*R2.rot_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p, Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1 || H2) { if (H1 || H2) {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_; if (H2) *H2 = rot_;
} }
return Point3(rot_ * p.vector()); return Point3(rot_ * p.vector());
} }
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SO(3) section // see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point3 q(rot_.transpose()*p.vector()); // q = Rt*p Point3 q(rot_.transpose()*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = transpose(); if (H2) *H2 = transpose();
return q; return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) { Vector3 Rot3::Logmap(const Rot3& R) {
static const double PI = boost::math::constants::pi<double>(); static const double PI = boost::math::constants::pi<double>();
const Matrix3& rot = R.rot_; const Matrix3& rot = R.rot_;
// Get trace(R) // Get trace(R)
double tr = rot.trace(); double tr = rot.trace();
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (std::abs(tr+1.0) < 1e-10) { if (std::abs(tr+1.0) < 1e-10) {
if(std::abs(rot(2,2)+1.0) > 1e-10) if(std::abs(rot(2,2)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(2,2) )) * return (PI / sqrt(2.0+2.0*rot(2,2) )) *
Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
else if(std::abs(rot(1,1)+1.0) > 1e-10) else if(std::abs(rot(1,1)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(1,1))) * return (PI / sqrt(2.0+2.0*rot(1,1))) *
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (PI / sqrt(2.0+2.0*rot(0,0))) * return (PI / sqrt(2.0+2.0*rot(0,0))) *
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
} else { } else {
double magnitude; double magnitude;
double tr_3 = tr-3.0; // always negative double tr_3 = tr-3.0; // always negative
if (tr_3<-1e-7) { if (tr_3<-1e-7) {
double theta = acos((tr-1.0)/2.0); double theta = acos((tr-1.0)/2.0);
magnitude = theta/(2.0*sin(theta)); magnitude = theta/(2.0*sin(theta));
} else { } else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0; magnitude = 0.5 - tr_3*tr_3/12.0;
} }
return magnitude*Vector3( return magnitude*Vector3(
rot(2,1)-rot(1,2), rot(2,1)-rot(1,2),
rot(0,2)-rot(2,0), rot(0,2)-rot(2,0),
rot(1,0)-rot(0,1)); rot(1,0)-rot(0,1));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retractCayley(const Vector& omega) const { Rot3 Rot3::retractCayley(const Vector& omega) const {
const double x = omega(0), y = omega(1), z = omega(2); const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x * x, y2 = y * y, z2 = z * z; const double x2 = x * x, y2 = y * y, z2 = z * z;
const double xy = x * y, xz = x * z, yz = y * z; const double xy = x * y, xz = x * z, yz = y * z;
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
return (*this) return (*this)
* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) { if(mode == Rot3::EXPMAP) {
return (*this)*Expmap(omega); return (*this)*Expmap(omega);
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
return retractCayley(omega); return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega); Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2); return (*this)*Cayley<3>(-Omega/2);
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) { if(mode == Rot3::EXPMAP) {
return Logmap(between(T)); return Logmap(between(T));
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// Mathematica closed form optimization (procrastination?) gone wild: // Mathematica closed form optimization (procrastination?) gone wild:
const double a=A(0,0),b=A(0,1),c=A(0,2); const double a=A(0,0),b=A(0,1),c=A(0,2);
const double d=A(1,0),e=A(1,1),f=A(1,2); const double d=A(1,0),e=A(1,1),f=A(1,2);
const double g=A(2,0),h=A(2,1),i=A(2,2); const double g=A(2,0),h=A(2,1),i=A(2,2);
const double di = d*i, ce = c*e, cd = c*d, fg=f*g; const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
const double M = 1 + e - f*h + i + e*i; const double M = 1 + e - f*h + i + e*i;
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
const double x = (a * f - cd + f) * K; const double x = (a * f - cd + f) * K;
const double y = (b * f - ce - c) * K; const double y = (b * f - ce - c) * K;
const double z = (fg - di - d) * K; const double z = (fg - di - d) * K;
return -2 * Vector3(x, y, z); return -2 * Vector3(x, y, z);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// using templated version of Cayley // using templated version of Cayley
Eigen::Matrix3d Omega = Cayley<3>(A); Eigen::Matrix3d Omega = Cayley<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::matrix() const { Matrix3 Rot3::matrix() const {
return rot_; return rot_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::transpose() const { Matrix3 Rot3::transpose() const {
return rot_.transpose(); return rot_.transpose();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::column(int index) const{ Point3 Rot3::column(int index) const{
return Point3(rot_.col(index)); return Point3(rot_.col(index));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.col(0)); } Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r2() const { return Point3(rot_.col(1)); } Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r3() const { return Point3(rot_.col(2)); } Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::xyz() const { Vector3 Rot3::xyz() const {
Matrix3 I;Vector3 q; Matrix3 I;Vector3 q;
boost::tie(I,q)=RQ(rot_); boost::tie(I,q)=RQ(rot_);
return q; return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::ypr() const { Vector3 Rot3::ypr() const {
Vector3 q = xyz(); Vector3 q = xyz();
return Vector3(q(2),q(1),q(0)); return Vector3(q(2),q(1),q(0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::rpy() const { Vector3 Rot3::rpy() const {
return xyz(); return xyz();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Quaternion Rot3::toQuaternion() const { Quaternion Rot3::toQuaternion() const {
return Quaternion(rot_); return Quaternion(rot_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Rot3::quaternion() const { Vector Rot3::quaternion() const {
Quaternion q = toQuaternion(); Quaternion q = toQuaternion();
Vector v(4); Vector v(4);
v(0) = q.w(); v(0) = q.w();
v(1) = q.x(); v(1) = q.x();
v(2) = q.y(); v(2) = q.y();
v(3) = q.z(); v(3) = q.z();
return v; return v;
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) { pair<Matrix3, Vector3> RQ(const Matrix3& A) {
double x = -atan2(-A(2, 1), A(2, 2)); double x = -atan2(-A(2, 1), A(2, 2));
Rot3 Qx = Rot3::Rx(-x); Rot3 Qx = Rot3::Rx(-x);
Matrix3 B = A * Qx.matrix(); Matrix3 B = A * Qx.matrix();
double y = -atan2(B(2, 0), B(2, 2)); double y = -atan2(B(2, 0), B(2, 2));
Rot3 Qy = Rot3::Ry(-y); Rot3 Qy = Rot3::Ry(-y);
Matrix3 C = B * Qy.matrix(); Matrix3 C = B * Qy.matrix();
double z = -atan2(-C(1, 0), C(1, 1)); double z = -atan2(-C(1, 0), C(1, 1));
Rot3 Qz = Rot3::Rz(-z); Rot3 Qz = Rot3::Rz(-z);
Matrix3 R = C * Qz.matrix(); Matrix3 R = C * Qz.matrix();
Vector xyz = Vector3(x, y, z); Vector xyz = Vector3(x, y, z);
return make_pair(R, xyz); return make_pair(R, xyz);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam
#endif #endif

4
gtsam/geometry/Rot3Q.cpp
View File

@ -15,7 +15,9 @@
* @author Richard Roberts * @author Richard Roberts
*/ */
#ifdef GTSAM_DEFAULT_QUATERNIONS #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#ifdef GTSAM_USE_QUATERNIONS
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>

2
gtsam/geometry/tests/testRot3M.cpp
View File

@ -26,7 +26,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#ifndef GTSAM_DEFAULT_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

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

@ -23,7 +23,7 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#ifdef GTSAM_DEFAULT_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
using namespace gtsam; using namespace gtsam;

23
gtsam/global_includes.h Normal file
View File

@ -0,0 +1,23 @@
/* ----------------------------------------------------------------------------
* 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 global_includes.h
* @brief Included from all GTSAM files
* @author Richard Roberts
* @addtogroup base
*/
#pragma once
#include <gtsam/config.h> // Configuration from CMake
#include <gtsam/base/types.h> // Basic types, constants, and compatibility functions
// types.h includes dllexport.h, which contains macros for dllspec tags for Windows DLLs

2
gtsam/inference/BayesNet.h
View File

@ -25,7 +25,7 @@
#include <boost/assign/list_inserter.hpp> #include <boost/assign/list_inserter.hpp>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/Permutation.h>

2
gtsam/inference/BayesTree.h
View File

@ -27,7 +27,7 @@
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNet.h>

2
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -21,7 +21,7 @@
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/weak_ptr.hpp> #include <boost/weak_ptr.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNet.h>

2
gtsam/inference/ClusterTree.h
View File

@ -27,7 +27,7 @@
#include <boost/weak_ptr.hpp> #include <boost/weak_ptr.hpp>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
namespace gtsam { namespace gtsam {

2
gtsam/inference/Factor.h
View File

@ -27,7 +27,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/function/function1.hpp> #include <boost/function/function1.hpp>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
namespace gtsam { namespace gtsam {

2
gtsam/inference/GenericSequentialSolver.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <boost/function.hpp> #include <boost/function.hpp>

2
gtsam/inference/IndexConditional.h
View File

@ -17,7 +17,7 @@
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/Conditional.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/Permutation.h>

2
gtsam/inference/Permutation.h
View File

@ -23,7 +23,7 @@
#include <iostream> #include <iostream>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
namespace gtsam { namespace gtsam {

2
gtsam/inference/SymbolicFactorGraph.h
View File

@ -17,7 +17,7 @@
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactor.h>

2
gtsam/inference/VariableIndex.h
View File

@ -23,7 +23,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
namespace gtsam { namespace gtsam {

2
gtsam/inference/VariableSlots.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <iostream> #include <iostream>

2
gtsam/linear/GaussianBayesNet.h
View File

@ -20,7 +20,7 @@
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNet.h>

2
gtsam/linear/GaussianConditional.h
View File

@ -21,7 +21,7 @@
#include <boost/utility.hpp> #include <boost/utility.hpp>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/blockMatrices.h> #include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>

1
gtsam/linear/GaussianISAM.cpp
View File

@ -15,7 +15,6 @@
* @author Michael Kaess * @author Michael Kaess
*/ */
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <gtsam/linear/GaussianISAM.h> #include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>

1
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -15,7 +15,6 @@
* @date Oct 21, 2010 * @date Oct 21, 2010
*/ */
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <gtsam/linear/GaussianMultifrontalSolver.h> #include <gtsam/linear/GaussianMultifrontalSolver.h>
namespace gtsam { namespace gtsam {

1
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -15,7 +15,6 @@
* @date Oct 19, 2010 * @date Oct 19, 2010
*/ */
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>

2
gtsam/linear/JacobianFactor.h
View File

@ -22,7 +22,7 @@
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/blockMatrices.h> #include <gtsam/base/blockMatrices.h>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>

2
gtsam/linear/VectorValues.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>

2
gtsam/linear/linearExceptions.h
View File

@ -17,7 +17,7 @@
*/ */
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <exception> #include <exception>

2
gtsam/nonlinear/Key.h
View File

@ -20,7 +20,7 @@
#include <boost/function.hpp> #include <boost/function.hpp>
#include <string> #include <string>
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>

1
gtsam/nonlinear/Marginals.cpp
View File

@ -16,7 +16,6 @@
* @date May 14, 2012 * @date May 14, 2012
*/ */
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h> #include <gtsam/linear/GaussianMultifrontalSolver.h>

4
gtsam/slam/dataset.cpp
View File

@ -40,8 +40,8 @@ namespace gtsam {
string findExampleDataFile(const string& name) { string findExampleDataFile(const string& name) {
// Search source tree and installed location // Search source tree and installed location
vector<string> rootsToSearch; vector<string> rootsToSearch;
rootsToSearch.push_back(SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt rootsToSearch.push_back(SourceTreeDatasetDir); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
rootsToSearch.push_back(INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt rootsToSearch.push_back(InstalledDatasetDir); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
// Search for filename as given, and with .graph and .txt extensions // Search for filename as given, and with .graph and .txt extensions
vector<string> namesToSearch; vector<string> namesToSearch;

2
gtsam_unstable/base/Dummy.h
View File

@ -17,7 +17,7 @@
* @date June 14, 2012 * @date June 14, 2012
*/ */
#include <gtsam/base/types.h> #include <gtsam/global_includes.h>
#include <gtsam_unstable/base/dllexport.h> #include <gtsam_unstable/base/dllexport.h>
#include <string> #include <string>

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -3,8 +3,6 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <gtsam/3rdparty/Eigen/Eigen/LU>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/Lie-inl.h> #include <gtsam/base/Lie-inl.h>