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

Additional Fixes for Rot3

release/4.3a0
Fan Jiang 2019-12-10 15:10:32 -05:00
parent b156a6498e
commit 4d256eae0f
3 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -41,8 +41,14 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
return result.at<T>(kKey); return result.at<T>(kKey);
} }
template <class T, class ALLOC> template <class T,
T FindKarcherMean(const vector<T, ALLOC>& rotations) { typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
T FindKarcherMean(const std::vector<T>& rotations) {
return FindKarcherMeanImpl(rotations);
}
template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
return FindKarcherMeanImpl(rotations); return FindKarcherMeanImpl(rotations);
} }

4
gtsam/slam/KarcherMeanFactor.h
View File

@ -29,8 +29,8 @@ namespace gtsam {
* the given rotations, by constructing a factor graph out of simple * the given rotations, by constructing a factor graph out of simple
* PriorFactors. * PriorFactors.
*/ */
template <class T, class ALLOC = Eigen::aligned_allocator<T>> template <class T>
T FindKarcherMean(const std::vector<T, ALLOC>& rotations); T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations);
template <class T> template <class T>
T FindKarcherMean(std::initializer_list<T>&& rotations); T FindKarcherMean(std::initializer_list<T>&& rotations);

2
gtsam/slam/tests/testKarcherMeanFactor.cpp
View File

@ -75,7 +75,7 @@ static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished());
/* ************************************************************************* */ /* ************************************************************************* */
TEST(KarcherMean, FindSO4) { TEST(KarcherMean, FindSO4) {
std::vector<SO4> rotations = {Q, Q.inverse()}; std::vector<SO4, Eigen::aligned_allocator<SO4>> rotations = {Q, Q.inverse()};
auto expected = SO4(); //::ChordalMean(rotations); auto expected = SO4(); //::ChordalMean(rotations);
auto actual = FindKarcherMean(rotations); auto actual = FindKarcherMean(rotations);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));