vector of vector buffer c++ - c++

Question

for my application I need to create a fixed size buffer (3 elements) of point clouds.
To do this I tried the naive way in my callback (I'm working on ROS):
vector< vector<Point2d> > points_buffer(3); // buffer of point clouds ,fixed size = 3
void laserToWorldCallback(const icars_laser_roi::stx_points::ConstPtr& laser_points, const icars_2d_map_manager::Status::ConstPtr& car_pos){
double x_w, y_w;
double x, y;
vector<Point2d> temp;
for(int i = 0; i < laser_points->points_x.size(); i++){
// get the coordinates
x = laser_points->points_x[i];
y = laser_points->points_y[i];
// tranform the coordinates
x_w = car_pos->xGlobal + x*cos(car_pos->yaw) - y*sin(car_pos->yaw);
y_w = car_pos->yGlobal + x*sin(car_pos->yaw) + y*cos(car_pos->yaw);
temp.push_back(Point2d(x_w, y_w));
}
if(points_buffer.size() != 3){ // the buffer is not empty
points_buffer.push_back(temp);
}else{ // the buffer is empty, delete last element and push_back
// delete last element
points_buffer[0] = points_buffer[1];
points_buffer[1] = points_buffer[2];
points_buffer[3] = temp;
}
}
}
But this way seems to me a bit rough and not efficient at all.
Might someone suggest me a more elegant and efficient way to do what I want?
Thank you
Regards

Answer 1

To fix some efficiency problems. First after declaration of temp you can already reserve the memory it will use with
temp.reserve(laser_points->points_x.size());
So there will be no reallocation of memory in push_back method.
If you are using c++11 or greater, in the case buffer is not yet full, you can move the content of the temp with std::move.
points_buffer.push_back(std::move(temp));
This is a O(1) operation. The content of temp after this is valid but unspecified.
Then in the deleting the last element use vector::swap instead of copy as it will swap the content and is guaranteed to be constant in time.
points_buffer[0].swap(points_buffer[1]);
points_buffer[1].swap(points_buffer[2]);
points_buffer[2].swap(temp); //There is a typo here index should be 2 not 3.
The program would be more readable, if you would wrap point_buffer in a class. Then you could also consider not rotating the content of whole vector but keeping track of the first index. This would work well also for larger point_buffer than 3. Then adding new element to buffer would be just
point_buffer[fist_element_].swap(temp);
first_element=(first_element_+1)%3;
then to access the element at position i you could implement the operator[] as
vector<Point2d>& operator[](int i){
return point_buffer[(i+first_element)%3];
}