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

Merge pull request #422 from borglab/dellaert/issue420 

Dellaert/issue420
release/4.3a0
Frank Dellaert 2020-08-02 16:16:01 -04:00 committed by GitHub
parent 0e6b208276 09bb25498f
commit 269dea3a24
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
47 changed files with 39 additions and 876 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.travis.python.sh
View File

@ -29,7 +29,7 @@ cmake $CURRDIR -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \
-DCMAKE_INSTALL_PREFIX=$CURRDIR/../gtsam_install
make -j$(nproc) install

2
.travis.sh
View File

@ -61,7 +61,7 @@ function configure()
-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_V4:-OFF} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DCMAKE_VERBOSE_MAKEFILE=OFF
}

2
.travis.yml
View File

@ -87,7 +87,7 @@ jobs:
- stage: special
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON
script: bash .travis.sh -b
# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
- stage: special

4
CMakeLists.txt
View File

@ -75,7 +75,7 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
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)
@ -590,7 +590,7 @@ print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default R
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1")
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")

7
README.md
View File

@ -44,9 +44,12 @@ Optional prerequisites - used automatically if findable by CMake:
## GTSAM 4 Compatibility
GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. We also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 to use the deprecated methods.
GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag GTSAM_ALLOW_DEPRECATED_SINCE_V41 for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
## Wrappers

5
gtsam/base/DSFVector.h
View File

@ -55,11 +55,6 @@ public:
/// Merge the sets containing i1 and i2. Does nothing if i1 and i2 are already in the same set.
void merge(const size_t& i1, const size_t& i2);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline size_t findSet(size_t key) const {return find(key);}
inline void makeUnionInPlace(const size_t& i1, const size_t& i2) {return merge(i1,i2);}
#endif
};
/**

56
gtsam/base/Manifold.h
View File

@ -167,62 +167,6 @@ struct FixedDimension {
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
"FixedDimension instantiated for dymanically-sized type.");
};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Helper class to construct the product manifold of two other manifolds, M1 and M2
/// Deprecated because of limited usefulness, maximum obfuscation
template<typename M1, typename M2>
class ProductManifold: public std::pair<M1, M2> {
BOOST_CONCEPT_ASSERT((IsManifold<M1>));
BOOST_CONCEPT_ASSERT((IsManifold<M2>));
protected:
enum { dimension1 = traits<M1>::dimension };
enum { dimension2 = traits<M2>::dimension };
public:
enum { dimension = dimension1 + dimension2 };
inline static size_t Dim() { return dimension;}
inline size_t dim() const { return dimension;}
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
/// Default constructor needs default constructors to be defined
ProductManifold():std::pair<M1,M2>(M1(),M2()) {}
// Construct from two original manifold values
ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
/// Retract delta to manifold
ProductManifold retract(const TangentVector& xi) const {
M1 m1 = traits<M1>::Retract(this->first, xi.template head<dimension1>());
M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
return ProductManifold(m1,m2);
}
/// Compute the coordinates in the tangent space
TangentVector localCoordinates(const ProductManifold& other) const {
typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first, other.first);
typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
TangentVector v;
v << v1, v2;
return v;
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
};
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename M1, typename M2>
struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
};
#endif
} // \ namespace gtsam
///**

24
gtsam/base/Matrix.h
View File

@ -23,14 +23,11 @@
// \callgraph
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/config.h>
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#endif
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/tuple/tuple.hpp>
@ -521,23 +518,6 @@ struct MultiplyWithInverseFunction {
const Operator phi_;
};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); }
inline Matrix ones( size_t m, size_t n ) { return Matrix::Ones(m,n); }
inline Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); }
inline Matrix eye( size_t m ) { return eye(m,m); }
inline Matrix diag(const Vector& v) { return v.asDiagonal(); }
inline void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { e += alpha * A * x; }
inline void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { e += A * x; }
inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { x += alpha * A.transpose() * e; }
inline void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { x += A.transpose() * e; }
inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { x += alpha * A.transpose() * e; }
inline void insertColumn(Matrix& A, const Vector& col, size_t j) { A.col(j) = col; }
inline void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { A.col(j).segment(i, col.size()) = col; }
inline void solve(Matrix& A, Matrix& B) { B = A.fullPivLu().solve(B); }
inline Matrix inverse(const Matrix& A) { return A.inverse(); }
#endif
GTSAM_EXPORT Matrix LLt(const Matrix& A);
GTSAM_EXPORT Matrix RtR(const Matrix& A);

20
gtsam/base/Vector.h
View File

@ -256,26 +256,6 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
* concatenate Vectors
*/
GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline Vector abs(const Vector& v){return v.cwiseAbs();}
inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); }
inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;}
inline size_t dim(const Vector& v) { return v.size(); }
inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
inline double max(const Vector &a){return a.maxCoeff();}
inline double norm_2(const Vector& v) {return v.norm();}
inline Vector ones(size_t n) { return Vector::Ones(n); }
inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
inline double sum(const Vector &a){return a.sum();}
inline bool zero(const Vector& v){ return v.isZero(); }
inline Vector zero(size_t n) { return Vector::Zero(n); }
#endif
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

2
gtsam/config.h.in
View File

@ -70,7 +70,7 @@
#cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION
// Make sure dependent projects that want it can see deprecated functions
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41
// Publish flag about Eigen typedef
#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS

11
gtsam/geometry/EssentialMatrix.h
View File

@ -176,17 +176,6 @@ class EssentialMatrix {
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3 transform_to(const Point3& p,
OptionalJacobian<3, 5> DE = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const {
return transformTo(p, DE, Dpoint);
};
/// @}
#endif
private:
/// @name Advanced Interface
/// @{

12
gtsam/geometry/Point2.cpp
View File

@ -79,18 +79,6 @@ ostream &operator<<(ostream &os, const Point2& p) {
return os;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol) {
return circleCircleIntersection(R_d, r_d, tol);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh) {
return circleCircleIntersection(c1, c2, fh);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) {
return circleCircleIntersection(c1, r1, c2, r2, tol);
}
#endif
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */

27
gtsam/geometry/Point2.h
View File

@ -46,12 +46,7 @@ public:
/// @{
/// default constructor
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Deprecated default constructor initializes to zero, in contrast to new behavior below
Point2() { setZero(); }
#else
Point2() {}
#endif
Point2() {}
using Vector2::Vector2;
@ -113,25 +108,7 @@ public:
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point2 inverse() const { return -(*this); }
Point2 compose(const Point2& q) const { return (*this)+q;}
Point2 between(const Point2& q) const { return q-(*this);}
Vector2 localCoordinates(const Point2& q) const { return between(q);}
Point2 retract(const Vector2& v) const { return compose(Point2(v));}
static Vector2 Logmap(const Point2& p) { return p;}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
inline double dist(const Point2& p2) const {return distance(p2);}
GTSAM_EXPORT static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
GTSAM_EXPORT static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
GTSAM_EXPORT static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
/// @}
#endif
private:
private:
/// @name Advanced Interface
/// @{

17
gtsam/geometry/Point3.cpp
View File

@ -62,23 +62,6 @@ ostream &operator<<(ostream &os, const Point3& p) {
return os;
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
return *this + q;
}
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = -I_3x3;
return *this - q;
}
#endif
#endif
/* ************************************************************************* */
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,

27
gtsam/geometry/Point3.h
View File

@ -51,11 +51,6 @@ class Point3 : public Vector3 {
/// @name Standard Constructors
/// @{
// Deprecated default constructor initializes to zero, in contrast to new behavior below
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Point3() { setZero(); }
#endif
using Vector3::Vector3;
/// @}
@ -118,27 +113,7 @@ class Point3 : public Vector3 {
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);}
Vector3 localCoordinates(const Point3& q) const { return between(q);}
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p;}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
inline double dist(const Point3& q) const { return (q - *this).norm(); }
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
GTSAM_EXPORT Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
GTSAM_EXPORT Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// @}
#endif
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

16
gtsam/geometry/Pose2.h
View File

@ -289,22 +289,6 @@ public:
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point2 transform_from(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return transformFrom(point, Dpose, Dpoint);
}
Point2 transform_to(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return transformTo(point, Dpose, Dpoint);
}
/// @}
#endif
private:
// Serialization function

9
gtsam/geometry/Pose3.cpp
View File

@ -290,15 +290,6 @@ Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
return wTa.compose(aTb, Hself, HaTb);
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
Point3 t = pose.transform_to(t_);
return Pose3(cRv, t);
}
#endif
/* ************************************************************************* */
Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> HwTb) const {

29
gtsam/geometry/Pose3.h
View File

@ -326,30 +326,6 @@ public:
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3 transform_from(const Point3& point,
OptionalJacobian<3, 6> Hself = boost::none,
OptionalJacobian<3, 3> Hpoint = boost::none) const {
return transformFrom(point, Hself, Hpoint);
}
Point3 transform_to(const Point3& point,
OptionalJacobian<3, 6> Hself = boost::none,
OptionalJacobian<3, 3> Hpoint = boost::none) const {
return transformTo(point, Hself, Hpoint);
}
Pose3 transform_pose_to(const Pose3& pose,
OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> Hpose = boost::none) const {
return transformPoseTo(pose, Hself, Hpose);
}
/**
* @deprecated: this function is neither here not there. */
Pose3 transform_to(const Pose3& pose) const;
/// @}
#endif
private:
/** Serialization function */
friend class boost::serialization::access;
@ -380,11 +356,6 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
// deprecated: use Pose3::Align with point pairs ordered the opposite way
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& baPointPairs);
#endif
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;

17
gtsam/geometry/Rot3.h
View File

@ -500,23 +500,6 @@ namespace gtsam {
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }
static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); }
static Rot3 yaw (double t) { return Yaw(t); }
static Rot3 pitch(double t) { return Pitch(t); }
static Rot3 roll (double t) { return Roll(t); }
static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);}
static Rot3 quaternion(double w, double x, double y, double z) {
return Rot3::Quaternion(w, x, y, z);
}
/// @}
#endif
private:
/** Serialization function */
friend class boost::serialization::access;

21
gtsam/geometry/triangulation.h
View File

@ -123,27 +123,6 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
return std::make_pair(graph, values);
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// DEPRECATED: PinholeCamera specific version
template<class CALIBRATION>
Point3 triangulateNonlinear(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, const Point3& initialEstimate) {
return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
(cameras, measurements, initialEstimate);
}
/// DEPRECATED: PinholeCamera specific version
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate) {
return triangulationGraph<PinholeCamera<CALIBRATION> > //
(cameras, measurements, landmarkKey, initialEstimate);
}
#endif
/**
* Optimize for triangulation
* @param graph nonlinear factors for projection

19
gtsam/inference/BayesTree.h
View File

@ -249,25 +249,6 @@ namespace gtsam {
// Friend JunctionTree because it directly fills roots and nodes index.
template<class BAYESRTEE, class GRAPH> friend class EliminatableClusterTree;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
public:
/// @name Deprecated
/// @{
void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) {
removePath(clique, &bn, &orphans);
}
void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) {
removeTop(keys, &bn, &orphans);
}
void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const {
getCliqueData(clique, &stats);
}
void addFactorsToGraph(FactorGraph<FactorType>& graph) const{
addFactorsToGraph(& graph);
}
/// @}
#endif
private:
/** Serialization function */
friend class boost::serialization::access;

11
gtsam/inference/ISAM.h
View File

@ -72,17 +72,6 @@ class ISAM : public BAYESTREE {
const Eliminate& function = EliminationTraitsType::DefaultEliminate);
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
void update_internal(
const FactorGraphType& newFactors, Cliques& orphans,
const Eliminate& function = EliminationTraitsType::DefaultEliminate) {
updateInternal(newFactors, &orphans, function);
}
/// @}
#endif
};
} // namespace gtsam

12
gtsam/linear/GaussianConditional.h
View File

@ -128,17 +128,9 @@ namespace gtsam {
/** Scale the values in \c gy according to the sigmas for the frontal variables in this
* conditional. */
void scaleFrontalsBySigma(VectorValues& gy) const;
// __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const; // FIXME: depreciated flag doesn't appear to exist?
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
constABlock get_R() const { return R(); }
constABlock get_S() const { return S(); }
constABlock get_S(const_iterator it) const { return S(it); }
const constBVector get_d() const { return d(); }
/// @}
#endif
// FIXME: deprecated flag doesn't appear to exist?
// __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const;
private:
/** Serialization function */

11
gtsam/linear/HessianFactor.h
View File

@ -354,16 +354,7 @@ namespace gtsam {
/// Solve the system A'*A delta = A'*b in-place, return delta as VectorValues
VectorValues solve();
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
const SymmetricBlockMatrix& matrixObject() const { return info_; }
/// @}
#endif
private:
private:
/// Allocate for given scatter pattern
void Allocate(const Scatter& scatter);

12
gtsam/linear/LossFunctions.h
View File

@ -82,10 +82,6 @@ class GTSAM_EXPORT Base {
*/
virtual double loss(double distance) const { return 0; };
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double residual(double distance) const { return loss(distance); };
#endif
/*
* This method is responsible for returning the weight function for a given
* amount of error. The weight function is related to the analytic derivative
@ -278,14 +274,6 @@ class GTSAM_EXPORT Welsch : public Base {
ar &BOOST_SERIALIZATION_NVP(c_);
}
};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
// Welsh implements the "Welsch" robust error model (Zhang97ivc)
// This was misspelled in previous versions of gtsam and should be
// removed in the future.
using Welsh = Welsch;
#endif
/// GemanMcClure implements the "Geman-McClure" robust error model
/// (Zhang97ivc).

11
gtsam/linear/NoiseModel.cpp
View File

@ -376,17 +376,6 @@ Vector Constrained::whiten(const Vector& v) const {
return c;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/* ************************************************************************* */
double Constrained::error(const Vector& v) const {
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
if (constrained(i)) // whiten makes constrained variables zero
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
return 0.5 * w.dot(w);
}
#endif
/* ************************************************************************* */
double Constrained::squaredMahalanobisDistance(const Vector& v) const {
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements

39
gtsam/linear/NoiseModel.h
View File

@ -103,15 +103,6 @@ namespace gtsam {
return 0.5 * squared_distance;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// calculate the error value given measurement error vector
virtual double error(const Vector& v) const = 0;
virtual double distance(const Vector& v) {
return error(v) * 2;
}
#endif
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
@ -226,19 +217,6 @@ namespace gtsam {
Vector whiten(const Vector& v) const override;
Vector unwhiten(const Vector& v) const override;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double Mahalanobis(const Vector& v) const {
return squaredMahalanobisDistance(v);
}
/**
* error value 0.5 * v'*R'*R*v
*/
inline double error(const Vector& v) const override {
return 0.5 * squaredMahalanobisDistance(v);
}
#endif
/**
* Multiply a derivative with R (derivative of whiten)
* Equivalent to whitening each column of the input matrix.
@ -483,15 +461,6 @@ namespace gtsam {
return MixedVariances(precisions.array().inverse());
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/**
* The error function for a constrained noisemodel,
* for non-constrained versions, uses sigmas, otherwise
* uses the penalty function with mu
*/
double error(const Vector& v) const override;
#endif
double squaredMahalanobisDistance(const Vector& v) const override;
/** Fully constrained variations */
@ -720,14 +689,6 @@ namespace gtsam {
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline Vector unwhiten(const Vector& /*v*/) const override
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline double distance(const Vector& v) override {
return robust_->loss(this->unweightedWhiten(v).norm());
}
// Fold the use of the m-estimator loss(...) function into error(...)
inline double error(const Vector& v) const override
{ return robust_->loss(noise_->mahalanobisDistance(v)); }
#endif
double loss(const double squared_distance) const override {
return robust_->loss(std::sqrt(squared_distance));

11
gtsam/linear/Sampler.cpp
View File

@ -54,17 +54,6 @@ Vector Sampler::sample() const {
return sampleDiagonal(sigmas);
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {}
Vector Sampler::sampleNewModel(
const noiseModel::Diagonal::shared_ptr& model) const {
assert(model.get());
const Vector& sigmas = model->sigmas();
return sampleDiagonal(sigmas);
}
#endif
/* ************************************************************************* */
} // namespace gtsam

8
gtsam/linear/Sampler.h
View File

@ -84,14 +84,6 @@ class GTSAM_EXPORT Sampler {
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
explicit Sampler(uint_fast64_t seed = 42u);
Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const;
/// @}
#endif
protected:
/** given sigmas for a diagonal model, returns a sample */
Vector sampleDiagonal(const Vector& sigmas) const;

17
gtsam/linear/SubgraphSolver.cpp
View File

@ -65,23 +65,6 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
: SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2,
parameters) {}
/**************************************************************************************************/
// deprecated variants
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph &Ab2,
const Parameters &parameters)
: SubgraphSolver(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2),
parameters) {}
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
const GaussianFactorGraph &Ab2,
const Parameters &parameters,
const Ordering &ordering)
: SubgraphSolver(Ab1, boost::make_shared<GaussianFactorGraph>(Ab2),
parameters, ordering) {}
#endif
/**************************************************************************************************/
VectorValues SubgraphSolver::optimize() const {
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,

17
gtsam/linear/SubgraphSolver.h
View File

@ -136,23 +136,6 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
const GaussianFactorGraph &gfg);
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
const Parameters &parameters, const Ordering &ordering)
: SubgraphSolver(*A, parameters, ordering) {}
SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &,
const Parameters &, const Ordering &);
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, const Ordering &ordering)
: SubgraphSolver(*Ab1, Ab2, parameters, ordering) {}
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &,
const GaussianFactorGraph &, const Parameters &);
/// @}
#endif
};
} // namespace gtsam

2
gtsam/navigation/AHRSFactor.h
View File

@ -75,7 +75,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
biasHat_(bias_hat),
preintMeasCov_(preint_meas_cov) {}
const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
const Vector3& biasHat() const { return biasHat_; }
const Matrix3& preintMeasCov() const { return preintMeasCov_; }

58
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -148,29 +148,6 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
const bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
p->biasAccCovariance = biasAccCovariance;
p->biasOmegaCovariance = biasOmegaCovariance;
p->biasAccOmegaInt = biasAccOmegaInt;
p_ = p;
resetIntegration();
preintMeasCov_.setZero();
}
#endif
//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
@ -275,41 +252,6 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os;
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
using P = CombinedPreintegratedMeasurements::Params;
auto p = boost::allocate_shared<P>(Eigen::aligned_allocator<P>(), pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
_PIM_.p_ = p;
}
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose;
vel_j = pvb.velocity;
}
#endif
}
/// namespace gtsam

32
gtsam/navigation/CombinedImuFactor.h
View File

@ -220,17 +220,6 @@ public:
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration = true);
#endif
private:
/// Serialization function
friend class boost::serialization::access;
@ -338,26 +327,7 @@ public:
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated typename
typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
/// @deprecated constructor
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j, const CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
// @deprecated use PreintegrationBase::predict
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis = false);
#endif
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>

64
gtsam/navigation/ImuFactor.cpp
View File

@ -106,32 +106,6 @@ void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements&
preintMeasCov_ = P + *H2 * pim12.preintMeasCov_ * H2->transpose();
}
#endif
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
p_ = p;
resetIntegration();
}
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<Pose3> body_P_sensor) {
// modify parameters to accommodate deprecated method:-(
p_->body_P_sensor = body_P_sensor;
integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}
#endif
//------------------------------------------------------------------------------
// ImuFactor methods
//------------------------------------------------------------------------------
@ -218,43 +192,15 @@ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
// return new factor
auto pim02 =
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(),// P0
f01->key2(),// V0
f12->key3(),// P2
f12->key4(),// V2
f01->key5(),// B
return boost::make_shared<ImuFactor>(f01->key1(), // P0
f01->key2(), // V0
f12->key3(), // P2
f12->key4(), // V2
f01->key5(), // B
pim02);
}
#endif
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedImuMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias), _PIM_(pim) {
boost::shared_ptr<PreintegrationParams> p = boost::make_shared<
PreintegrationParams>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
_PIM_.p_ = p;
}
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedImuMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose;
vel_j = pvb.velocity;
}
#endif
//------------------------------------------------------------------------------
// ImuFactor2 methods
//------------------------------------------------------------------------------

41
gtsam/navigation/ImuFactor.h
View File

@ -140,24 +140,7 @@ public:
void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
#endif
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
bool use2ndOrderIntegration = true);
/// @deprecated version of integrateMeasurement with body_P_sensor
/// Use parameters instead
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt,
boost::optional<Pose3> body_P_sensor);
#endif
private:
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
@ -253,27 +236,7 @@ public:
static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
#endif
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated typename
typedef PreintegratedImuMeasurements PreintegratedMeasurements;
/// @deprecated constructor, in the new one gravity, coriolis settings are in PreintegrationParams
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
/// @deprecated use PreintegrationBase::predict,
/// in the new one gravity, coriolis settings are in PreintegrationParams
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
#endif
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

16
gtsam/navigation/PreintegrationBase.cpp
View File

@ -193,22 +193,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
return error;
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) const {
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
q->n_gravity = n_gravity;
q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}
#endif
//------------------------------------------------------------------------------
} // namespace gtsam

47
gtsam/navigation/PreintegrationBase.h
View File

@ -32,25 +32,6 @@
namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated
struct PoseVelocityBias {
Pose3 pose;
Vector3 velocity;
imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
const imuBias::ConstantBias _bias) :
pose(_pose), velocity(_velocity), bias(_bias) {
}
PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) :
pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {
}
NavState navState() const {
return NavState(pose, velocity);
}
};
#endif
/**
* PreintegrationBase is the base class for PreintegratedMeasurements
* (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor).
@ -63,11 +44,6 @@ class GTSAM_EXPORT PreintegrationBase {
typedef PreintegrationParams Params;
protected:
/// Parameters. Declared mutable only for deprecated predict method.
/// TODO(frank): make const once deprecated method is removed
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
mutable
#endif
boost::shared_ptr<Params> p_;
/// Acceleration and gyro bias used for preintegration
@ -117,16 +93,11 @@ class GTSAM_EXPORT PreintegrationBase {
}
/// const reference to params
const Params& p() const {
return *boost::static_pointer_cast<Params>(p_);
Params& p() const {
return *p_;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
void set_body_P_sensor(const Pose3& body_P_sensor) {
p_->body_P_sensor = body_P_sensor;
}
#endif
/// @}
/// @}
/// @name Instance variables access
/// @{
@ -201,18 +172,6 @@ class GTSAM_EXPORT PreintegrationBase {
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
/// @deprecated predict
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const;
/// @}
#endif
private:
/** Serialization function */
friend class boost::serialization::access;

9
gtsam/navigation/ScenarioRunner.h
View File

@ -104,15 +104,6 @@ class GTSAM_EXPORT ScenarioRunner {
/// Estimate covariance of sampled noise for sanity-check
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
: ScenarioRunner(*scenario, p, imuSampleTime, bias) {}
/// @}
#endif
};
} // namespace gtsam

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

@ -163,37 +163,6 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
TEST(NavState, Update) {
Vector3 omega(M_PI / 100.0, 0.0, 0.0);
Vector3 acc(0.1, 0.0, 0.0);
double dt = 10;
Matrix9 aF;
Matrix93 aG1, aG2;
boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
boost::none, boost::none);
Vector3 b_acc = kAttitude * acc;
NavState expected(kAttitude.expmap(dt * omega),
kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),
kVelocity + b_acc * dt);
NavState actual = kState1.update(acc, omega, dt, aF, aG1, aG2);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
// Try different values
omega = Vector3(0.1, 0.2, 0.3);
acc = Vector3(0.4, 0.5, 0.6);
kState1.update(acc, omega, dt, aF, aG1, aG2);
EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
}
#endif
/* ************************************************************************* */
static const double dt = 2.0;
boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(

34
gtsam/navigation/tests/testPoseVelocityBias.cpp
View File

@ -25,40 +25,6 @@
using namespace std;
using namespace gtsam;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
Matrix3 R1 = pvb1.pose.rotation().matrix();
// Ri.transpose() translate the error from the global frame into pose1's frame
const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation());
const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity);
const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation());
const Vector3 fR = Rot3::Logmap(R1BetweenR2);
Vector9 r;
r << fp, fv, fR;
return r;
}
/* ************************************************************************************************/
TEST(PoseVelocityBias, error) {
Point3 i1(0, 1, 0), j1(-1, 0, 0), k1(0, 0, 1);
Pose3 x1(Rot3(i1, j1, k1), Point3(5.0, 1.0, 0.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
imuBias::ConstantBias bias1(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
Pose3 x2(Rot3(i1, j1, k1).expmap(Vector3(0.1, 0.2, 0.3)), Point3(5.5, 1.0, 6.0));
Vector3 v2(Vector3(0.5, 4.0, 3.0));
imuBias::ConstantBias bias2(Vector3(0.1, 0.2, -0.3), Vector3(0.2, 0.3, 0.1));
PoseVelocityBias pvb1(x1, v1, bias1), pvb2(x2, v2, bias2);
Vector9 expected, actual = error(pvb1, pvb2);
expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3;
EXPECT(assert_equal(expected, actual, 1e-9));
}
#endif
/* ************************************************************************************************/
int main() {
TestResult tr;

17
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -140,21 +140,14 @@ public:
* Utility function for converting linear graphs to nonlinear graphs
* consisting of LinearContainerFactors.
*/
GTSAM_EXPORT static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph,
GTSAM_EXPORT
static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph,
const Values& linearizationPoint = Values());
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
GTSAM_EXPORT static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
const Values& linearizationPoint = Values()) {
return ConvertLinearGraph(linear_graph, linearizationPoint);
}
#endif
protected:
GTSAM_EXPORT void initializeLinearizationPoint(const Values& linearizationPoint);
private:
protected:
GTSAM_EXPORT void initializeLinearizationPoint(const Values& linearizationPoint);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

19
gtsam/nonlinear/NonlinearFactor.h
View File

@ -29,13 +29,6 @@
#include <boost/serialization/base_object.hpp>
#include <boost/assign/list_of.hpp>
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
#define ADD_CLONE_NONLINEAR_FACTOR(Derived) \
gtsam::NonlinearFactor::shared_ptr clone() const override { \
return boost::static_pointer_cast<gtsam::NonlinearFactor>( \
gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }
#endif
namespace gtsam {
using boost::assign::cref_list_of;
@ -251,21 +244,13 @@ public:
*/
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
SharedNoiseModel get_noiseModel() const { return noiseModel_; }
/// @}
#endif
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(noiseModel_);
}

19
gtsam/slam/SmartProjectionFactor.h
View File

@ -443,25 +443,6 @@ public:
/** return the farPoint state */
bool isFarPoint() const { return result_.farPoint(); }
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
// It does not make sense to optimize for a camera where the pose would not be
// the actual pose of the camera. An unfortunate consequence of deprecating
// this constructor means that we cannot optimize for calibration when the
// camera is offset from the body pose. That would need a new factor with
// (body) pose and calibration as variables. However, that use case is
// unlikely: when a global offset is know, calibration is typically known.
SmartProjectionFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::optional<Pose3> body_P_sensor,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, body_P_sensor),
params_(params),
result_(TriangulationResult::Degenerate()) {}
/// @}
#endif
private:
/// Serialization function

11
gtsam/slam/dataset.h
View File

@ -324,13 +324,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
/// Aliases for backwards compatibility
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
typedef SfmMeasurement SfM_Measurement;
typedef SiftIndex SIFT_Index;
typedef SfmTrack SfM_Track;
typedef SfmCamera SfM_Camera;
typedef SfmData SfM_data;
#endif
} // namespace gtsam
} // namespace gtsam

9
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -117,15 +117,6 @@ public:
const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys,
const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
static NonlinearFactorGraph calculateMarginalFactors(
const NonlinearFactorGraph& graph, const Values& theta, const std::set<Key>& keys,
const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky) {
KeyVector keyVector(keys.begin(), keys.end());
return CalculateMarginalFactors(graph, theta, keyVector, eliminateFunction);
}
#endif
protected:
/** A typedef defining an Key-Factor mapping **/

28
tests/testManifold.cpp
View File

@ -149,34 +149,6 @@ TEST(Manifold, DefaultChart) {
EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
//******************************************************************************
typedef ProductManifold<Point2,Point2> MyPoint2Pair;
// Define any direct product group to be a model of the multiplicative Group concept
namespace gtsam {
template<> struct traits<MyPoint2Pair> : internal::ManifoldTraits<MyPoint2Pair> {
static void Print(const MyPoint2Pair& m, const string& s = "") {
cout << s << "(" << m.first << "," << m.second << ")" << endl;
}
static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) {
return m1 == m2;
}
};
}
TEST(Manifold, ProductManifold) {
BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
MyPoint2Pair pair1(Point2(0,0),Point2(0,0));
Vector4 d;
d << 1,2,3,4;
MyPoint2Pair expected(Point2(1,2),Point2(3,4));
MyPoint2Pair pair2 = pair1.retract(d);
EXPECT(assert_equal(expected,pair2,1e-9));
EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9));
}
#endif
//******************************************************************************
int main() {
TestResult tr;