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

Merged in fix/alignment (pull request #333) 

Fixing more alignment issues
release/4.3a0
Frank Dellaert 2018-11-07 15:12:58 +00:00
parent 1d16a6c391 8e7892875a
commit 2194762f11
28 changed files with 257 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -17,3 +17,5 @@ cython/gtsam.so
cython/gtsam_wrapper.pxd
.vscode
.env
/.vs/
/CMakeSettings.json

42
examples/SolverComparer.cpp
View File

@ -251,10 +251,10 @@ void runIncremental()
Key firstPose;
while(nextMeasurement < datasetMeasurements.size())
{
if(BetweenFactor<Pose>::shared_ptr measurement =
if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{
Key key1 = measurement->key1(), key2 = measurement->key2();
Key key1 = factor->key1(), key2 = factor->key2();
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep
firstPose = std::min(key1, key2);
@ -302,52 +302,53 @@ void runIncremental()
NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
if(BetweenFactor<Pose>::shared_ptr measurement =
if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
{
// Stop collecting measurements that are for future steps
if(measurement->key1() > step || measurement->key2() > step)
if(factor->key1() > step || factor->key2() > step)
break;
// Require that one of the nodes is the current one
if(measurement->key1() != step && measurement->key2() != step)
if(factor->key1() != step && factor->key2() != step)
throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add a new factor
newFactors.push_back(measurement);
newFactors.push_back(factor);
const auto& measured = factor->measured();
// Initialize the new variable
if(measurement->key1() > measurement->key2()) {
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
if(factor->key1() > factor->key2()) {
if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(measurement->key1(), measurement->measured().inverse());
newVariables.insert(factor->key1(), measured.inverse());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
newVariables.insert(factor->key1(), prevPose * measured.inverse());
}
}
} else {
if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry
if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(measurement->key2(), measurement->measured());
newVariables.insert(factor->key2(), measured);
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
newVariables.insert(measurement->key2(), prevPose * measurement->measured());
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
newVariables.insert(factor->key2(), prevPose * measured);
}
}
}
}
else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
{
Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
// Stop collecting measurements that are for future steps
if(poseKey > step)
throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add new factor
newFactors.push_back(measurement);
newFactors.push_back(factor);
// Initialize new landmark
if(!isam2.getLinearizationPoint().exists(lmKey))
@ -357,8 +358,9 @@ void runIncremental()
pose = isam2.calculateEstimate<Pose>(poseKey);
else
pose = newVariables.at<Pose>(poseKey);
Rot2 measuredBearing = measurement->measured().first;
double measuredRange = measurement->measured().second;
const auto& measured = factor->measured();
Rot2 measuredBearing = measured.bearing();
double measuredRange = measured.range();
newVariables.insert(lmKey,
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
}

2
gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h vendored
View File

@ -52,7 +52,7 @@ struct general_matrix_matrix_triangular_product<Index,Scalar,LhsStorageOrder,Con
static EIGEN_STRONG_INLINE void run(Index size, Index depth,const Scalar* lhs, Index lhsStride, \
const Scalar* rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking<Scalar, Scalar>& blocking) \
{ \
if ( lhs==rhs && ((UpLo&(Lower|Upper)==UpLo)) ) { \
if ( lhs==rhs && ((UpLo&(Lower|Upper))==UpLo) ) { \
general_matrix_matrix_rankupdate<Index,Scalar,LhsStorageOrder,ConjugateLhs,ColMajor,UpLo> \
::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resStride,alpha,blocking); \
} else { \

2
gtsam/base/GenericValue.h
View File

@ -110,7 +110,7 @@ public:
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
virtual boost::shared_ptr<Value> clone() const {
return boost::make_shared<GenericValue>(*this);
return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
}
/// Generic Value interface version of retract

11
gtsam/base/Manifold.h
View File

@ -168,9 +168,9 @@ struct FixedDimension {
"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
/// Assumes nothing except manifold structure for M1 and M2, and the existence
/// of default constructor for those types
/// Deprecated because of limited usefulness, maximum obfuscation
template<typename M1, typename M2>
class ProductManifold: public std::pair<M1, M2> {
BOOST_CONCEPT_ASSERT((IsManifold<M1>));
@ -209,12 +209,19 @@ public:
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:
EIGEN_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

89
gtsam/geometry/BearingRange.h
View File

@ -45,23 +45,39 @@ struct Range;
* and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
*/
template <typename A1, typename A2>
struct BearingRange
: public ProductManifold<typename Bearing<A1, A2>::result_type,
typename Range<A1, A2>::result_type> {
struct BearingRange {
private:
typedef typename Bearing<A1, A2>::result_type B;
typedef typename Range<A1, A2>::result_type R;
typedef ProductManifold<B, R> Base;
B bearing_;
R range_;
public:
enum { dimB = traits<B>::dimension };
enum { dimR = traits<R>::dimension };
enum { dimension = dimB + dimR };
/// @name Standard Constructors
/// @{
BearingRange() {}
BearingRange(const ProductManifold<B, R>& br) : Base(br) {}
BearingRange(const B& b, const R& r) : Base(b, r) {}
BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
/// @}
/// @name Standard Interface
/// @{
/// Return bearing measurement
const B& bearing() const { return bearing_; }
/// Return range measurement
const R& range() const { return range_; }
/// Prediction function that stacks measurements
static BearingRange Measure(
const A1& a1, const A2& a2,
OptionalJacobian<Base::dimension, traits<A1>::dimension> H1 = boost::none,
OptionalJacobian<Base::dimension, traits<A2>::dimension> H2 =
boost::none) {
OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
typename MakeJacobian<B, A1>::type HB1;
typename MakeJacobian<B, A2>::type HB2;
typename MakeJacobian<R, A1>::type HR1;
@ -75,25 +91,68 @@ struct BearingRange
return BearingRange(b, r);
}
/// @}
/// @name Testable
/// @{
void print(const std::string& str = "") const {
std::cout << str;
traits<B>::Print(this->first, "bearing ");
traits<R>::Print(this->second, "range ");
traits<B>::Print(bearing_, "bearing ");
traits<R>::Print(range_, "range ");
}
bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
return traits<B>::Equals(this->first, m2.first, tol) &&
traits<R>::Equals(this->second, m2.second, tol);
return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
traits<R>::Equals(range_, m2.range_, tol);
}
/// @}
/// @name Manifold
/// @{
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;
/// Retract delta to manifold
BearingRange retract(const TangentVector& xi) const {
B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
return BearingRange(m1, m2);
}
/// Compute the coordinates in the tangent space
TangentVector localCoordinates(const BearingRange& other) const {
typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
TangentVector v;
v << v1, v2;
return v;
}
/// @}
/// @name Advanced Interface
/// @{
private:
/// Serialization function
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("bearing", this->first);
ar& boost::serialization::make_nvp("range", this->second);
ar& boost::serialization::make_nvp("bearing", bearing_);
ar& boost::serialization::make_nvp("range", range_);
}
friend class boost::serialization::access;
/// @}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum {
NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// Declare this to be both Testable and a Manifold

7
gtsam/geometry/CalibratedCamera.h
View File

@ -229,10 +229,6 @@ private:
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(pose_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholeBase
@ -416,9 +412,6 @@ private:
}
/// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
// manifold traits

62
gtsam/geometry/EssentialMatrix.h
View File

@ -11,7 +11,9 @@
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Manifold.h>
#include <iosfwd>
#include <string>
namespace gtsam {
@ -21,19 +23,13 @@ namespace gtsam {
* but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
*/
class GTSAM_EXPORT EssentialMatrix : private ProductManifold<Rot3, Unit3> {
class GTSAM_EXPORT EssentialMatrix {
private:
typedef ProductManifold<Rot3, Unit3> Base;
Rot3 R_; ///< Rotation
Unit3 t_; ///< Translation
Matrix3 E_; ///< Essential matrix
/// Construct from Base
EssentialMatrix(const Base& base) :
Base(base), E_(direction().skew() * rotation().matrix()) {
}
public:
/// Static function to convert Point2 to homogeneous coordinates
static Vector3 Homogeneous(const Point2& p) {
return Vector3(p.x(), p.y(), 1);
@ -43,13 +39,12 @@ public:
/// @{
/// Default constructor
EssentialMatrix() :
Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
EssentialMatrix() :E_(t_.skew()) {
}
/// Construct from rotation and translation
EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
R_(aRb), t_(aTb), E_(t_.skew() * R_.matrix()) {
}
/// Named constructor with derivatives
@ -79,27 +74,32 @@ public:
/// assert equality up to a tolerance
bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
return rotation().equals(other.rotation(), tol)
&& direction().equals(other.direction(), tol);
return R_.equals(other.R_, tol)
&& t_.equals(other.t_, tol);
}
/// @}
/// @name Manifold
/// @{
enum { dimension = 5 };
inline static size_t Dim() { return dimension;}
inline size_t dim() const { return dimension;}
using Base::dimension;
using Base::dim;
using Base::Dim;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
/// Retract delta to manifold
EssentialMatrix retract(const TangentVector& v) const {
return Base::retract(v);
EssentialMatrix retract(const Vector5& xi) const {
return EssentialMatrix(R_.retract(xi.head<3>()), t_.retract(xi.tail<2>()));
}
/// Compute the coordinates in the tangent space
TangentVector localCoordinates(const EssentialMatrix& other) const {
return Base::localCoordinates(other);
Vector5 localCoordinates(const EssentialMatrix& other) const {
auto v1 = R_.localCoordinates(other.R_);
auto v2 = t_.localCoordinates(other.t_);
Vector5 v;
v << v1, v2;
return v;
}
/// @}
@ -108,12 +108,12 @@ public:
/// Rotation
inline const Rot3& rotation() const {
return this->first;
return R_;
}
/// Direction
inline const Unit3& direction() const {
return this->second;
return t_;
}
/// Return 3*3 matrix representation
@ -123,12 +123,12 @@ public:
/// Return epipole in image_a , as Unit3 to allow for infinity
inline const Unit3& epipole_a() const {
return direction();
return t_;
}
/// Return epipole in image_b, as Unit3 to allow for infinity
inline Unit3 epipole_b() const {
return rotation().unrotate(direction());
return R_.unrotate(t_);
}
/**
@ -160,7 +160,7 @@ public:
}
/// epipolar error, algebraic
double error(const Vector3& vA, const Vector3& vB, //
double error(const Vector3& vA, const Vector3& vB,
OptionalJacobian<1, 5> H = boost::none) const;
/// @}
@ -177,7 +177,6 @@ public:
/// @}
private:
/// @name Advanced Interface
/// @{
@ -185,8 +184,8 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(first);
ar & BOOST_SERIALIZATION_NVP(second);
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
ar & boost::serialization::make_nvp("E11", E_(0, 0));
ar & boost::serialization::make_nvp("E12", E_(0, 1));
@ -200,6 +199,9 @@ private:
}
/// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
template<>
@ -208,5 +210,5 @@ struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
template<>
struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
} // gtsam
} // namespace gtsam

6
gtsam/geometry/Pose2.h
View File

@ -278,6 +278,12 @@ private:
ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(r_);
}
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
public:
// Make sure Pose2 is aligned if it contains a Vector2
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
}; // Pose2
/** specialization for pose2 wedge function (generic template in Lie.h) */

4
gtsam/geometry/Pose3.h
View File

@ -327,9 +327,11 @@ public:
}
/// @}
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
// Pose3 class

2
gtsam/geometry/Rot2.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
/// Named constructor from angle in degrees
static Rot2 fromDegrees(double theta) {
const double degree = M_PI / 180;
static const double degree = M_PI / 180;
return fromAngle(theta * degree);
}

4
gtsam/geometry/Rot3.h
View File

@ -508,9 +508,11 @@ namespace gtsam {
#endif
}
#ifdef GTSAM_USE_QUATERNIONS
// only align if quaternion, Matrix3 has no alignment requirements
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
/**

6
gtsam/inference/FactorGraph.h
View File

@ -26,6 +26,8 @@
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
#include <Eigen/Core> // for Eigen::aligned_allocator
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
@ -164,7 +166,7 @@ namespace gtsam {
template<class DERIVEDFACTOR, class... Args>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
emplace_shared(Args&&... args) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(std::forward<Args>(args)...));
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), std::forward<Args>(args)...));
}
/** push back many factors with an iterator over shared_ptr (factors are not copied) */
@ -194,7 +196,7 @@ namespace gtsam {
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor));
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
}
//#endif

6
gtsam/navigation/AttitudeFactor.h
View File

@ -129,6 +129,9 @@ private:
ar & BOOST_SERIALIZATION_NVP(nZ_);
ar & BOOST_SERIALIZATION_NVP(bRef_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits
@ -213,6 +216,9 @@ private:
ar & BOOST_SERIALIZATION_NVP(nZ_);
ar & BOOST_SERIALIZATION_NVP(bRef_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits

4
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -252,8 +252,8 @@ CombinedImuFactor::CombinedImuFactor(
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
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;

3
gtsam/navigation/CombinedImuFactor.h
View File

@ -320,6 +320,9 @@ private:
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
// class CombinedImuFactor

6
gtsam/navigation/PreintegratedRotation.h
View File

@ -59,8 +59,11 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
/**
@ -169,8 +172,11 @@ class GTSAM_EXPORT PreintegratedRotation {
ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
template <>

3
gtsam/navigation/PreintegrationBase.h
View File

@ -202,7 +202,8 @@ public:
/// @}
#endif
/// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} /// namespace gtsam

3
gtsam/navigation/PreintegrationParams.h
View File

@ -75,8 +75,11 @@ protected:
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
} // namespace gtsam

12
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -22,19 +22,26 @@
#include <CppUnitLite/TestHarness.h>
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp>
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
#if GEOGRAPHICLIB_VERSION_MINOR<37
static const auto& kWGS84 = Geocentric::WGS84;
#else
static const auto& kWGS84 = Geocentric::WGS84();
#endif
// *************************************************************************
namespace example {
// ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
// Convert from GPS to ENU
LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84);
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
// Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274;
@ -107,8 +114,7 @@ TEST(GPSData, init) {
// GPS Reading 1 will be ENU origin
double t1 = 84831;
Point3 NED1(0, 0, 0);
LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54,
Geocentric::WGS84);
LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
// GPS Readin 2
double t2 = 84831.5;

17
gtsam/navigation/tests/testGeographicLib.cpp
View File

@ -15,6 +15,7 @@
* @author Frank Dellaert
*/
#include <GeographicLib/Config.h>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/LocalCartesian.hpp>
@ -29,21 +30,27 @@ using namespace std;
using namespace GeographicLib;
// Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274;
static const double lat = 33.87071, lon = -84.30482, h = 274;
#if GEOGRAPHICLIB_VERSION_MINOR<37
static const auto& kWGS84 = Geocentric::WGS84;
#else
static const auto& kWGS84 = Geocentric::WGS84();
#endif
//**************************************************************************
TEST( GeographicLib, Geocentric) {
// From lat-lon to geocentric
double X, Y, Z;
Geocentric::WGS84.Forward(lat, lon, h, X, Y, Z);
kWGS84.Forward(lat, lon, h, X, Y, Z);
EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
// From geocentric to lat-lon
double lat_, lon_, h_;
Geocentric::WGS84.Reverse(X, Y, Z, lat_, lon_, h_);
kWGS84.Reverse(X, Y, Z, lat_, lon_, h_);
EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
@ -69,11 +76,9 @@ TEST( GeographicLib, UTM) {
//**************************************************************************
TEST( GeographicLib, ENU) {
const Geocentric& earth = Geocentric::WGS84;
// ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
LocalCartesian enu(lat0, lon0, h0, earth);
LocalCartesian enu(lat0, lon0, h0, kWGS84);
// From lat-lon to geocentric
double E, N, U;

4
gtsam/nonlinear/Expression-inl.h
View File

@ -200,7 +200,7 @@ T Expression<T>::valueAndJacobianMap(const Values& values,
// allocated on Visual Studio. For more information see the issue below
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
#ifdef _MSC_VER
internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size];
auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(_aligned_malloc(size, internal::TraceAlignment));
#else
internal::ExecutionTraceStorage traceStorage[size];
#endif
@ -210,7 +210,7 @@ T Expression<T>::valueAndJacobianMap(const Values& values,
trace.startReverseAD1(jacobians);
#ifdef _MSC_VER
delete[] traceStorage;
_aligned_free(traceStorage);
#endif
return value;

5
gtsam/nonlinear/ExpressionFactor.h
View File

@ -205,6 +205,11 @@ private:
BOOST_SERIALIZATION_SPLIT_MEMBER()
friend class boost::serialization::access;
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// ExpressionFactor

3
gtsam/nonlinear/ExpressionFactorGraph.h
View File

@ -42,7 +42,8 @@ public:
template<typename T>
void addExpressionFactor(const Expression<T>& h, const T& z,
const SharedNoiseModel& R) {
push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
using F = ExpressionFactor<T>;
push_back(boost::allocate_shared<F>(Eigen::aligned_allocator<F>(), R, z, h));
}
/// @}

5
gtsam/slam/BetweenFactor.h
View File

@ -122,6 +122,11 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; // \class BetweenFactor
/// traits

3
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -103,6 +103,9 @@ private:
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measuredE_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
// \class EssentialMatrixConstraint

5
gtsam/slam/PriorFactor.h
View File

@ -104,6 +104,11 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
} /// namespace gtsam

3
gtsam/slam/ProjectionFactor.h
View File

@ -186,6 +186,9 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits