Investigation on wrong 3D computation when head (or torso) is moving

[pattacini]

@towardthesea's preliminary analyses show that the source of the problem is likely due to a missing call to update the global matrix that accounts for the current kinematic configuration of the head.

@towardthesea, could you please report here on the piece of code under inspection?

Eventually, please open a PR to fix the problem.

[towardthesea]

The global variable is HL_root, which involves in transformation computation as:

P=HL_root*Hrect*P;

in here, here and here. They belong to method get3DPointsAndDisp, get3DPoints and get3DPointMatch respectively. However, the HL_root is only updated once periodically by calling the getCameraHGazeCtrl in updateModule.

I plan to introduce the getCameraHGazeCtrl to update the value of HL_root before conducting computation with it.

By the way, I don't have permission with this repo :smile:

[pattacini]

Hi @towardthesea You don't need permission to fork and open a PR 😉

[towardthesea]

As a step to isolate the source of the bug, we conducted the following experiment, experiment 0.

Experiment 0:

In this experiment:

• Feeding SFM with the dumped sequences of images (in which the scene was almost fixed) by yarpdataplayer. Theoretically, this leads to the same result from disparity computation of the stereoCamera .
• Moving the neck or torso of icub.
• Asking SFM to give the 3D coordinates of 3 CvPoint with coordinates as: (170 220), (150 220) and (160 180) respectively. The code for request is as following:

  
  Bottle cmd,reply;
  cmd.addString("Rect");
  cmd.addInt(point.x-3);
  cmd.addInt(point.y-3);
  cmd.addInt(7);
  cmd.addInt(7);
  cmd.addInt(2);

mutexResourcesSFM.lock(); rpcGet3D.write(cmd,reply); mutexResourcesSFM.unlock();

Please have look on following video for details of experiment. The 3 red boxes in the video have the 3D coordinates obtained from **SFM**, while their sizes were estimated and fixed.

[![experiment 0](http://img.youtube.com/vi/nCL1Jn01lE0/0.jpg)](https://www.youtube.com/watch?v=nCL1Jn01lE0)

As we got from this experiment, during and after the motion of an iCub part (neck/torso), the 3 boxes move accordingly.

## Data:
These are the dumped data used for the experiment: [:minidisc:](https://drive.google.com/file/d/0B5uzFFH3HdnoTDhyS1ZPeFFsTHM/view?usp=sharing)

## Quick conclusion:  
- **Kinematic computation** for frame of reference transformation seems to be good and updated.

[towardthesea]

Another result: the disparity computation takes about 50-60ms to complete; during the computation, the module is blocked by mutex.

Is this result acceptable @GiuliaP @pattacini

[pattacini]

@towardthesea 50 ms corresponds to 20 fps, which is the rate we've been experiencing with SFM, hence we're in the expected range.

If that call needs to be protected against race conditions (likely yes) and what this implies for the remaining code and, specifically, for our investigations is to be determined 😉

[towardthesea]

@Tobias-Fischer a problem that I mentioned yesterday!

[Tobias-Fischer]

@towardthesea: I see. I'm sure we can investigate this together. I am not too familiar with the stereo-vision code though.

One quick observation: The mutexDisp is only protecting computeDisparity, however the results of computeDisparity are only being retrieved in https://github.com/robotology/stereo-vision/blob/master/modules/SFM/SFM.cpp#L392 - so I am not sure whether the result in L392 corresponds to the computation in L363 (and whether this is important at all, if not, please ignore ;)).

[towardthesea]

I will take a note and consider your point when I can come back to this issue :+1: @Tobias-Fischer