Merge branch 'develop' into feature/ShonanCLI
Frank Dellaert
GitHub
commit
30ee2b9409
|
|
@ -63,11 +63,11 @@ function configure()
|
|||
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
|
||||
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
|
||||
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
|
||||
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
|
||||
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
|
||||
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
|
||||
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
|
||||
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
|
||||
-DGTSAM_TYPEDEF_POINTS_TO_VECTOR=${GTSAM_TYPEDEF_POINTS_TO_VECTOR:-OFF} \
|
||||
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
|
||||
-DCMAKE_VERBOSE_MAKEFILE=ON \
|
||||
-DBOOST_ROOT=$BOOST_ROOT \
|
||||
-DBoost_NO_SYSTEM_PATHS=ON \
|
||||
-DBoost_ARCHITECTURE=-x64
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ jobs:
|
|||
if: runner.os == 'Linux'
|
||||
run: |
|
||||
echo "BOOST_ROOT = $BOOST_ROOT"
|
||||
- name: Build (Linux)
|
||||
- name: Build and Test (Linux)
|
||||
if: runner.os == 'Linux'
|
||||
run: |
|
||||
bash .github/scripts/unix.sh -t
|
||||
|
|
@ -45,7 +45,7 @@ jobs:
|
|||
echo "::set-env name=CC::clang"
|
||||
echo "::set-env name=CXX::clang++"
|
||||
fi
|
||||
- name: Build (macOS)
|
||||
- name: Build and Test (macOS)
|
||||
if: runner.os == 'macOS'
|
||||
run: |
|
||||
bash .github/scripts/unix.sh -t
|
||||
|
|
@ -20,7 +20,7 @@ jobs:
|
|||
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
|
||||
name: [
|
||||
ubuntu-18.04-gcc-5,
|
||||
ubuntu-18.04-gcc-9,
|
||||
# ubuntu-18.04-gcc-9, # TODO Disabled for now because of timeouts
|
||||
ubuntu-18.04-clang-9,
|
||||
macOS-10.15-xcode-11.3.1,
|
||||
]
|
||||
|
|
@ -34,10 +34,11 @@ jobs:
|
|||
compiler: gcc
|
||||
version: "5"
|
||||
|
||||
- name: ubuntu-18.04-gcc-9
|
||||
os: ubuntu-18.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
# TODO Disabled for now because of timeouts
|
||||
# - name: ubuntu-18.04-gcc-9
|
||||
# os: ubuntu-18.04
|
||||
# compiler: gcc
|
||||
# version: "9"
|
||||
|
||||
- name: ubuntu-18.04-clang-9
|
||||
os: ubuntu-18.04
|
||||
|
|
|
|||
|
|
@ -0,0 +1,112 @@
|
|||
name: Special Cases CI
|
||||
|
||||
on: [pull_request]
|
||||
|
||||
jobs:
|
||||
build:
|
||||
name: ${{ matrix.name }} ${{ matrix.build_type }}
|
||||
runs-on: ${{ matrix.os }}
|
||||
|
||||
env:
|
||||
CTEST_OUTPUT_ON_FAILURE: ON
|
||||
CTEST_PARALLEL_LEVEL: 2
|
||||
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
|
||||
GTSAM_BUILD_UNSTABLE: ON
|
||||
|
||||
strategy:
|
||||
fail-fast: false
|
||||
|
||||
matrix:
|
||||
# Github Actions requires a single row to be added to the build matrix.
|
||||
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
|
||||
name:
|
||||
[
|
||||
ubuntu-gcc-deprecated,
|
||||
ubuntu-gcc-quaternions,
|
||||
ubuntu-gcc-points-vector,
|
||||
]
|
||||
|
||||
build_type: [Debug, Release]
|
||||
|
||||
include:
|
||||
- name: ubuntu-gcc-deprecated
|
||||
os: ubuntu-18.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: deprecated
|
||||
|
||||
- name: ubuntu-gcc-quaternions
|
||||
os: ubuntu-18.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: quaternions
|
||||
|
||||
- name: ubuntu-gcc-points-vector
|
||||
os: ubuntu-18.04
|
||||
compiler: gcc
|
||||
version: "9"
|
||||
flag: points-vector
|
||||
|
||||
steps:
|
||||
- name: Checkout
|
||||
uses: actions/checkout@master
|
||||
|
||||
- name: Install (Linux)
|
||||
if: runner.os == 'Linux'
|
||||
run: |
|
||||
# LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
|
||||
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
|
||||
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
|
||||
fi
|
||||
sudo apt-get -y update
|
||||
|
||||
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
|
||||
|
||||
echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
|
||||
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
|
||||
|
||||
if [ "${{ matrix.compiler }}" = "gcc" ]; then
|
||||
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
|
||||
echo "::set-env name=CC::gcc-${{ matrix.version }}"
|
||||
echo "::set-env name=CXX::g++-${{ matrix.version }}"
|
||||
else
|
||||
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
|
||||
echo "::set-env name=CC::clang-${{ matrix.version }}"
|
||||
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
|
||||
fi
|
||||
|
||||
- name: Install (macOS)
|
||||
if: runner.os == 'macOS'
|
||||
run: |
|
||||
brew install cmake ninja boost
|
||||
if [ "${{ matrix.compiler }}" = "gcc" ]; then
|
||||
brew install gcc@${{ matrix.version }}
|
||||
echo "::set-env name=CC::gcc-${{ matrix.version }}"
|
||||
echo "::set-env name=CXX::g++-${{ matrix.version }}"
|
||||
else
|
||||
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
|
||||
echo "::set-env name=CC::clang"
|
||||
echo "::set-env name=CXX::clang++"
|
||||
fi
|
||||
|
||||
- name: Set Allow Deprecated Flag
|
||||
if: matrix.flag == 'deprecated'
|
||||
env:
|
||||
GTSAM_ALLOW_DEPRECATED_SINCE_V41: ON
|
||||
run: echo "Allow deprecated since version 4.1"
|
||||
|
||||
- name: Set Use Quaternions Flag
|
||||
if: matrix.flag == 'quaternions'
|
||||
env:
|
||||
GTSAM_USE_QUATERNIONS: ON
|
||||
run: echo "Use Quaternions for rotations"
|
||||
|
||||
- name: Set Typedef Points to Vector Flag
|
||||
if: matrix.flag == 'points-vector'
|
||||
env:
|
||||
GTSAM_TYPEDEF_POINTS_TO_VECTOR: ON
|
||||
run: echo "Typedef Points to Vector"
|
||||
|
||||
- name: Build & Test
|
||||
run: |
|
||||
bash .github/scripts/unix.sh -t
|
||||
|
|
@ -79,7 +79,6 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also
|
|||
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
|
||||
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF)
|
||||
option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
|
||||
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF)
|
||||
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
|
||||
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
|
||||
if(NOT MSVC AND NOT XCODE_VERSION)
|
||||
|
|
@ -114,10 +113,6 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
|
|||
endif()
|
||||
|
||||
if(GTSAM_BUILD_PYTHON)
|
||||
if (NOT GTSAM_TYPEDEF_POINTS_TO_VECTORS)
|
||||
message(FATAL_ERROR "GTSAM_BUILD_PYTHON requires GTSAM_TYPEDEF_POINTS_TO_VECTORS to be enabled but it is not.")
|
||||
endif()
|
||||
|
||||
if(GTSAM_UNSTABLE_BUILD_PYTHON)
|
||||
if (NOT GTSAM_BUILD_UNSTABLE)
|
||||
message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
|
||||
|
|
@ -586,7 +581,6 @@ print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency c
|
|||
print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
|
||||
print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
|
||||
print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1")
|
||||
print_enabled_config(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
|
||||
print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
|
||||
print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
|
||||
|
||||
|
|
|
|||
17
README.md
17
README.md
|
|
@ -1,6 +1,10 @@
|
|||
# README - Georgia Tech Smoothing and Mapping Library
|
||||
|
||||
**As of August 1, develop is officially in "Pre 4.1" mode, and features deprecated in 4.0 were removed. Use the last 4.0.3 release if you need those features. However, most are easily converted and can be tracked down (in 4.0.3) by disabling the cmake flag GTSAM_ALLOW_DEPRECATED_SINCE_V4**
|
||||
**Important Note**
|
||||
|
||||
As of August 1 2020, the `develop` branch is officially in "Pre 4.1" mode, and features deprecated in 4.0 have been removed. Please use the last [4.0.3 release](https://github.com/borglab/gtsam/releases/tag/4.0.3) if you need those features.
|
||||
|
||||
However, most are easily converted and can be tracked down (in 4.0.3) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4`.
|
||||
|
||||
## What is GTSAM?
|
||||
|
||||
|
|
@ -9,13 +13,14 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes
|
|||
Networks as the underlying computing paradigm rather than sparse
|
||||
matrices.
|
||||
|
||||
| Platform | Build Status |
|
||||
|:---------:|:-------------:|
|
||||
| gcc/clang | [](https://travis-ci.com/borglab/gtsam/) |
|
||||
| MSVC | [](https://ci.appveyor.com/project/dellaert/gtsam) |
|
||||
| Platform | Compiler | Build Status |
|
||||
|:------------:|:---------:|:-------------:|
|
||||
| Ubuntu 18.04 | gcc/clang |  |
|
||||
| macOS | clang |  |
|
||||
| Windows | MSVC |  |
|
||||
|
||||
|
||||
On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](##Wrappers).
|
||||
On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](#wrappers).
|
||||
|
||||
|
||||
## Quickstart
|
||||
|
|
|
|||
|
|
@ -133,6 +133,18 @@ endif()
|
|||
# paths so that the compiler uses GTSAM headers in our source directory instead
|
||||
# of any previously installed GTSAM headers.
|
||||
target_include_directories(gtsam BEFORE PUBLIC
|
||||
# main gtsam includes:
|
||||
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
|
||||
$<INSTALL_INTERFACE:include/>
|
||||
# config.h
|
||||
$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
|
||||
# unit tests:
|
||||
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
|
||||
)
|
||||
# 3rdparty libraries: use the "system" flag so they are included via "-isystem"
|
||||
# and warnings (and warnings-considered-errors) in those headers are not
|
||||
# reported as warnings/errors in our targets:
|
||||
target_include_directories(gtsam SYSTEM BEFORE PUBLIC
|
||||
# SuiteSparse_config
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config>
|
||||
$<INSTALL_INTERFACE:include/gtsam/3rdparty/SuiteSparse_config>
|
||||
|
|
@ -141,13 +153,6 @@ target_include_directories(gtsam BEFORE PUBLIC
|
|||
# CCOLAMD
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
|
||||
$<INSTALL_INTERFACE:include/gtsam/3rdparty/CCOLAMD>
|
||||
# main gtsam includes:
|
||||
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
|
||||
$<INSTALL_INTERFACE:include/>
|
||||
# config.h
|
||||
$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
|
||||
# unit tests:
|
||||
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
|
||||
)
|
||||
if(GTSAM_SUPPORT_NESTED_DISSECTION)
|
||||
target_include_directories(gtsam BEFORE PUBLIC
|
||||
|
|
|
|||
|
|
@ -72,9 +72,6 @@
|
|||
// Make sure dependent projects that want it can see deprecated functions
|
||||
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41
|
||||
|
||||
// Publish flag about Eigen typedef
|
||||
#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
|
||||
// Support Metis-based nested dissection
|
||||
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION
|
||||
|
||||
|
|
|
|||
|
|
@ -50,37 +50,6 @@ double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1,
|
|||
}
|
||||
}
|
||||
|
||||
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Point2::print(const string& s) const {
|
||||
cout << s << *this << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Point2::equals(const Point2& q, double tol) const {
|
||||
return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point2::norm(OptionalJacobian<1,2> H) const {
|
||||
return gtsam::norm2(*this, H);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
|
||||
OptionalJacobian<1,2> H2) const {
|
||||
return gtsam::distance2(*this, point, H1, H2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const Point2& p) {
|
||||
os << '(' << p.x() << ", " << p.y() << ')';
|
||||
return os;
|
||||
}
|
||||
|
||||
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Math inspired by http://paulbourke.net/geometry/circlesphere/
|
||||
boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
|
||||
|
|
|
|||
|
|
@ -22,112 +22,10 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
|
||||
/// As of GTSAM 4, in order to make GTSAM more lean,
|
||||
/// it is now possible to just typedef Point2 to Vector2
|
||||
typedef Vector2 Point2;
|
||||
|
||||
#else
|
||||
|
||||
/**
|
||||
* A 2D point
|
||||
* Complies with the Testable Concept
|
||||
* Functional, so no set functions: once created, a point is constant.
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Point2 : public Vector2 {
|
||||
private:
|
||||
|
||||
public:
|
||||
enum { dimension = 2 };
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// default constructor
|
||||
Point2() {}
|
||||
|
||||
using Vector2::Vector2;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/// construct from 2D vector
|
||||
explicit Point2(const Vector2& v):Vector2(v) {}
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
GTSAM_EXPORT void print(const std::string& s = "") const;
|
||||
|
||||
/// equals with an tolerance, prints out message if unequal
|
||||
GTSAM_EXPORT bool equals(const Point2& q, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity
|
||||
inline static Point2 identity() {return Point2(0,0);}
|
||||
|
||||
/// @}
|
||||
/// @name Vector Space
|
||||
/// @{
|
||||
|
||||
/** creates a unit vector */
|
||||
Point2 unit() const { return *this/norm(); }
|
||||
|
||||
/** norm of point, with derivative */
|
||||
GTSAM_EXPORT double norm(OptionalJacobian<1,2> H = boost::none) const;
|
||||
|
||||
/** distance between two points */
|
||||
GTSAM_EXPORT double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
|
||||
OptionalJacobian<1,2> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// equality
|
||||
inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();}
|
||||
|
||||
/// get x
|
||||
inline double x() const {return (*this)[0];}
|
||||
|
||||
/// get y
|
||||
inline double y() const {return (*this)[1];}
|
||||
|
||||
/// return vectorized form (column-wise).
|
||||
const Vector2& vector() const { return *this; }
|
||||
|
||||
/// @}
|
||||
|
||||
/// Streaming
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
|
||||
|
||||
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_BASE_OBJECT_NVP(Vector2);}
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<Point2> : public internal::VectorSpace<Point2> {
|
||||
};
|
||||
|
||||
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
|
||||
/// Distance of the point from the origin, with Jacobian
|
||||
GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
|
||||
|
||||
|
|
|
|||
|
|
@ -22,47 +22,6 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
bool Point3::equals(const Point3 &q, double tol) const {
|
||||
return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol &&
|
||||
std::abs(z() - q.z()) < tol);
|
||||
}
|
||||
|
||||
void Point3::print(const string& s) const {
|
||||
cout << s << *this << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
|
||||
OptionalJacobian<1, 3> H2) const {
|
||||
return gtsam::distance3(*this,q,H1,H2);
|
||||
}
|
||||
|
||||
double Point3::norm(OptionalJacobian<1,3> H) const {
|
||||
return gtsam::norm3(*this, H);
|
||||
}
|
||||
|
||||
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
|
||||
return gtsam::normalize(*this, H);
|
||||
}
|
||||
|
||||
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) const {
|
||||
return gtsam::cross(*this, q, H1, H2);
|
||||
}
|
||||
|
||||
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
|
||||
OptionalJacobian<1, 3> H2) const {
|
||||
return gtsam::dot(*this, q, H1, H2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const Point3& p) {
|
||||
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]'";
|
||||
return os;
|
||||
}
|
||||
|
||||
#endif
|
||||
/* ************************************************************************* */
|
||||
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
|
||||
OptionalJacobian<1, 3> H2) {
|
||||
|
|
|
|||
|
|
@ -29,107 +29,10 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
|
||||
/// As of GTSAM 4, in order to make GTSAM more lean,
|
||||
/// it is now possible to just typedef Point3 to Vector3
|
||||
typedef Vector3 Point3;
|
||||
|
||||
#else
|
||||
|
||||
/**
|
||||
* A 3D point is just a Vector3 with some additional methods
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Point3 : public Vector3 {
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
using Vector3::Vector3;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional string */
|
||||
GTSAM_EXPORT void print(const std::string& s = "") const;
|
||||
|
||||
/** equals with an tolerance */
|
||||
GTSAM_EXPORT bool equals(const Point3& p, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
|
||||
|
||||
/// @}
|
||||
/// @name Vector Space
|
||||
/// @{
|
||||
|
||||
/** distance between two points */
|
||||
GTSAM_EXPORT double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
|
||||
OptionalJacobian<1, 3> H2 = boost::none) const;
|
||||
|
||||
/** Distance of the point from the origin, with Jacobian */
|
||||
GTSAM_EXPORT double norm(OptionalJacobian<1,3> H = boost::none) const;
|
||||
|
||||
/** normalize, with optional Jacobian */
|
||||
GTSAM_EXPORT Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
|
||||
|
||||
/** cross product @return this x q */
|
||||
GTSAM_EXPORT Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
|
||||
OptionalJacobian<3, 3> H_q = boost::none) const;
|
||||
|
||||
/** dot product @return this * q*/
|
||||
GTSAM_EXPORT double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
|
||||
OptionalJacobian<1, 3> H_q = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// return as Vector3
|
||||
const Vector3& vector() const { return *this; }
|
||||
|
||||
/// get x
|
||||
inline double x() const {return (*this)[0];}
|
||||
|
||||
/// get y
|
||||
inline double y() const {return (*this)[1];}
|
||||
|
||||
/// get z
|
||||
inline double z() const {return (*this)[2];}
|
||||
|
||||
/// @}
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
|
||||
|
||||
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(Vector3);
|
||||
}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<Point3> : public internal::VectorSpace<Point3> {};
|
||||
|
||||
template<>
|
||||
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
|
||||
|
||||
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
|
||||
// Convenience typedef
|
||||
typedef std::pair<Point3, Point3> Point3Pair;
|
||||
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
|
||||
|
|
|
|||
|
|
@ -237,16 +237,6 @@ TEST( Point2, circleCircleIntersection) {
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
TEST( Point2, stream) {
|
||||
Point2 p(1, 2);
|
||||
std::ostringstream os;
|
||||
os << p;
|
||||
EXPECT(os.str() == "(1, 2)");
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main () {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -153,16 +153,6 @@ TEST( Point3, cross2) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
TEST( Point3, stream) {
|
||||
Point3 p(1, 2, -3);
|
||||
std::ostringstream os;
|
||||
os << p;
|
||||
EXPECT(os.str() == "[1, 2, -3]'");
|
||||
}
|
||||
#endif
|
||||
|
||||
//*************************************************************************
|
||||
TEST (Point3, normalize) {
|
||||
Matrix actualH;
|
||||
|
|
|
|||
|
|
@ -857,19 +857,11 @@ TEST( Pose3, adjointTranspose) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, stream)
|
||||
{
|
||||
Pose3 T;
|
||||
TEST( Pose3, stream) {
|
||||
std::ostringstream os;
|
||||
os << T;
|
||||
|
||||
string expected;
|
||||
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0 0 0";;
|
||||
#else
|
||||
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'";
|
||||
#endif
|
||||
os << Pose3();
|
||||
|
||||
string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0 0 0";
|
||||
EXPECT(os.str() == expected);
|
||||
}
|
||||
|
||||
|
|
@ -1042,7 +1034,6 @@ TEST(Pose3, print) {
|
|||
|
||||
// Add expected rotation
|
||||
expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
|
||||
|
||||
expected << "t: 1 2 3\n";
|
||||
|
||||
// reset cout to the original stream
|
||||
|
|
|
|||
|
|
@ -215,11 +215,7 @@ TEST(NavState, Stream)
|
|||
os << state;
|
||||
|
||||
string expected;
|
||||
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0";
|
||||
#else
|
||||
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: [0, 0, 0]'\nv: [0, 0, 0]'";
|
||||
#endif
|
||||
|
||||
EXPECT(os.str() == expected);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,30 +75,6 @@ inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
|
|||
return Unit3_(x, &Rot3::unrotate, p);
|
||||
}
|
||||
|
||||
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
|
||||
namespace internal {
|
||||
// define a rotate and unrotate for Vector3
|
||||
inline Vector3 rotate(const Rot3& R, const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) {
|
||||
return R.rotate(v, H1, H2);
|
||||
}
|
||||
inline Vector3 unrotate(const Rot3& R, const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) {
|
||||
return R.unrotate(v, H1, H2);
|
||||
}
|
||||
} // namespace internal
|
||||
inline Expression<Vector3> rotate(const Rot3_& R,
|
||||
const Expression<Vector3>& v) {
|
||||
return Expression<Vector3>(internal::rotate, R, v);
|
||||
}
|
||||
inline Expression<Vector3> unrotate(const Rot3_& R,
|
||||
const Expression<Vector3>& v) {
|
||||
return Expression<Vector3>(internal::unrotate, R, v);
|
||||
}
|
||||
#endif
|
||||
|
||||
// Projection
|
||||
|
||||
typedef Expression<Cal3_S2> Cal3_S2_;
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/stl_bind.h>
|
||||
#include <pybind11/pybind11.h>
|
||||
#include "gtsam/config.h"
|
||||
#include "gtsam/base/serialization.h"
|
||||
#include "gtsam/nonlinear/utilities.h" // for RedirectCout.
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,13 @@
|
|||
// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
|
||||
// These are required to save one copy operation on Python calls
|
||||
#ifdef GTSAM_ALLOCATOR_TBB
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::BetweenFactor<gtsam::Pose3>>);
|
||||
#else
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::BetweenFactor<gtsam::Pose3>>);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,7 +1,12 @@
|
|||
// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
|
||||
// These are required to save one copy operation on Python calls
|
||||
#ifdef GTSAM_ALLOCATOR_TBB
|
||||
py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
|
||||
#else
|
||||
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
|
||||
#endif
|
||||
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
|
||||
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
|
||||
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
|
||||
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,13 @@
|
|||
#!/bin/bash
|
||||
REF=${1-master} # branch or tag; defaults to 'master' if parameter 1 not present
|
||||
REMOTE=wrap # just a name to identify the remote
|
||||
REPO=git@github.com:borglab/wrap.git # replace this with your repository URL
|
||||
FOLDER=wrap # where to mount the subtree
|
||||
|
||||
git remote add $REMOTE --no-tags $REPO
|
||||
if [[ -d $FOLDER ]]; then # update the existing subtree
|
||||
git subtree pull $REMOTE $REF --prefix=$FOLDER --squash -m "Merging '$REF' into '$FOLDER'"
|
||||
else # add the subtree
|
||||
git subtree add $REMOTE $REF --prefix=$FOLDER --squash -m "Merging '$REF' into '$FOLDER'"
|
||||
fi
|
||||
git remote remove $REMOTE
|
||||
Loading…
Reference in New Issue