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

Fix KarcherMeanFactor

release/4.3a0
Fan Jiang 2019-12-10 12:19:38 -05:00
parent 6d0705c5dc
commit b156a6498e
3 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,8 +26,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
template <class T> template <class T, class ALLOC>
T FindKarcherMean(const vector<T>& rotations) { T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
// Cost function C(R) = \sum PriorFactor(R_i)::error(R) // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
// No closed form solution. // No closed form solution.
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -41,6 +41,16 @@ T FindKarcherMean(const vector<T>& rotations) {
return result.at<T>(kKey); return result.at<T>(kKey);
} }
template <class T, class ALLOC>
T FindKarcherMean(const vector<T, ALLOC>& rotations) {
return FindKarcherMeanImpl(rotations);
}
template <class T>
T FindKarcherMean(std::initializer_list<T>&& rotations) {
return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
}
template <class T> template <class T>
template <typename CONTAINER> template <typename CONTAINER>
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d) KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)

5
gtsam/slam/KarcherMeanFactor.h
View File

@ -29,8 +29,11 @@ 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>>
T FindKarcherMean(const std::vector<T, ALLOC>& rotations);
template <class T> template <class T>
T FindKarcherMean(const std::vector<T>& rotations); T FindKarcherMean(std::initializer_list<T>&& rotations);
/** /**
* The KarcherMeanFactor creates a constraint on all SO(n) variables with * The KarcherMeanFactor creates a constraint on all SO(n) variables with

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

@ -91,7 +91,9 @@ TEST(KarcherMean, FactorSO4) {
Values initial; Values initial;
initial.insert<SO4>(1, Q.inverse()); initial.insert<SO4>(1, Q.inverse());
initial.insert<SO4>(2, Q); initial.insert<SO4>(2, Q);
const auto expected = FindKarcherMean<SO4>({Q, Q.inverse()});
std::vector<SO4, Eigen::aligned_allocator<SO4> > rotations = {Q, Q.inverse()};
const auto expected = FindKarcherMean<SO4>(rotations);
auto result = GaussNewtonOptimizer(graph, initial).optimize(); auto result = GaussNewtonOptimizer(graph, initial).optimize();
const auto actual = const auto actual =