-
Notifications
You must be signed in to change notification settings - Fork 249
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add Bundle and BundleTangent types #175
Conversation
2166ec0
to
d8c0ca3
Compare
Hi @pettni and thanks for this PR! It is quite some work, there is a lot to review and go through. First of all, I essentially agree with your first 3 points. I know it can be painful to write this kind of code in C++11 but I believe that this low requirement is also one of the nice feature of Altho it is not reflected in #84, we decided a while back to name this class Would you however add some more composite types to the test suites using the test macros? Make the types diverse just to make sure that virtually any combination is doable (e.g. Regarding the MSVC failure, the first error it complains about is Error C2947 around this line. I believe it can be resolved with something along the lines, template<size_t Idx>
using PartType = typename std::tuple_element<
Idx, std::tuple<typename _T<_Scalar>::template Tangent ...>
>::type; ^^^ |
Hi Petter, thanks for this very interesting input! Responding to Jeremie-s question, regarding the name bundle vs. composite, there are pros and cons:
After these two considerations, I really have no preference for one or the other. In my talks and other discussions about this topic, I use "composite" and not "bundle", but these talks are about math and not about code. |
Interesting. What is the key in this map to access the composite's components? |
If I'm not mistaken, given |
Yes this is exactly how it works (except that it is zero-indexed); I was referring to an I have not been successful in fixing the issue on MSVC. I think that it can be solved by iterating various forms of the problematic template, but as I don't have access to the compiler it is tedious to debug. After adding the additional tests the CI job with Ceres also fails, seemingly due to the compiler running out of memory. |
Right, I fixed my comment.
I don't have access to a machine with MSVC at the moment either. Altho I might try and give it a shot against the CI to help out.
The CI is actually running out of time rather than memory. It is hitting a default limit on Travis so that if the compilation does not produce any output for more than 10min, it aborts. I had similar a issue with |
Ah OK I see. Thanks for the clarification. BTW a question comes to my mind: is it possible with this implementation to know which type of Lie group a certain block is? For example, to know which type is |
where only the block How to represent the Jacobian See eq. 89 of the paper. |
Some assistance with those issues would be great!
The parts are known at compile time so it's fine to write e.g.
Ah yes that seems correct. What I had in mind was that (I believe) all the Jacobians that are provided by the library become block-diagonal, since the associated mappings (log, exp, inverse, act, compose, etc) are perfectly decomposed, something like
However, I guess a user would multiply those block-diagonal jacobians with other derivatives that are not block-diagonal to obtain jacobians for more general mappings such as the one you proposed, so it seems like the best representation is as a full matrix. I admit I haven't used the jacobian functionality in To facilitate working with full-size jacobians it's an option to expose the |
1e6fed3
to
ac91687
Compare
Codecov Report
@@ Coverage Diff @@
## devel #175 +/- ##
==========================================
+ Coverage 98.40% 98.57% +0.16%
==========================================
Files 51 57 +6
Lines 1506 1679 +173
==========================================
+ Hits 1482 1655 +173
Misses 24 24 |
d96244f
to
650398e
Compare
Was able to make MSVC happy after some experimentation. Some changes I did outside the bundle type:
Let me know if there are further things you'd like to see to accept the PR. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Here is a first round of review, please add a reference to the Bundle
object in the readme as well.
I'll give a more thorough look after the festivities.
include/manif/impl/bundle/Bundle.h
Outdated
// | ||
|
||
/** | ||
* @brief Represents a Bundle element. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Can you add a little more documentation, especially references to Chap. IV and Example 7 of the paper.
/** | ||
* @brief Get the inverse of this. | ||
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. | ||
* @note r^-1 = -r |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Remove this note.
/** | ||
* @brief intseq: std::integer_sequence-equivalent | ||
*/ | ||
template<int ... _I> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please move all of the meta functions and helpers to the traits.h
. Moreover, move all of it under the manif::internal
namespace and remove the *::bundle
namespace.
* @tparam _Idx part index | ||
*/ | ||
template<int _Idx> | ||
Eigen::Map<PartType<_Idx>> get(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The function get
is too generic, we need a better name - e.g. group/tangent
where appropriate or bundled
. @joansola Any suggestion?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
In the paper we speak of block
s, but a function block()
does not appear too explicit for me either.
I suppose from BundleTangent::get<Idx>()
we obtain the Idx
-th tangent element?
And similarly that from Bundle::get<Idx>()
we obtain the Idx
-th group element?
What about element()
, block()
, part()
?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
OK I think I see it clearer now.
If the syntax BundleTangent::get<Idx>()
is correct, then I'd use block<Idx>()
Same for Bundle::block<Idx>()
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Whatever the decision, I think it's good to be consistent. So, if one part of the code uses PartType
, then this should be part<Idx>()
. Otherwise, use BlockType
and block<Idx>()
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Block
as a meaning in Eigen
so I'd rather use element
to avoid confusion.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Actually, the meaning of block
in Eigen is quite similar than in our case, if that helps.
element
is OK too.
Would it be ElementType
too, instead of PartType
?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Would it be ElementType too, instead of PartType?
I suppose so. Note that PartType<Idx>
really only is a place holder for whatever underlying manif
type (SO2
, RnTangent
etc...).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
OK I just said it in case Part
had the same semantic meaning as element
, otherwise it's not good to use the same word of course!
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
BTW it would be interesting to see Examples V.B and V.C in the paper implemented with the new bundle!
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks for the suggestions, I went with block<Idx>()
here.
Edit: Ops, I didn't see your new comments when writing this. I don't have a strong opinion on this matter so if you prefer element
I'll change to that
Thanks for the suggestions, I went ahead and took care of those for now. I realized that |
What's the status of this PR? |
This PR has some conflicts now and could probably use a rebasing. I'll take care of that. I would suggest to move this #include <manif/experimental/bundle.h> The main motivation is the 'hackyness' of the implementation, especially the cpp11 unfolding trick. (no offense @pettni, it is fairly difficult to use this level of meta-programming with only cpp11). So if we are to merge it as is, I would strongly suggest the |
If it's only the fold trick you don't like we could probably do something different. I'm not exactly a fan either but decided to prioritize conciseness (and was coming from cpp17). Here's an alternative template<typename F, typename ... Seqs>
void for_each(F && f, Seqs ...)
{
f.template func<intseq_element<0, Seqs>::value ...>();
for_each(std::forward<F>(f), typename intseq_tail<1, Seqs>::type{}...);
}
template<typename F, int ... Is>
void for_each(F && f, intseq<Is> ...)
{
f.template func<Is...>();
}
struct MyFunctor
{
template<int I, int J, int K>
static void func()
{
std::cout << I << " " << J << " " << K << std::endl;
}
};
int main()
{
auto a = intseq<0, 1, 2, 3, 4>{};
auto b = intseq<4, 3, 2, 1, 0>{};
auto c = intseq<1, 2, 1, 2, 1>{};
for_each(MyFunctor{}, a, b, c);
return 0;
} It compiles with template<std::size_t _Idx, typename _Seq>
struct intseq_tail;
template<std::size_t _Idx, template<int ...> class _IntSeq, int Head, int ... Tail>
struct intseq_tail<_Idx, _IntSeq<Head, Tail...>>
: public intseq_tail<_Idx - 1, _IntSeq<Tail ...>>
{};
template<template<int...> class _IntSeq, int Head, int ... Tail>
struct intseq_tail<0, _IntSeq<Head, Tail...>>
{
using type = _IntSeq<Head, Tail...>;
}; |
@pettni thanks for the quick feedback. |
I'll leave it to the maintainer to decide what's experimental and not :). To me for_each vs fold is mostly a matter of taste. If you think |
I do think it is preferable but hold on implementing it 'til we discuss the following.
What #150 does is that it implements (struct) functors for each of the functions that requires per-group specialization (e.g. template <typename LieGroup> struct ExpFunctor;
template <> struct ExpFunctor<SO2> { static void run(Coeffs& c) { ... } }; which are then called directly from the base (e.g. The difference between such functors ( If you have time, would you consider implementing an example as suggested by @joansola?? These are great from a library documentation point of view. |
I ported the example in V.B to the bundle type, but in my opinion it's not a great use case since the problem size must be known at compile time. One could envision a "run-time bundle" that supports dynamically adding element etc, but that's a whole other project, and the compile-time version is more useful for the problems I encounter. The new example does however map well between the paper and the bundle type, so it might make sense to have it in the library. The state update reduces to this which is pretty nice: X += BundleT::Tangent(dX); I also modified the |
I'm not sure we could write functors that work for both #150 and this PR, although there may be benefits from writing them at the same time in terms of keeping the code cohesive. What I had in mind here are functors along these lines that can be looped over with struct Functor
{
Functor(M & m, Jac & J)
: m_(m), J_(J) {}
template<std::size_t Element, int Beg, int Len>
void func()
{
// do some operation on m_.element<Element>() and insert derivative into J_.block<Len, Len>(Beg, Beg)
}
private:
M & m_;
Jac & J_;
}; |
Thanks for the example. Indeed V.B is not a great fit for the
One could indeed, but I think that's outside the scope of
I can only assume you know that
See e.g. the |
Ah yeah this is exactly the type of problem I have used it for in the past and why I'm interested in getting the bundle into manif :). I didn't make the connection when looking through the paper since the bundle in V.C is defined similarly to V.B. Since it's harder to write a new example than to port an existing one, and a fair amount of documentation would be warranted given that there is no corresponding example in the paper, I'm not sure I have the time right now to write such an example so I'd prefer leaving it for the future.
Yeah, they are very different from a dynamics perspective, but, in my understanding, as representations of orientation plus translation they are more or less equivalent and in some applications it can be better to use <R3, SO3> since computations are lighter. I think this is true for problems like pose graph estimation and sensor-to-sensor calibration, but probably not for filtering which is closely intertwined with dynamics. I considered changing |
Hi @pettni, Sorry for the long wait. I have finally been able to 'play' around with the I've made a few modifications that I've temporarily pushed on the branch from template<int ... _Idx, int ... _BegDoF, int ... _LenDoF>
LieGroup inverse_impl(OptJacobianRef, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; to template <int ... _Idx>
LieGroup inverse_impl(OptJacobianRef, internal::intseq<_Idx...>) const; I'm not quite done yet, there are still a corner or two I'd like to smooth but I'd love to hear your thoughts on these modifications. Anyway I feel like I'm having a good grip on it and I'm feeling more confident to merge it soon. In case you're interested, I'm using the Bundle class in a Kalman filter for the self-calibration of a diff-drive motion model (see here - @joansola you're likely going to be interested as well). Note that this is WIP as well... |
Cool, I think that's a good improvement. Let me know if there's anything else I can do on my end. |
I felt compelled to take a stab at #84 and this is the result.
I believe it is functional as it is, though it could for sure use more testing. Wanted to see what you think before proceeding.
Some comments:
Composite
andCompositeTangent
are templated on the underlying lie group type, i.e.CompositeTangent<double, SE3, R2>
(and notCompositeTangent<double, SE3Tangent, R2Tangent>
) is the tangent ofComposite<double, SE3, R2>
.template<size_t I> get()
that allows access to the individual parts via aMap
. I would have preferred to inherit fromstd::tuple
to getstd::get
for free but it seems like it is assumed throughout the library that all types store the underlying data as a single continuous array.std::integer_sequence
and the previous C++17 fold expressions are now using this C++11initializer_list
trick.LieAlg
andTransformation
types. They are there so that the full interface is implemented and the tests runJacobian
andTransformation
are block-diagonal and could potentially be represented with sparse instead of dense matrices.