Diff drive 2

This PR is on top of branch dynamic_sensor_param thus I'm pushing there.
It replaces !93 (closed) and is ready for review.

• add CaptureVelocity
• add CaptureWheelJointPosition
• add SensorDiffDrive
• add ProcessorDiffDrive
• add FeatureDiffDrive
• add ConstraintDiffDrive

Added also an example with data : $ ./test_processor_diff_drive path/to/wolf/src/test/data/diff_drive/wheel_data.dat 1.

@jsola can you please review the calibration pipeline ??
Hopefully I got it right.

Jeremie: mind the names of classes:

VelocityCapture -> CaptureVelocity

WheelJointPositionCapture -> CaptureWheelJointPosition

we want to keep the first word as the important word, the noun Capture.

All Wolf names are like this.

Oh I see in the files it's OK

changed the description

I updated the PR description ;)

Well I just realized that I added here the file eigen_checks.h which is already merged in master under the name eigen_assert.h. I'll remove it from this PR but it means this won't compile until the branch is rebased on master and a couple includes fixed.

Dont have much time to check code. But computeCurrentDelta() I think it's wrong.

Please see the operations in the pipeline, then apply the chain rule correctly. For this, use appropriate naming for jacobians:

y = f(x) => J_y_x = df/dx -- use 'y' and not 'f' in the jacobian name
z = g(y) => J_z_y = dg/dx -- use 'z' and not 'g' in the jacobian name

Then,

z = g( f(x) ) => J_z_x = J_z_y * J_y_x -- see how subindices chain perfectly. This is the proper chain rule.

Your code in processor_diff_drive.cpp line 125 does not follow this rule. Maybe you just need to swap the product, I did not check further.

Well given :

vel   = f(data, calib) => J_vel_calib
delta = g(vel)         => J_delta_vel

delta = g(f(data))

my initial chain rule is correct, the notation wasn't.

J_delta_calib = J_delta_vel * J_vel_calib

That is what we are looking for in the computeCurrentDelta function right ? There are no use of J_vel_data here ?

added 5 commits

• 07ebf975 - add CaptureWheelJointPosition
• 4895de7b - add SensorDiffDrive & diff-drive tools
• bd920864 - add ProcessorDiffDrive
• 484dced1 - add FeatureDiffDrive
• 299a248e - dummy diff-drive example

Compare with previous version

Forced-push quit a few fixes.
I believe the math are correct now (not sure about the notation though).
I'm only missing the appropriate constraint now.

added 3 commits

• a88f6c4a - add FeatureDiffDrive
• d282ce67 - add ConstraintDiffDrive
• d8bf03bd - dummy diff-drive example

Compare with previous version

added Code organization enhancement labels

Added the Feature/ConstraintDiffDrive classes.
Calibration pipeline to be reviewed.

mentioned in merge request !93 (closed)

changed the description

OK for the jacobians and the chain rule.

As for J_vel_data, we use it in computeCurrentDelta() to compute J_delta_data = J_delta_vel * J_vel_data, also with the chain rule.

So what we really need is J_delta_data and J_delta_calib only. The first is used to compute delta_cov from data_cov, also computed in computeCurrentDelta(). The second one is for auto-calibration. All other Jacobians are temporaries inside your function.

I do not compute J_delta_data explicitly but I do compute
vel_cov = J_vel_data * data_cov * J_vel_data.transpose()
and then
delta_cov = J_delta_vel * vel_cov * J_delta_vel.transpose().

This is hidden within the inner functions in computeCurrentDelta().

Yes, OK, what you do is correct. But just consider these two ways of proceeding:

1. Compose Jacs, then compute cov

J_delta_data = J_delta_vel * J_vel_data --> 1 matrix product
delta_cov = J_delt_data * data_cov * J_delta_data.tr --> 2 matrix products.

Total: 3 matrix products

2. Compute one cov, then compute the other cov

vel_cov = J_vel_data * data_cov * J_vel_cov.tr --> 2 matrix products
delta_cov = J_delta_vel * vel_cov * J_delta_vel.tr --> 2 matrix products

Total: 4 matrix products

So it's a matter of efficiency to choose one method over the other one.

There is a very odd thing. In line 38 you have:

/// Retrieve sensor intrinsics
const IntrinsicsDiffDrive& intrinsics =
      *(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());

The intrinsic parameters are precisely the calibration parameters. They are usually stored in the MotionBuffer of the last_prt_ Capture, and passed by ProcessorMotion to ProcessorDerived::computeCurrentDelta() over the input parameter _calib, as you can see here (from ProcessorMotion::integrateOneStep()):

void ProcessorMotion::integrateOneStep()
{
    // Set dt
    updateDt();

    // get vector of parameters to calibrate
    calib_ = getBuffer().getCalibrationPreint();

    // get data and convert it to delta, and obtain also the delta covariance
    computeCurrentDelta(incoming_ptr_->getData(),
                        incoming_ptr_->getDataCovariance(),
                        calib_,
                        dt_,
                        delta_,
                        delta_cov_,
                        jacobian_delta_calib_);
[...]

Therefore, inside computeCurrentDelta(), you just need to extract from _calib your params, e.g.:

Scalar r_left     = _calib(0);
Scalar r_right    = _calib(1);
Scalar separation = _calib(2);

and proceed from here.

If you take these parameters from elsewhere, I cannot guarantee that your method will perform autocalibration properly.

Joan