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);
}
template <class T, class ALLOC>
T FindKarcherMean(const vector<T, ALLOC>& rotations) {
template <class T,
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);
}

4
gtsam/slam/KarcherMeanFactor.h
View File

@ -29,8 +29,8 @@ namespace gtsam {
* the given rotations, by constructing a factor graph out of simple
* PriorFactors.
*/
template <class T, class ALLOC = Eigen::aligned_allocator<T>>
T FindKarcherMean(const std::vector<T, ALLOC>& rotations);
template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations);
template <class T>
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) {
std::vector<SO4> rotations = {Q, Q.inverse()};
std::vector<SO4, Eigen::aligned_allocator<SO4>> rotations = {Q, Q.inverse()};
auto expected = SO4(); //::ChordalMean(rotations);
auto actual = FindKarcherMean(rotations);
EXPECT(assert_equal(expected, actual));