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

Removed unnecessary templated traits from partial pose priors - now uses rotationInterval and translationInterval

release/4.3a0
Alex Cunningham 2013-04-24 17:34:48 +00:00
parent 124a38f72d
commit 1c17065376
2 changed files with 8 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam_unstable/slam/PoseRotationPrior.h
View File

@ -9,7 +9,10 @@
#pragma once
#include <gtsam_unstable/slam/PoseTranslationPrior.h>
//#include <gtsam_unstable/slam/PoseTranslationPrior.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -66,10 +69,8 @@ public:
const size_t rDim = newR.dim(), xDim = pose.dim();
if (H) {
*H = gtsam::zeros(rDim, xDim);
if (pose_traits::isRotFirst<Pose>())
(*H).leftCols(rDim).setIdentity(rDim, rDim);
else
(*H).rightCols(rDim).setIdentity(rDim, rDim);
std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
(*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
}
return Rotation::Logmap(newR) - Rotation::Logmap(measured_);

22
gtsam_unstable/slam/PoseTranslationPrior.h
View File

@ -14,22 +14,6 @@
namespace gtsam {
/** simple pose traits for building factors */
namespace pose_traits {
/** checks whether parameterization of rotation is before or after translation in Lie algebra */
template<class POSE>
bool isRotFirst() {
throw std::invalid_argument("PoseTrait: no implementation for this pose type");
return false;
}
// Instantiate for common poses
template<> inline bool isRotFirst<Pose3>() { return true; }
template<> inline bool isRotFirst<Pose2>() { return false; }
} // \namespace pose_traits
/**
* A prior on the translation part of a pose
*/
@ -78,10 +62,8 @@ public:
const size_t tDim = newTrans.dim(), xDim = pose.dim();
if (H) {
*H = gtsam::zeros(tDim, xDim);
if (pose_traits::isRotFirst<Pose>())
(*H).rightCols(tDim) = R.matrix();
else
(*H).leftCols(tDim) = R.matrix();
std::pair<size_t, size_t> transInterval = POSE::translationInterval();
(*H).middleCols(transInterval.first, tDim) = R.matrix();
}
return newTrans.vector() - measured_.vector();