Add covariance

[efernandez]

Add pose and twist covariance.

Also add tests for the covariance and the twist fixed in #2.

Also roslint has been added and several warnings fixed.

[efernandez]

@servos @paulbovbel

I'll squash the commits after your review.

For the positive definite test/check on the covariance matrix I cannot compared against 0 because of numerical issues with Eigen eigensolver for self-adjoint matrices. I've also tried the Cholesky decomposition, and it's only slightly more robuts. With Matlab eig the test passes. As a workaround, which I think it's appropriate in this case, I compared against -EPS. With this, all the tests pass.

[servos]

This fails (ie vl = 0) if T is an int (not that that is likely). Any reason not to do (vr + vl) / T(2)?

[efernandez]

T must be double/float or a dual number like Jet<double/float, N>. It should never be int, but I'm not sure how to avoid that.

I use T(0.5) as it was done before, no difference with dividing by T(2).

[servos]

Fair enough.

[mikepurvis]

Depending how strongly you feel, you can use static_assert to prevent the template being instantiated with inappropriate types, eg:

#include <boost/type_traits.hpp>
static_assert(!boost::is_floating_point<T>::value, "The pose components must be specified as float values.");

(Note that the type_traits module has been brought into the STL for c++11.)

[efernandez]

Good one.

Since C++11 is not allowed upstream, I've used BOOST_ASSERT_MSG instead of static_assert tough.

[paulbovbel]

I think you're looking for BOOST_STATIC_ASSERT_MSG for compile time

[efernandez]

Yes, that's true. Fixed.

It seems that with Boost the message isn't shown, and instead a quite strange compilation error is shown. This doesn't happen with static_assert (C++11) tough. See the two links for comparison purposes here.

I've decided to keep the check (using Boost, since C++11 isn't recommended in ROS as I understand from REP3), since the case when this happens is uncommon and at least the error points to the line with the static assert.

[efernandez]

BTW, after make test all tests pass:

100% tests passed, 0 tests failed out of 12

Now there are many more tests, in the sense that for the basic controller test also the covariance is tested.

[efernandez]

@mikepurvis Should I use a Clearpath Robotics copyright for the new files I've created here? If that's the case, could you provide me a link to an example?

[mikepurvis]

https://github.com/clearpathrobotics/headers/blob/master/bsd/cpp

[efernandez]

Thanks.

I'll make it 80-column width tough.

[efernandez]

@paulbovbel Any comments before I start squashing commits and merging?

[efernandez]

@mikepurvis Do you know if this is actually needed when cmake_modules is already a build depend, following this?

[mikepurvis]

I believe so. cmake_modules only gets you the ability to find_package it, but you still need the package itself to be present on the system.

[efernandez]

I've just squashed the commits.
I've also added a comment to make it clear that a covariance matrix is positive semidefinite, but we check for positive definite (up to some EPS precision) because in the tests we know (or want to enforce that) the covariance matrices must be positive definite.

If there are no additional comments, I'll merge this.