C++20 concepts for common robotics/computer vision datatypes and algorithms

[bergercookie]

This is an early attempt to define a set of C++ robotic constructs to encourage code reuse and interoperability among the various open-source robotic projects.

These can be implemented in the form of the C++20 concepts TS

More specifically concepts can be used to define and enforce specific behavior (e.g., methods to be present in a class) for all the classical robotic constructs that need to be implemented in any new project. As an example these may include:

• Pose (2D, 3D, etc.)
• Graph
• PointXD / Landmark
• SensorMeasurement
• PointCloud
• ...

Having these constructs in place will allow to define a standard among the different implementations; one will know what to expect from a Point instance regardless if they deal with an MRPT CPoint2D, a JDERobot CPoint3D a custom implementation of theirs or anything else that could abide to that concept. It will also help to reason about and benchmark the various implementations.

An example boilerplate code for e.g., a pose would be the following (does not compile):

// forces every pose object to implement specific methods
template<typename T>
concept bool PoseAble = requires(T a){
	{ a.isPDF() } -> bool;
	{ a.dims() } -> std::size_t;
	{ a.translation() } -> VectorisAble&;
	{ a.rotation() } -> RotationMatAble&;
	// ...
};

void compose(const PoseAble& p1, const PoseAble& p2)
{
	// compose the two poses by calling methods that should
	// be present since they implement the `PoseAble`
	// concept
}

int main(int argc, char *argv[])
{
	PoseAble& p1 = mrpt::poses::CPose2D(1.0, 2.0, M_PI/4.0);
	PoseAble& p2 = mrpt::poses::CPose2D(2.0, 3.0, 0.0);

	compose(p1, p2);
	return 0;
}

P.S. The latest update is that concepts are to be included in C++20 version. There is also a concepts implementation in gcc6 and can be used by compiling with -fconcepts -std=c++1z

Links

• TS Document
• Concepts - cppreference
• Minimal concepts example

Next actions

To get this going I can create a separate repo and then transfer ownership to the MRPT org. There we can also discuss the specification and format of the concepts to be implemented in separate gh-issues (issue per concept).

How does this all sound to you?
Any ideas for the name? how about robot-concepts?

@jolting, @jlblancoc, @EduFdez,

Also adding @eperdices, @okanasik from the JDERobot project and @rcintas from Robocmp

[jlblancoc]

Thanks for putting all the first draft ideas together! As briefly mentioned in private, of course I like this initiative. 👍

Another byproduct of it, if gets finally done, if that we would be able to implement generic ROS node wrappers for families of algorithms: navigation, mapping, localization, egomotion data fusion, etc. which could then be linked to each particular library.

We'll probably find hard times when we reach the point of particularizing how to refer to mid-to-large objects: references? shared ptrs? something that could be both? ROS-like msgs?

On the name: robot-concepts is good! I only miss a way to get the computer vision guys to feel included... However, I don't have a better proposal for now!

That's all my contribution for now, will get back...

[bergercookie]

Boilerplate code: https://github.com/bergercookie/robot-concepts

[jolting]

Wow. You really ran with it. Cool

[jolting]

What about a convention like APose?

[jlblancoc]

Wow. You really ran with it. Cool

Absolutely, he did! I'm simply out of spare time to check it for now in detail, sorry, will do it asap.

But here's another idea: what about adding ROS REP-like draft documents to the robot-concepts repo? They should be the "rationale" behind:

• the entire "concepts" system (!)
• one document per "concept" or family of related "concepts"

[jlblancoc]

Project title tiny change proposal:

- C++20 concepts for common robotics/computer vision datatypes
+ C++20 concepts for common robotics/computer vision datatypes and algorithms

[bergercookie]

Absolutely, he did! I'm simply out of spare time to check it for now in detail, sorry, will do it asap.

Nah, nothing to check yet, it's just the directory sturcture and a README for now :)

What about a convention like APose?

LGTM, I used .*Able in the example but A.* is better

But here's another idea: what about adding ROS REP-like draft documents to the robot-concepts repo? They should be the "rationale" behind:

I was thinking of the same, but with the rust rfc in mind.

If we do decide to go for this I 'd argue one doc per concept. Also we might as well put it in a separate repo (e.g., robot-concepts-rfc) to track it separately from the code changes

[jolting]

isPDF should be a pose trait

template <typename T>
inline constexpr bool mrpt::pose_traits::isPDF_v<T>

[bergercookie]

isPDF should be a pose trait

I was under the impression that C++ concepts are traits-like cosntructs enforced in compile time. What's the benefit of having it as an independent trait?

[jolting]

An APosePDF is composed of a mean and a covariance. The mean is an APose and the convariance could have some concept like ACovariance and can be some kind of matrix.

isPDF is essentially the same as saying the type is required to be an APosePDF. I think that's a better way to express it and avoid the whole isPDF thing. Just treat them as two concepts.

CPose2DPDF and CPose2D are fairly disparate types and you really can't generalize algorithms any further than that anyway. This also is in line with the object-oriented composition over inheritance principle.

[jolting]

#include <cstddef>
#include <array>

template<typename T>
concept bool AVector = true;

template<typename T>
concept bool ARotation = true;

template<typename T>
concept bool APose = requires(T a){
        { a.dims } -> std::size_t;
        { a.translation } -> AVector&;
        { a.rotation } -> ARotation&;
};

template<typename T>
concept bool ACovariance = true;

template<typename T>
concept bool APosePDF = requires(T a){
        requires a.dims == a.mean.dims;
        { a.mean } -> APose;
        { a.covariance } -> ACovariance;
};


class Pose2D{
public:
        static constexpr std::size_t dims = 2;

        std::array<double, 2> translation;
        std::array<double, 3> rotation;
};

class Pose2DPDF{
public:
        using Pose = Pose2D;
        static constexpr std::size_t dims = Pose::dims;
        Pose mean;
        std::array<double, 9> covariance;
};

int main(int argc, char *argv[])
{
        Pose2D p1;
        Pose2DPDF p2;
        APose& r1 = p1;
        APosePDF& r2 = p2;
        return 0;
}

[jlblancoc]

Probably overkilling, but found it interesting:
https://github.com/ldionne/dyno/blob/master/README.md

[jolting]

Unfortunately, runtime polymorphism is not a zero-cost abstraction.

[jlblancoc]

Great to see the progress up to bergercookie/robot-concepts@cd7049b !
It seems not easy at all to write down "concepts", especially the first times :-)

I still have to build it but just from a first glance, I can see a possible source of controversy:

{ a.rotation() } -> RotationMatrix<N>&;

I would say a pose, mathematically an "isometry" in SE(2)/SE(3), comprises:

• translation in N-dimensional R^N space (ok)
• rotation in that space, SO(2) or SO(3). (here's the beef).

To make concepts as generic as possible (while still usable by engineers!) we should make them at least compatible with:

• SO(2):
  • a plain heading angle (universally used)
  • a 2x2 rotation matrix
• SO(3)
  • a quaternion: this parameterization is heavily used as you all know in most modern solvers (g2o, etc.)
  • a 3x3 rotation matrix.

As it is now, the rotational part is defined in terms of the matrix, which is correct from the Math/Algebraic-point-of-view, but it would be a bottleneck for efficiency if it ends up enforcing all implementations to pass forth and back to/from matrices, even if they use quaternions inside.

Do you see my point?
I guess we should define the concept of rotation in terms of what a rotation can do, instead of how it is parameterized...

I don't mean you should take back the rotation() "virtual"/"abstract" method, since it might be really useful in many cases. Just thinking aloud on how to make it in the most efficient way...

Probably the rotational part of a Pose should have its own concept: SO, with tparam 2 or 3:

/**
 * Rotation: as a rotation matrix for SO(2)/SO(3)
 */
{ asMatrix() } -> RotationMatrix<N>&;

/**
 * Rotation: as a heading angle for SO(2) or (qw,qx,qy,qz) for SO(3)
 * (M = 1 or 4 for each case!!)
 */
{ asParams() } -> Vector<M>&;

/**
 * Lie Algebra logarithm map: vee operator applied to the logarithm of SO(2)/SO(3) 
 * (L = 1 or 3 for each case!!)
 */
{ log() } -> Vector<L>&;

[jolting]

Std C++ uses std::to_string for conversion methods.

Consider a seperate function to_matrix or to_quaternion for explicit conversations. It would make it easier to adopt for libraries if you don't use all representations and honestly most algorithms won't care about underlying representation, so best not to include it if it is not necessary.

[bergercookie]

Sorry about the late reply,

As it is now, the rotational part is defined in terms of the matrix, which is correct from the Math/Algebraic-point-of-view, but it would be a bottleneck for efficiency if it ends up enforcing all implementations to pass forth and back to/from matrices, even if they use quaternions inside.
Do you see my point?

I see, yeah. This is all super boilerplate code at the moment, I just wanted to have some basic concepts implemented to see how they work.

I'll be looking into formulating this more concretely and into reading a bit more into using concepts effectively (i'mm still getting the hang of this as I haven't found that much resources/examples of them yet)

Consider a seperate function to_matrix or to_quaternion for explicit conversations. It would make it easier to adopt for libraries if you don't use all representations and honestly most algorithms won't care about underlying representation, so best not to include it if it is not necessary.

Agree