diff --git a/gtsam_unstable/slam/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h index 7725121fe..acd2cb043 100644 --- a/gtsam_unstable/slam/PoseRotationPrior.h +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -9,7 +9,10 @@ #pragma once -#include +//#include +#include +#include + 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()) - (*H).leftCols(rDim).setIdentity(rDim, rDim); - else - (*H).rightCols(rDim).setIdentity(rDim, rDim); + std::pair rotInterval = POSE::rotationInterval(); + (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); } return Rotation::Logmap(newR) - Rotation::Logmap(measured_); diff --git a/gtsam_unstable/slam/PoseTranslationPrior.h b/gtsam_unstable/slam/PoseTranslationPrior.h index be558346c..ada85667f 100644 --- a/gtsam_unstable/slam/PoseTranslationPrior.h +++ b/gtsam_unstable/slam/PoseTranslationPrior.h @@ -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 -bool isRotFirst() { - throw std::invalid_argument("PoseTrait: no implementation for this pose type"); - return false; -} - -// Instantiate for common poses -template<> inline bool isRotFirst() { return true; } -template<> inline bool isRotFirst() { 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()) - (*H).rightCols(tDim) = R.matrix(); - else - (*H).leftCols(tDim) = R.matrix(); + std::pair transInterval = POSE::translationInterval(); + (*H).middleCols(transInterval.first, tDim) = R.matrix(); } return newTrans.vector() - measured_.vector();