Revision history [back]

Firstly a set of 3D points is a point_cloud, is there a reason you're not using the existing point cloud messages for this? They would have the advantage of working with the visualizers in RVIZ as well as compatibility with a lot of existing nodes.

Secondly if you define a custom message with an member geometry_msgs::Point[] my_points then in C++ this will already be implemented as a std::vector<geometry_msgs::Point> so you don't need to cast anything. However the std::vector type will not help you remove duplicate points.

You'll need an algorithm to detect and remove duplicate points, how you do this will depend on how many points you're working with. If it's a few hundred then you can probably get away with simply comparing each new point to the set of existing unique points. However this is O(n^2) time complexity so will get very slow for larger point clouds. If you're working with large point clouds (anything over a thousand points) I'd recommend using PCL and producing a KD tree, the KD tree allows you to search for duplicate points significantly faster and will make a big difference for larger clouds. PCL will also give you functions to convert the result to a PointCloud2 ros message into the bargain.

Hope this gives you some ideas.