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

Merge remote-tracking branch 'origin/develop' into feature/frobeniusfactor

release/4.3a0
Fan Jiang 2020-06-27 01:26:53 -04:00
parent 1c9f429d9c 8ebd7f7f54
commit 8739c372fb
40 changed files with 550 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam.h
View File

@ -281,7 +281,7 @@ virtual class Value {
};
#include <gtsam/base/GenericValue.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
@ -2984,6 +2984,7 @@ class PreintegratedImuMeasurements {
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
@ -3045,6 +3046,7 @@ class PreintegratedCombinedMeasurements {
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;

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

@ -17,7 +17,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
endforeach(eigen_dir)
if(GTSAM_WITH_EIGEN_UNSUPPORTED)
message("-- Installing Eigen Unsupported modules")
message(STATUS "Installing Eigen Unsupported modules")
# do the same for the unsupported eigen folder
file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")

2
gtsam/base/GenericValue.h
View File

@ -181,7 +181,7 @@ public:
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
/// use this macro instead of BOOST_CLASS_EXPORT for GenericValues

2
gtsam/base/Manifold.h
View File

@ -214,7 +214,7 @@ public:
enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// Define any direct product group to be a model of the multiplicative Group concept

67
gtsam/base/make_shared.h Normal file
View File

@ -0,0 +1,67 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file make_shared.h
* @brief make_shared trampoline function to ensure proper alignment
* @author Fan Jiang
*/
#pragma once
#include <gtsam/base/types.h>
#include <Eigen/Core>
#include <boost/make_shared.hpp>
#include <type_traits>
namespace gtsam {
/// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
template<bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;
}
namespace gtsam {
/**
* Add our own `make_shared` as a layer of wrapping on `boost::make_shared`
* This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs
* at runtime, which is notoriously hard to debug.
*
* Explanation
* ===============
* The template `needs_eigen_aligned_allocator<T>::value` will evaluate to `std::true_type` if the type alias
* `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the
* `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro.
*
* This function declaration will only be taken when the above condition is true, so if some object does not need to
* be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for
* `boost::make_shared`.
*
* @tparam T The type of object being constructed
* @tparam Args Type of the arguments of the constructor
* @param args Arguments of the constructor
* @return The object created as a boost::shared_ptr<T>
*/
template<typename T, typename ... Args>
gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
}
/// Fall back to the boost version if no need for alignment
template<typename T, typename ... Args>
gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
return boost::make_shared<T>(std::forward<Args>(args)...);
}
}

249
gtsam/base/serialization.h
View File

@ -42,123 +42,218 @@
namespace gtsam {
// Serialization directly to strings in compressed format
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
/** @name Standard serialization
* Serialization in default compressed format
*/
///@{
/// serializes to a stream
template <class T>
void serializeToStream(const T& input, std::ostream& out_archive_stream) {
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
/// deserializes from a stream
template <class T>
void deserializeFromStream(std::istream& in_archive_stream, T& output) {
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
}
template<class T>
/// serializes to a string
template <class T>
std::string serializeToString(const T& input) {
std::ostringstream out_archive_stream;
serializeToStream(input, out_archive_stream);
return out_archive_stream.str();
}
/// deserializes from a string
template <class T>
void deserializeFromString(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
deserializeFromStream(in_archive_stream, output);
}
/// serializes to a file
template <class T>
bool serializeToFile(const T& input, const std::string& filename) {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
if (!out_archive_stream.is_open()) return false;
serializeToStream(input, out_archive_stream);
out_archive_stream.close();
return true;
}
template<class T>
/// deserializes from a file
template <class T>
bool deserializeFromFile(const std::string& filename, T& output) {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
if (!in_archive_stream.is_open()) return false;
deserializeFromStream(in_archive_stream, output);
in_archive_stream.close();
return true;
}
// Serialization to XML format with named structures
template<class T>
std::string serializeXML(const T& input, const std::string& name="data") {
std::ostringstream out_archive_stream;
// braces to flush out_archive as it goes out of scope before taking str()
// fixes crash with boost 1.66-1.68
// see https://github.com/boostorg/serialization/issues/82
{
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
return out_archive_stream.str();
/// serializes to a string
template <class T>
std::string serialize(const T& input) {
return serializeToString(input);
}
template<class T>
void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") {
std::istringstream in_archive_stream(serialized);
/// deserializes from a string
template <class T>
void deserialize(const std::string& serialized, T& output) {
deserializeFromString(serialized, output);
}
///@}
/** @name XML Serialization
* Serialization to XML format with named structures
*/
///@{
/// serializes to a stream in XML
template <class T>
void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
const std::string& name = "data") {
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
/// deserializes from a stream in XML
template <class T>
void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
const std::string& name = "data") {
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
}
template<class T>
bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);;
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
in_archive_stream.close();
return true;
}
// Serialization to binary format with named structures
template<class T>
std::string serializeBinary(const T& input, const std::string& name="data") {
/// serializes to a string in XML
template <class T>
std::string serializeToXMLString(const T& input,
const std::string& name = "data") {
std::ostringstream out_archive_stream;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
serializeToXMLStream(input, out_archive_stream, name);
return out_archive_stream.str();
}
template<class T>
void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") {
/// deserializes from a string in XML
template <class T>
void deserializeFromXMLString(const std::string& serialized, T& output,
const std::string& name = "data") {
std::istringstream in_archive_stream(serialized);
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
deserializeFromXMLStream(in_archive_stream, output, name);
}
template<class T>
bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
/// serializes to an XML file
template <class T>
bool serializeToXMLFile(const T& input, const std::string& filename,
const std::string& name = "data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
if (!out_archive_stream.is_open()) return false;
serializeToXMLStream(input, out_archive_stream, name);
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") {
/// deserializes from an XML file
template <class T>
bool deserializeFromXMLFile(const std::string& filename, T& output,
const std::string& name = "data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
if (!in_archive_stream.is_open()) return false;
deserializeFromXMLStream(in_archive_stream, output, name);
in_archive_stream.close();
return true;
}
} // \namespace gtsam
/// serializes to a string in XML
template <class T>
std::string serializeXML(const T& input,
const std::string& name = "data") {
return serializeToXMLString(input, name);
}
/// deserializes from a string in XML
template <class T>
void deserializeXML(const std::string& serialized, T& output,
const std::string& name = "data") {
deserializeFromXMLString(serialized, output, name);
}
///@}
/** @name Binary Serialization
* Serialization to binary format with named structures
*/
///@{
/// serializes to a stream in binary
template <class T>
void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
const std::string& name = "data") {
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
/// deserializes from a stream in binary
template <class T>
void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
const std::string& name = "data") {
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
}
/// serializes to a string in binary
template <class T>
std::string serializeToBinaryString(const T& input,
const std::string& name = "data") {
std::ostringstream out_archive_stream;
serializeToBinaryStream(input, out_archive_stream, name);
return out_archive_stream.str();
}
/// deserializes from a string in binary
template <class T>
void deserializeFromBinaryString(const std::string& serialized, T& output,
const std::string& name = "data") {
std::istringstream in_archive_stream(serialized);
deserializeFromBinaryStream(in_archive_stream, output, name);
}
/// serializes to a binary file
template <class T>
bool serializeToBinaryFile(const T& input, const std::string& filename,
const std::string& name = "data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open()) return false;
serializeToBinaryStream(input, out_archive_stream, name);
out_archive_stream.close();
return true;
}
/// deserializes from a binary file
template <class T>
bool deserializeFromBinaryFile(const std::string& filename, T& output,
const std::string& name = "data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open()) return false;
deserializeFromBinaryStream(in_archive_stream, output, name);
in_archive_stream.close();
return true;
}
/// serializes to a string in binary
template <class T>
std::string serializeBinary(const T& input,
const std::string& name = "data") {
return serializeToBinaryString(input, name);
}
/// deserializes from a string in binary
template <class T>
void deserializeBinary(const std::string& serialized, T& output,
const std::string& name = "data") {
deserializeFromBinaryString(serialized, output, name);
}
///@}
} // namespace gtsam

58
gtsam/base/serializationTestHelpers.h
View File

@ -26,6 +26,7 @@
#include <gtsam/base/serialization.h>
#include <boost/serialization/serialization.hpp>
#include <boost/filesystem.hpp>
// whether to print the serialized text to stdout
@ -40,22 +41,37 @@ T create() {
return T();
}
// Creates or empties a folder in the build folder and returns the relative path
boost::filesystem::path resetFilesystem(
boost::filesystem::path folder = "actual") {
boost::filesystem::remove_all(folder);
boost::filesystem::create_directory(folder);
return folder;
}
// Templated round-trip serialization
template<class T>
void roundtrip(const T& input, T& output) {
// Serialize
std::string serialized = serialize(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
deserialize(serialized, output);
}
// This version requires equality operator
// Templated round-trip serialization using a file
template<class T>
void roundtripFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.dat";
serializeToFile(input, path.string());
deserializeFromFile(path.string(), output);
}
// This version requires equality operator and uses string and file round-trips
template<class T>
bool equality(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtrip<T>(input,output);
return input==output;
roundtripFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable
@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) {
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeXML(serialized, output);
}
// Templated round-trip serialization using XML File
template<class T>
void roundtripXMLFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.xml";
serializeToXMLFile(input, path.string());
deserializeFromXMLFile(path.string(), output);
}
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtripXML<T>(input,output);
return input==output;
roundtripXMLFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable
@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) {
// Templated round-trip serialization using XML
template<class T>
void roundtripBinary(const T& input, T& output) {
// Serialize
std::string serialized = serializeBinary<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeBinary(serialized, output);
}
// Templated round-trip serialization using Binary file
template<class T>
void roundtripBinaryFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.bin";
serializeToBinaryFile(input, path.string());
deserializeFromBinaryFile(path.string(), output);
}
// This version requires equality operator
template<class T>
bool equalityBinary(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtripBinary<T>(input,output);
return input==output;
roundtripBinaryFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable

47
gtsam/base/types.h
View File

@ -230,3 +230,50 @@ namespace std {
#ifdef ERROR
#undef ERROR
#endif
namespace gtsam {
/// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::`
template<typename ...> using void_t = void;
/**
* A SFINAE trait to mark classes that need special alignment.
*
* This is required to make boost::make_shared and etc respect alignment, which is essential for the Python
* wrappers to work properly.
*
* Explanation
* =============
* When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template
* will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`.
*
* Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`.
*
* Please refer to `gtsam/base/make_shared.h` for an example.
*/
template<typename, typename = void_t<>>
struct needs_eigen_aligned_allocator : std::false_type {
};
template<typename T>
struct needs_eigen_aligned_allocator<T, void_t<typename T::_eigen_aligned_allocator_trait>> : std::true_type {
};
}
/**
* This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
* memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
* See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
*/
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
using _eigen_aligned_allocator_trait = void;
/**
* This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
* memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
* See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
*/
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
using _eigen_aligned_allocator_trait = void;

2
gtsam/geometry/BearingRange.h
View File

@ -162,7 +162,7 @@ private:
NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// Declare this to be both Testable and a Manifold

2
gtsam/geometry/CameraSet.h
View File

@ -319,7 +319,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<class CAMERA>

2
gtsam/geometry/EssentialMatrix.h
View File

@ -212,7 +212,7 @@ class EssentialMatrix {
/// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<>

2
gtsam/geometry/PinholeCamera.h
View File

@ -325,7 +325,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// manifold traits

4
gtsam/geometry/PinholePose.h
View File

@ -222,7 +222,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholeBaseK
@ -425,7 +425,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholePose

2
gtsam/geometry/Pose2.h
View File

@ -317,7 +317,7 @@ public:
public:
// Align for Point2, which is either derived from, or is typedef, of Vector2
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; // Pose2
/** specialization for pose2 wedge function (generic template in Lie.h) */

2
gtsam/geometry/Pose3.h
View File

@ -355,7 +355,7 @@ public:
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
// Pose3 class

2
gtsam/geometry/Rot3.h
View File

@ -544,7 +544,7 @@ namespace gtsam {
#ifdef GTSAM_USE_QUATERNIONS
// only align if quaternion, Matrix3 has no alignment requirements
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

4
gtsam/geometry/SOn.h
View File

@ -20,8 +20,8 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/make_shared.h>
#include <gtsam/dllexport.h>
#include <Eigen/Core>
#include <iostream> // TODO(frank): how to avoid?
@ -54,7 +54,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
protected:
MatrixNN matrix_; ///< Rotation matrix

2
gtsam/geometry/Unit3.h
View File

@ -214,7 +214,7 @@ private:
/// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// Define GTSAM traits

2
gtsam/geometry/triangulation.h
View File

@ -215,7 +215,7 @@ struct CameraProjectionMatrix {
private:
const Matrix3 K_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/**

4
gtsam/navigation/AttitudeFactor.h
View File

@ -139,7 +139,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits
@ -219,7 +219,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits

6
gtsam/navigation/CombinedImuFactor.h
View File

@ -100,7 +100,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
@ -210,7 +210,7 @@ public:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/**
@ -332,7 +332,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// class CombinedImuFactor

2
gtsam/navigation/ImuBias.h
View File

@ -171,7 +171,7 @@ private:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
/// @}
}; // ConstantBias class

4
gtsam/navigation/PreintegratedRotation.h
View File

@ -69,7 +69,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
@ -182,7 +182,7 @@ class GTSAM_EXPORT PreintegratedRotation {
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

2
gtsam/navigation/PreintegrationBase.h
View File

@ -214,7 +214,7 @@ class GTSAM_EXPORT PreintegrationBase {
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
} /// namespace gtsam

2
gtsam/navigation/PreintegrationParams.h
View File

@ -84,7 +84,7 @@ protected:
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

2
gtsam/navigation/TangentPreintegration.h
View File

@ -141,7 +141,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
} /// namespace gtsam

2
gtsam/nonlinear/ExpressionFactor.h
View File

@ -209,7 +209,7 @@ private:
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// ExpressionFactor

6
gtsam/nonlinear/NonlinearEquality.h
View File

@ -175,7 +175,7 @@ public:
/// @}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:
@ -265,7 +265,7 @@ public:
traits<X>::Print(value_, "Value");
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:
@ -331,7 +331,7 @@ public:
return traits<X>::Local(x1,x2);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:

2
gtsam/nonlinear/PriorFactor.h
View File

@ -114,7 +114,7 @@ namespace gtsam {
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
} /// namespace gtsam

2
gtsam/nonlinear/internal/ExpressionNode.h
View File

@ -150,7 +150,7 @@ public:
return constant_;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
//-----------------------------------------------------------------------------

2
gtsam/slam/BetweenFactor.h
View File

@ -126,7 +126,7 @@ namespace gtsam {
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; // \class BetweenFactor
/// traits

2
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -105,7 +105,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// \class EssentialMatrixConstraint

6
gtsam/slam/EssentialMatrixFactor.h
View File

@ -81,7 +81,7 @@ public:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/**
@ -201,7 +201,7 @@ public:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor2
@ -286,7 +286,7 @@ public:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor3

2
gtsam/slam/ProjectionFactor.h
View File

@ -189,7 +189,7 @@ namespace gtsam {
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits

2
gtsam/slam/RotateFactor.h
View File

@ -113,7 +113,7 @@ public:
return error;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace gtsam

2
gtsam/slam/SmartFactorBase.h
View File

@ -81,7 +81,7 @@ protected:
mutable FBlocks Fs;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;

90
gtsam_unstable/slam/PoseToPointFactor.h Normal file
View File

@ -0,0 +1,90 @@
/**
* @file PoseToPointFactor.hpp
* @brief This factor can be used to track a 3D landmark over time by
*providing local measurements of its location.
* @author David Wisth
**/
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <ostream>
namespace gtsam {
/**
* A class for a measurement between a pose and a point.
* @addtogroup SLAM
*/
class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
private:
typedef PoseToPointFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base;
Point3 measured_; /** the point measurement in local coordinates */
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
/** default constructor - only use for serialization */
PoseToPointFactor() {}
/** Constructor */
PoseToPointFactor(Key key1, Key key2, const Point3& measured,
const SharedNoiseModel& model)
: Base(model, key1, key2), measured_(measured) {}
virtual ~PoseToPointFactor() {}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors
* @brief Error = wTwi.inverse()*wPwp - measured_
* @param wTwi The pose of the sensor in world coordinates
* @param wPwp The estimated point location in world coordinates
*
* Note: measured_ and the error are in local coordiantes.
*/
Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return wTwi.transformTo(wPwp, H1, H2) - measured_;
}
/** return the measured */
const Point3& measured() const { return measured_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class PoseToPointFactor
} // namespace gtsam

86
gtsam_unstable/slam/tests/testPoseToPointFactor.h Normal file
View File

@ -0,0 +1,86 @@
/**
* @file testPoseToPointFactor.cpp
* @brief
* @author David Wisth
* @date June 20, 2020
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/slam/PoseToPointFactor.h>
using namespace gtsam;
using namespace gtsam::noiseModel;
/// Verify zero error when there is no noise
TEST(PoseToPointFactor, errorNoiseless) {
Pose3 pose = Pose3::identity();
Point3 point(1.0, 2.0, 3.0);
Point3 noise(0.0, 0.0, 0.0);
Point3 measured = t + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor factor(pose_key, point_key, measured,
Isotropic::Sigma(3, 0.05));
Vector expectedError = Vector3(0.0, 0.0, 0.0);
Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/// Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise) {
Pose3 pose = Pose3::identity();
Point3 point(1.0, 2.0, 3.0);
Point3 noise(-1.0, 0.5, 0.3);
Point3 measured = t + noise;
Key pose_key(1);
Key point_key(2);
PoseToPointFactor factor(pose_key, point_key, measured,
Isotropic::Sigma(3, 0.05));
Vector expectedError = noise;
Vector actualError = factor.evaluateError(pose, point);
EXPECT(assert_equal(expectedError, actualError, 1E-5));
}
/// Check Jacobians are correct
TEST(PoseToPointFactor, jacobian) {
// Measurement
gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
// Linearisation point
gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
Pose3 p(p_R, p_t);
gtsam::Point3 l = gtsam::Point3(3, 0, 5);
// Factor
Key pose_key(1);
Key point_key(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives
auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
boost::none, boost::none);
Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
// Use the factor to calculate the derivative
Matrix actual_H1;
Matrix actual_H2;
factor.evaluateError(p, l, actual_H1, actual_H2);
// Verify we get the expected error
EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8));
EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

5
wrap/Module.cpp
View File

@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
" T* get()\n"
" long use_count() const\n"
" T& operator*()\n\n"
" cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n"
" cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n";
// gtsam alignment-friendly shared_ptr
pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n"
" cdef shared_ptr[T] make_shared[T](const T& r)\n\n";
for(const TypedefPair& types: typedefs)

2
wrap/tests/expected-cython/geometry.pxd
View File

@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost":
T& operator*()
cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)
cdef extern from "gtsam/base/make_shared.h" namespace "gtsam":
cdef shared_ptr[T] make_shared[T](const T& r)
cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam":