Improve 3D detection pipeline

[maciejmajek]

Is your feature request related to a problem? Please describe.
#681 & #680

Describe the solution you'd like
Finalizing the design from https://github.com/RobotecAI/rai/tree/feat/debug_3dpipe

todo:

• Make sure it works
• Clean up the code
• PR to main

[Juliaj]

@maciejmajek, happy to help with this! I'll start with unit tests and then look at the overall design and implementation. I'm going to start with synthetic data for unit tests, but real images would be really helpful. Do you have any test images we could use? Also, beyond these, what other areas do you think we should validate? Thanks.

[maciejmajek]

I should be able to provide real world data from the Realsense camera (RGB image + Depth map).
Regarding validation areas, I'm not sure. I should be able to take a deeper look next week. Thanks!