Fix Memory Leak in track_error_benchmark

[Chi-hong22]

Fix Memory Leak in track_error_benchmark

Description

Fixed a memory leak issue in the track_error_benchmark class that occurred during nested vector destruction of grids data structures.

Problem

When destructing large nested vector structures containing Eigen matrices in the add_benchmark method, a memory corruption occurred at the vector destructor level. This typically happened with large datasets due to improper memory management of temporary grid maps.

The specific error occurred in:

~vector() in libc.so.6!__GI___libc_free() 

Affecting the following nested vector structure:

vector<vector<vector<Eigen::MatrixXd>>>

Solution

1. Modified add_benchmark() to use smart pointers for managing grid_maps lifecycle
2. Explicitly releasing OpenCV Mats after usage
3. Added better scoping for temporary variables
4. Improved memory management in critical sections

The key changes include:

auto grid_maps = std::make_shared<std::vector<std::vector<std::vector<Eigen::MatrixXd>>>>(
    create_grids_from_matrices(maps_points));

And explicit resource cleanup:

error_img.release();  // Explicit release
mean_img.release();   // Explicit release 

[Chi-hong22]

@nilsbore Please review the code as soon as possible, this problem has bothered me for half a month, and there is no memory problem if you use your code before May