Add ContactResultMap class to reduce heap allocations for multiple contact requests

[Levi-Armstrong]

Initial testing shows this can have a significant performance bump. See results bellow where the last two columns show % reduction in Time, CPU Time. In case with large number of contacts the reduction is around 99%.

[Levi-Armstrong]

Baseline heaptrack summary for reference. I will post the results after with updates.

[Levi-Armstrong]

@marip8 If you have time it would not hurt to have another set of eyes on this PR before it gets merged.

[Levi-Armstrong]

Just finished adding the unit tests

[Levi-Armstrong]

Improvements results. This change has a significant impact on Peak Memory, Heap Allocation and Performance.

Highlights

• TrajOpt
  • Performance
    • Simple 88% decrease
    • Complex 3% decrease
  • Peak Memory: ~30MB (25%) decrease
  • Memory Allocations: 9 Million (35%) decrease
• TrajOpt Ifopt
  • Performance
    • Simple 89% decrease
    • Complex 4% decrease
  • Peak Memory: ~33MB (79%) decrease
  • Memory Allocations: 3 Million (38%) decrease

TrajOpt

					Test Body
ID	Simple Unit(us)	% Change	Planning Unit (ms)	% Change	Peak Memory	% Change	Allocations	Temp Allocations	Allocation Sum	% Change	Description
Baseline	2,860.00		23,432.00		111.40		24,739,478.00	885,722.00	25,625,200.00
1	332.00	-88.39%	22,741.00	-2.95%	83.00	-25.49%	16,175,975.00	470,917.00	16,646,892.00	-35.04%	Sel

TrajOpt Ifopt:

					Test Body
ID	Simple Unit(us)	% Change	Planning Unit (ms)	% Change	Peak Memory	% Change	Allocations	Temp Allocations	Allocation Sum	% Change	Description
Baseline	2,786.00		1,041.00		41.60		9,170,119.00	262,537.00	9,432,656.00
1	303.00	-89.12%	999.00	-4.03%	8.70	-79.09%	5,738,612.00	24,028.00	5,762,640.00	-38.91%

[Levi-Armstrong]

Improvements results including the last two commits

Highlights

• TrajOpt
  • Performance
    • Simple 88% decrease
    • Complex 3% decrease
  • Peak Memory: ~34MB (30%) decrease
  • Memory Allocations: 14.5 Million (57%) decrease
• TrajOpt Ifopt
  • Performance
    • Simple 89% decrease
    • Complex 10% decrease
  • Peak Memory: ~34MB (81%) decrease
  • Memory Allocations: 7.3 Million (77%) decrease

TrajOpt

	Baseline	Latest	% Change
Simple Unit(us)	2,860.00	331.00	-88.43%
Planning Unit (ms)	23,432.00	23,222.00	-0.90%
Peak Memory (MB)	111.40	77.40	-30.52%
Allocations	24,739,478.00	10,669,590.00	-56.87%
Temp Allocations	885,722.00	397,571.00	-55.11%
Total Allocations	25,625,200.00	11,067,161.00	-56.81%

TrajOpt Ifopt:

	Baseline	Latest	% Change
Simple Unit(us)	2,786.00	296.00	-89.38%
Planning Unit (ms)	1,041.00	933.00	-10.37%
Peak Memory (MB)	41.60	7.80	-81.25%
Allocations	9,170,119.00	2,098,024.00	-77.12%
Temp Allocations	262,537.00	21,060.00	-91.98%
Total Allocations	9,432,656.00	2,119,084.00	-77.53%

[Levi-Armstrong]

As a sanity check to make sure it was not an issue passing an object that contains a thread_local by reference into multiple threads is an issue. It was that it is not an issue and even though it was created in a different thread and passed by reference to two threads it still gets its own copy.

#include <iostream>
#include <vector>
#include <thread>

struct TempClass
{
  TempClass(int i) : cnt_(i) {}
  void fn()
  {
    thread_local std::vector<double> data;
    std::cout << "This: " << this << std::endl;
    std::cout << "Capacity: " << data.capacity() << std::endl;
    std::cout << "Address: " << &data << std::endl;
    data.reserve(cnt_);
  };
private:
  int cnt_;
};

void threadFn(TempClass& tmp)
{
    tmp.fn();
}

int main()
{

    TempClass a(5);
    auto f = [&a]()
    {
        a.fn();
    };
    std::thread t1(f);
    std::thread t2(f);

    t1.join();
    t2.join();
}

Results:

Program returned: 0
This: 0x7fff88a8429c
Capacity: 0
Address: 0x7fa15188a6e0
This: 0x7fff88a8429c
Capacity: 0
Address: 0x7fa1510896e0