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

Wrapped Pose[Translation|Rotation]Factor

release/4.3a0
Alex Cunningham 2012-08-02 19:57:10 +00:00
parent 93d9023a61
commit 4c836c6e3a
3 changed files with 55 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam_unstable/gtsam_unstable.h
View File

@ -6,6 +6,7 @@
virtual class gtsam::Value;
virtual class gtsam::Point3;
virtual class gtsam::Rot3;
virtual class gtsam::Pose2;
virtual class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::NonlinearFactor;
@ -165,5 +166,21 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
};
//*************************************************************************
// General nonlinear factor types
//*************************************************************************
#include <gtsam/geometry/Pose2.h>
}///\namespace gtsam
#include <gtsam_unstable/slam/PoseTranslationPrior.h>
template<POSE = {gtsam::Pose2,gtsam::Pose3}>
virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam_unstable/slam/PoseRotationPrior.h>
template<POSE = {gtsam::Pose2,gtsam::Pose3}>
virtual class PoseRotationPrior : gtsam::NonlinearFactor {
PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
};
} //\namespace gtsam

23
gtsam_unstable/slam/PoseRotationPrior.h
View File

@ -29,9 +29,23 @@ public:
/** standard constructor */
PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
: Base(key, model)
{
assert(rot_z.dim() == model->dim());
: Base(key, model) {
initialize(rot_z);
}
/** Constructor that pulls the translation from an incoming POSE */
PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
: Base(key, model) {
initialize(pose_z.rotation());
}
/** get the rotation used to create the measurement */
Rotation priorRotation() const { return Rotation::Expmap(this->prior_); }
protected:
/** loads the underlying partial prior factor */
void initialize(const Rotation& rot_z) {
assert(rot_z.dim() == this->noiseModel_->dim());
// Calculate the prior applied
this->prior_ = Rotation::Logmap(rot_z);
@ -52,9 +66,6 @@ public:
this->fillH();
}
/** get the rotation used to create the measurement */
Rotation priorRotation() const { return Rotation::Expmap(this->prior_); }
};
} // \namespace gtsam

29
gtsam_unstable/slam/PoseTranslationPrior.h
View File

@ -28,17 +28,31 @@ public:
GTSAM_CONCEPT_LIE_TYPE(Translation)
/** standard constructor */
PoseTranslationPrior(Key key, const Translation& rot_z, const SharedNoiseModel& model)
: Base(key, model)
{
assert(rot_z.dim() == model->dim());
PoseTranslationPrior(Key key, const Translation& trans_z, const SharedNoiseModel& model)
: Base(key, model) {
initialize(trans_z);
}
/** Constructor that pulls the translation from an incoming POSE */
PoseTranslationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
: Base(key, model) {
initialize(pose_z.translation());
}
/** get the rotation used to create the measurement */
Translation priorTranslation() const { return Translation::Expmap(this->prior_); }
protected:
/** loads the underlying partial prior factor */
void initialize(const Translation& trans_z) {
assert(trans_z.dim() == this->noiseModel_->dim());
// Calculate the prior applied
this->prior_ = Translation::Logmap(rot_z);
this->prior_ = Translation::Logmap(trans_z);
// Create the mask for partial prior
size_t pose_dim = Pose::identity().dim();
size_t rot_dim = rot_z.dim();
size_t rot_dim = trans_z.dim();
// get the interval of the lie coordinates corresponding to rotation
std::pair<size_t, size_t> interval = Pose::translationInterval();
@ -52,9 +66,6 @@ public:
this->fillH();
}
/** get the rotation used to create the measurement */
Translation priorTranslation() const { return Translation::Expmap(this->prior_); }
};
} // \namespace gtsam