Merge pull request #1729 from niwhsa9/amg/sim3
commit
eeb092ec41
|
@ -202,6 +202,18 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
|
|||
/// @{
|
||||
|
||||
private:
|
||||
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(R_);
|
||||
ar & BOOST_SERIALIZATION_NVP(t_);
|
||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
}
|
||||
#endif
|
||||
|
||||
/// Calculate expmap and logmap coefficients.
|
||||
static Matrix3 GetV(Vector3 w, double lambda);
|
||||
|
||||
|
|
|
@ -7,12 +7,13 @@ namespace gtsam {
|
|||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Similarity3.h>
|
||||
|
||||
// ######
|
||||
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
template <T = {double, Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
|
||||
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3,
|
||||
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity3,
|
||||
gtsam::imuBias::ConstantBias}>
|
||||
virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
||||
BetweenFactor(size_t key1, size_t key2, const T& relativePose,
|
||||
|
|
Loading…
Reference in New Issue