Improve furniture fitting

[jlunenburg]

In #37 and #38 a unit test was added and a lot of refactoring has been done in order to improve testability. These are the first steps to improve. Further improvements:

• write unit tests for the separate functions (especially findOptimum and to a lesser extend preProcessInputData) that are present after Feature/test tools: refactored 'Fitter'  #38 . This should give some insight in what is/is not working correctly (as well as the accuracy). Examples are:
  • usecase with other objects in the background
  • usecases with objects partially in view
  • usecases with objects 'colliding' with other WM objects
• improve the current unit test: right now the number of 'failed cases' as well as the thresholds for failing are quite arbitrary (they been selected as sort of a regression test). It's (i) not clear whether these 'failing' cases are unrealistic (hence cannot succeed) or that the algorithm doesn't suffice and (ii) if the thresholds are as good as we can get them.
• add an ' inspector' object that can be passed to one or more functions. This could be used, e.g., to 'visualize' what's actually going on. You might consider using OpenCV to draw results and display them/write them to disk. N.B.: I'd propose to do this in a way that' s not intrusive in the happy flow of the application to avoid a deterioration of performance
• even better: it would be nice if, in case fitting fails, the 'request' is written (serialized) to disk. A separate 'tool' can then be used to de-serialize the data and re-run the algorithm to incrementally improve.
• both the iteration over the beam index as well as the yaw are very naive: they simply go from the lowest value to the highest value.
  • basically, we have a 2D search space (beam index, yaw)
    • the search space is discretized. A yaw step of 0.1 rad = 5.7 degrees this is a lot! Can we make it smaller/adaptive?
    • the beam model also has a finite number of points. At a larger distance, this might also be an issue. What's a good number of beams?
  • can we use a 'smarter' search algorithm? --> Instead of starting at beam_index == 0 and yaw == yawrange.min, we start with the expected beam index and yaw == 0.0 and use a gradient descent method (so evaluate the eight options around it, pick the best one and evaluate the ones around that; repeat this until you've arrived at an optimum).

[PetervDooren]

@VictorvanHelden

[PetervDooren]

Another issue we may face is that we cannot check the preconditions of the algorithm.
In certain situations we may not see enough features of an entity to properly fit it. Take for example the situation of standing too close to the table. The robot will see one line, which might be the front or a segment of one of the sides. In these cases the optimum is determined only by the random variance in sensor points.