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

Merge branch 'develop' into fix/wrapper-todos

release/4.3a0
Varun Agrawal 2020-09-10 15:57:49 -04:00
parent 13486166c6 5a16ce4fe2
commit 2ebfdca175
15 changed files with 237 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -166,7 +166,7 @@ if(MSVC AND BUILD_SHARED_LIBS)
endif()
# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
set(BOOST_FIND_MINIMUM_VERSION 1.43)
set(BOOST_FIND_MINIMUM_VERSION 1.58)
set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
@ -174,7 +174,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM
# Required components
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.")
endif()
option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
@ -490,7 +490,7 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION
# Deb-package specific cpack
set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
###############################################################################

2
INSTALL.md
View File

@ -13,7 +13,7 @@ $ make install
## Important Installation Notes
1. GTSAM requires the following libraries to be installed on your system:
- BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
- BOOST version 1.58 or greater (install through Linux repositories or MacPorts)
- Cmake version 3.0 or higher
- Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher

2
README.md
View File

@ -40,7 +40,7 @@ $ make install
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.

9
gtsam/base/DSFMap.h
View File

@ -21,6 +21,7 @@
#include <cstdlib> // Provides size_t
#include <map>
#include <set>
#include <vector>
namespace gtsam {
@ -120,4 +121,12 @@ class IndexPair : public std::pair<size_t,size_t> {
inline size_t i() const { return first; };
inline size_t j() const { return second; };
};
typedef std::vector<IndexPair> IndexPairVector;
typedef std::set<IndexPair> IndexPairSet;
inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); }
typedef std::map<IndexPair, IndexPairSet> IndexPairSetMap;
typedef DSFMap<IndexPair> DSFMapIndexPair;
} // namespace gtsam

7
gtsam/base/numericalDerivative.h
View File

@ -700,9 +700,10 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),

75
gtsam/gtsam.i
View File

@ -259,11 +259,59 @@ class IndexPair {
size_t j() const;
};
template<KEY = {gtsam::IndexPair}>
class DSFMap {
DSFMap();
KEY find(const KEY& key) const;
void merge(const KEY& x, const KEY& y);
// template<KEY = {gtsam::IndexPair}>
// class DSFMap {
// DSFMap();
// KEY find(const KEY& key) const;
// void merge(const KEY& x, const KEY& y);
// std::map<KEY, Set> sets();
// };
class IndexPairSet {
IndexPairSet();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
void insert(gtsam::IndexPair key);
bool erase(gtsam::IndexPair key); // returns true if value was removed
bool count(gtsam::IndexPair key) const; // returns true if value exists
};
class IndexPairVector {
IndexPairVector();
IndexPairVector(const gtsam::IndexPairVector& other);
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::IndexPair at(size_t i) const;
void push_back(gtsam::IndexPair key) const;
};
gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
class IndexPairSetMap {
IndexPairSetMap();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::IndexPairSet at(gtsam::IndexPair& key);
};
class DSFMapIndexPair {
DSFMapIndexPair();
gtsam::IndexPair find(const gtsam::IndexPair& key) const;
void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y);
gtsam::IndexPairSetMap sets();
};
#include <gtsam/base/Matrix.h>
@ -2929,6 +2977,13 @@ class BinaryMeasurement {
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
class BinaryMeasurementsUnit3 {
BinaryMeasurementsUnit3();
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
};
#include <gtsam/sfm/ShonanAveraging.h>
// TODO(frank): copy/pasta below until we have integer template paremeters in wrap!
@ -3050,6 +3105,16 @@ class ShonanAveraging3 {
pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
};
#include <gtsam/sfm/TranslationRecovery.h>
class TranslationRecovery {
TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,
const gtsam::LevenbergMarquardtParams &lmParams);
TranslationRecovery(
const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams
gtsam::Values run(const double scale) const;
gtsam::Values run() const; // default scale = 1.0
};
//*************************************************************************
// Navigation
//*************************************************************************

33
gtsam/navigation/NavState.cpp
View File

@ -218,28 +218,37 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
const double dt2 = dt * dt;
const Vector3 omega_cross_vel = omega.cross(n_v);
Vector9 xi;
Matrix3 D_dP_R;
dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0);
dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(xi) << ((-2.0 * dt) * omega_cross_vel);
// Get perturbations in nav frame
Vector9 n_xi, xi;
Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
dR(n_xi) << ((-dt) * omega);
dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(n_xi) << ((-2.0 * dt) * omega_cross_vel);
if (secondOrder) {
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
dP(xi) -= (0.5 * dt2) * omega_cross2_t;
dV(xi) -= dt * omega_cross2_t;
dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
dV(n_xi) -= dt * omega_cross2_t;
}
// Transform n_xi into the body frame
xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0),
nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0),
nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
if (H) {
H->setZero();
const Matrix3 Omega = skewSymmetric(omega);
const Matrix3 D_cross_state = Omega * R();
H->setZero();
D_R_R(H) << D_dP_R;
D_t_v(H) << (-dt2) * D_cross_state;
D_v_v(H) << (-2.0 * dt) * D_cross_state;
D_R_R(H) << D_dR_R;
D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
D_t_R(H) << D_dP_R;
D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
D_v_R(H) << D_dV_R;
if (secondOrder) {
const Matrix3 D_cross2_state = Omega * D_cross_state;
D_t_t(H) -= (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= dt * D_cross2_state;
D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= D_body_nav * dt * D_cross2_state;
}
}
return xi;

43
gtsam/navigation/tests/testNavState.cpp
View File

@ -192,6 +192,49 @@ TEST(NavState, Coriolis2) {
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
}
TEST(NavState, Coriolis3) {
/** Consider a massless planet with an attached nav frame at
* n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with
* velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously
* aligned with the nav frame (i.e., nRb != I_3x3). Test that first and
* second order Coriolis corrections are as expected.
*/
// Get true first and second order coriolis accelerations
double dt = 2.0, dt2 = dt * dt;
Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0);
Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v),
n_aCorr2 = -n_omega.cross(n_omega.cross(n_t));
Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0),
bRn = nRb.inverse();
// Get expected first and second order corrections in the nav frame
Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
n_dV1e = dt * n_aCorr1,
n_dV2e = dt * (n_aCorr1 + n_aCorr2);
// Get expected first and second order corrections in the body frame
Vector3 dRe = -dt * (bRn * n_omega),
b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e,
b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e;
// Get actual first and scond order corrections in body frame
NavState kState2(nRb, n_t, n_v);
Vector9 dXi1a = kState2.coriolis(dt, n_omega, false),
dXi2a = kState2.coriolis(dt, n_omega, true);
Vector3 dRa = NavState::dR(dXi1a),
b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a),
b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a);
EXPECT(assert_equal(dRe, dRa));
EXPECT(assert_equal(b_dP1e, b_dP1a));
EXPECT(assert_equal(b_dV1e, b_dV1a));
EXPECT(assert_equal(b_dP2e, b_dP2a));
EXPECT(assert_equal(b_dV2e, b_dV2a));
}
/* ************************************************************************* */
TEST(NavState, CorrectPIM) {
Vector9 xi;

2
gtsam/slam/dataset.cpp
View File

@ -1130,7 +1130,7 @@ bool readBAL(const string &filename, SfmData &data) {
}
/* ************************************************************************* */
SfmData readBAL(const string &filename) {
SfmData readBal(const string &filename) {
SfmData data;
readBAL(filename, data);
return data;

3
gtsam/slam/dataset.h
View File

@ -283,7 +283,7 @@ GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
* @param filename The name of the BAL file.
* @return SfM structure where the data is stored.
*/
GTSAM_EXPORT SfmData readBAL(const std::string& filename);
GTSAM_EXPORT SfmData readBal(const std::string& filename);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
@ -362,6 +362,7 @@ parse3DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
inline boost::optional<IndexedPose> parseVertex(std::istream &is,
const std::string &tag) {

8
python/CMakeLists.txt
View File

@ -30,11 +30,14 @@ set(ignore
gtsam::ISAM2ThresholdMapValue
gtsam::FactorIndices
gtsam::FactorIndexSet
gtsam::IndexPairSetMap
gtsam::IndexPairVector
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Pose3Vector
gtsam::KeyVector)
gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3)
pybind_wrap(gtsam_py # target
${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
@ -76,7 +79,8 @@ set(ignore
gtsam::Point2Vector
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue)
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsUnit3)
pybind_wrap(gtsam_unstable_py # target
${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
"gtsam_unstable.cpp" # generated_cpp

1
python/gtsam/preamble.h
View File

@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);

3
python/gtsam/specializations.h
View File

@ -9,3 +9,6 @@ py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point
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");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");

58
python/gtsam/tests/test_TranslationRecovery.py Normal file
View File

@ -0,0 +1,58 @@
from __future__ import print_function
import numpy as np
import unittest
import gtsam
""" Returns example pose values of 3 points A, B and C in the world frame """
def ExampleValues():
T = []
T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65])))
T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00])))
T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00])))
data = gtsam.Values()
for i in range(len(T)):
data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i]))
return data
""" Returns binary measurements for the points in the given edges."""
def SimulateMeasurements(gt_poses, graph_edges):
measurements = gtsam.BinaryMeasurementsUnit3()
for edge in graph_edges:
Ta = gt_poses.atPose3(edge[0]).translation()
Tb = gt_poses.atPose3(edge[1]).translation()
measurements.append(gtsam.BinaryMeasurementUnit3( \
edge[0], edge[1], gtsam.Unit3(Tb - Ta), \
gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
return measurements
""" Tests for the translation recovery class """
class TestTranslationRecovery(unittest.TestCase):
"""Test selected Translation Recovery methods."""
def test_constructor(self):
"""Construct from binary measurements."""
algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3())
self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
def test_run(self):
gt_poses = ExampleValues()
measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
algorithm = gtsam.TranslationRecovery(measurements)
scale = 2.0
result = algorithm.run(scale)
w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation()
w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation()
w_aTc_expected = w_aTc*scale/np.linalg.norm(w_aTb)
w_aTb_expected = w_aTb*scale/np.linalg.norm(w_aTb)
np.testing.assert_array_almost_equal(result.atPoint3(0), np.array([0,0,0]), 6, "Origin result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(1), w_aTb_expected, 6, "Point B result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(2), w_aTc_expected, 6, "Point C result is incorrect.")
if __name__ == "__main__":
unittest.main()

14
python/gtsam/tests/test_dsf_map.py
View File

@ -35,6 +35,20 @@ class TestDSFMap(GtsamTestCase):
dsf.merge(pair1, pair2)
self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
def test_sets(self):
from gtsam import IndexPair
dsf = gtsam.DSFMapIndexPair()
dsf.merge(IndexPair(0, 1), IndexPair(1,2))
dsf.merge(IndexPair(0, 1), IndexPair(3,4))
dsf.merge(IndexPair(4,5), IndexPair(6,8))
sets = dsf.sets()
for i in sets:
s = sets[i]
for val in gtsam.IndexPairSetAsArray(s):
val.i()
val.j()
if __name__ == '__main__':
unittest.main()