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

Merge branch 'develop' into fix/smartfactor_body_P_sensor

release/4.3a0
Varun Agrawal 2020-07-09 10:47:49 -04:00
parent 0c199dd406 0f0752fd1f
commit b9264cf550
2 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/geometry/SOn.h
View File

@ -292,6 +292,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
boost::none) const; boost::none) const;
/// @} /// @}
template <class Archive>
friend void save(Archive&, SO&, const unsigned int);
template <class Archive>
friend void load(Archive&, SO&, const unsigned int);
template <class Archive> template <class Archive>
friend void serialize(Archive&, SO&, const unsigned int); friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access; friend class boost::serialization::access;
@ -329,6 +333,16 @@ template <>
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1, SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const; DynamicJacobian H2) const;
/** Serialization function */
template<class Archive>
void serialize(
Archive& ar, SOn& Q,
const unsigned int file_version
) {
Matrix& M = Q.matrix_;
ar& M;
}
/* /*
* Define the traits. internal::LieGroup provides both Lie group and Testable * Define the traits. internal::LieGroup provides both Lie group and Testable
*/ */

4
gtsam/slam/FrobeniusFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
* isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an * isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input. * error. If defaultToUnit == false throws an exception on unexepcted input.
*/ */
boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel( GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit = true); const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
/** /**
@ -125,7 +125,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
* the SO(p) matrices down to a Stiefel manifold of p*d matrices. * the SO(p) matrices down to a Stiefel manifold of p*d matrices.
* TODO(frank): template on D=2 or 3 * TODO(frank): template on D=2 or 3
*/ */
class FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> { class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2 Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_, dimension_; ///< dimensionality constants size_t p_, pp_, dimension_; ///< dimensionality constants
Matrix G_; ///< matrix of vectorized generators Matrix G_; ///< matrix of vectorized generators