Attention: Answers.ros.org is deprecated as of August the 11th, 2023. Please visit robotics.stackexchange.com to ask a new question. This site will remain online in read-only mode during the transition and into the foreseeable future. Selected questions and answers have been migrated, and redirects have been put in place to direct users to the corresponding questions on Robotics Stack Exchange. Additional details are available here.

Revision history [back]

I know this is really late and I'm sure you would have figured this out, but maybe this will benefit someone else in the future.

The error in your code is where you iterate over the points vector. Now since n_points = points.size(), you should only iterate until i < n_points/3 not until i < n_points since you are later multiplying i with 3 during the access.

ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.