When parameters do not change

[Mehdi Tlili]

I am using lmfit to perform extrinsic calibration of range sensors on my mobile base.
Sometimes, the result of the optimization is shown as:

[[Variables]]
    laser_rot_x:     0 (fixed)
    laser_rot_y:    -0 (fixed)
    laser_rot_z:    -3.13948530 +/- 0        (-0.00%) (init=-3.139485)
    laser_trans_x:  -0.24600000 +/- 0        (-0.00%) (init=-0.246)
    laser_trans_y:   0.02680000 +/- 0        (0.00%) (init= 0.0268)
    laser_trans_z:   0.159 (fixed)

I basically moving the robot, and visualize what the range sensors is observing and also the predicted observations calculated from the robot's motion.
Even though I can clearly see that the points are not overlapping, lmfit seems to see no error. What is intriguing me is that +/- 0 that usually never happens in real life.
So in which cases one does get those +/- 0?

[Matt Newville]

Mehdi,

Hard to tell from that alone what the cause is.   Generally this would arise if changing the starting values for the variables by a small amount (for the initial step in the fit) has no impact on the residual.  It can also happen if the objective function calculates NaNs or other useless values.

In general, one gets +/- 0 if the uncertainties couldn't be estimated, including if small changes to the parameter values has no impact on the result.

It might be helpful to calculate the model and/or cost function for the initial starting values.

--Matt

[Mehdi Tlili]

I took a look at the output of the residual and it doesn't seem to have any NaN's. Here is how it is computed:

def residual(params, csm_predictions, base_link_rotations, initial_points, initial_base_link):
    """A functions that computes the residual error of the laser pose from URDF."""
    # Get base_link to laser transform
    base_link_to_laser = tf.transformations.compose_matrix(None,
                                                           None,
                                                           [params['laser_rot_x'],
                                                            params['laser_rot_y'],
                                                            params['laser_rot_z']],
                                                           [params['laser_trans_x'],
                                                            params['laser_trans_y'],
                                                            params['laser_trans_z']],
                                                           None)
    # Now for each base_link_rotation predict the scans how they would appear in the rotated lasers
    nr_points = csm_predictions[0].shape[0]
    nr_predictions = len(csm_predictions)
    data = np.zeros((nr_predictions * nr_points, 2))
    model = np.zeros((nr_predictions * nr_points, 2))
    for idx, (base_link_rotation, columns_observation) in enumerate(
            zip(base_link_rotations, csm_predictions)):
        # Pose of laser in the rotated/moved robot
        laser_rotated_pose = (initial_base_link.dot(base_link_rotation)).dot(base_link_to_laser)
        # Initial pose of the laser corresponding to initial_base_link
        laser_initial_pose = initial_base_link.dot(base_link_to_laser)
        # Transform matrix describing the motion if the laser translation/rotation
        laser_rotation = np.linalg.inv(laser_initial_pose).dot(laser_rotated_pose)
        # Now project the initially observed columns to predict their appearance in new pose
        projected_columns_observation = (np.linalg.inv(laser_rotation).dot(initial_points.T)).T
        # Put the model prediction and the data observation in their respective vectors
        data[idx * nr_points:(idx + 1) * nr_points, :] = copy.deepcopy(columns_observation[:, :2])
        model[idx * nr_points:(idx + 1) * nr_points, :] = copy.deepcopy(
            projected_columns_observation[:, :2])
    return data.flatten() - model.flatten()

As you see I am extensively using trigonometric functions and matrice inverses. Could this lead to a problem?
I also noticed that this mostly happens In simulation, I set my model to fit to the data (correct lidar pose), which makes sense as the optimal solution is already set up. But when I make slight changes in the laser pose parameters, lmfit still have troubles. Only when the pose error is really big that lmfit converges. Where should I look into?

[Matt Newville]

Hi Mehdi,

I doubt it.    It should be no problem to do complex calculations in the resdiual.
 

I also noticed that this mostly happens In simulation, I set my model to fit to the data (correct lidar pose), which makes sense as the optimal solution is already set up. But when I make slight changes in the laser pose parameters, lmfit still have troubles. Only when the pose error is really big that lmfit converges. Where should I look into?

If I understand correctly, you are moving a physical object (robot or laser position or something) based on the values of the parameters.  I've run into similar trouble, so I ave a guess and suggestion.

The algorithm generally starts by making a very small move (by default,  this is around 1.5e-8) and calculating the finite-difference derivative matrix (Jacobian) to then predict which way to move.    But if this small change has no impact on the result, then the fit will give up -- it can't find a better solution than where it started.   If the parameters correspond to a physical device, this might be because the default step size is below the resolution of the device.  

You might need to control what the size of that step is.  You can do this by passing 'epsfcn=value' to minimize().   The relative step size for each variable will be ~sqrt(epsfcn), so you may want to set this to be slightly larger than the square of the resolution of your physical positioner.    You also might want to print out values in your objective function too, just to see what values it is actually sending.

As it turns out, I've run into this problem myself, and I found that I did have to set epsfcn to around 1.e-5 (the default is around 2.e-16), where motor precisions were around 0.001.  

Hope that helps,

--Matt

[Mehdi Tlili]

Thanks Matt for you suggestion. I tried this epsfcn value but it doesn't seem to have any effect. I am not sure where to put it actually. I couldn’t find a detailed documentation of the minimize(function).

Here is a vizualization of my data:

​

Where the green squares end up overlapping the red squares after the optimization of the lidar pose. As you can see the error is not that small after rotating the robot quite a bit and comparing observations and predictions (few centimetres). In this specific case the lidar pose had a mistake of 1cm in y and x directions (relativ to robot center) and 1 degree in yaw angle. It would be helpful to point me to the documentation of this epsfcn parameter. I am using lmfit  0.9.2.

Cheers

Mehdi

--

B.Sc, Technische Universität München
Phone: +49 157-8068-6298
Email: mehdi...@gmail.com

--

B.Sc, Technische Universität München
Phone: +49 157-8068-6298
Email: mehdi...@gmail.com

[Matt Newville]

Hi Mehdi,

On Tue, Apr 12, 2016 at 9:10 AM, Mehdi Tlili <mehdi...@gmail.com> wrote:

Thanks Matt for you suggestion. I tried this epsfcn value but it doesn't seem to have any effect. I am not sure where to put it actually. I couldn’t find a detailed documentation of the minimize(function).

Hm "doesn't seem to have any effect" and "I'm not sure where to put it" seem at odds.

As always (and for everyone), a short, minimal example that demonstrates the problem is helpful.  I don't know what you're doing, so providing help is challenging.   If we assume you are using the minimize() function, then doing

     result = minimize(objective_function, ....., epsfcn=1.e-5)

should change the step size for the variables.  As suggested earlier, you could print out the parameter values in your objective function to see if the values are indeed changing, and what size that change is.  

Motor resolution is one guess, but there could be other explanations for why the initial changes in value have no effect on the resulting residual (like, are you actually using the values?).    Again, a complete, minimal example is always helpful.

--Matt

[Mehdi Tlili]

Hi Matt,
thanks for your suggestions and sorry for the missing information. What I am trying to do is basically the extrinsic calibration of my lidar sensors relative to the robot. The lidar is mounted in a standard way (no pitch and roll, already tested and calibrated those) to scan horizontally what is in front of the robot.
The way I do it is as follows:
I put my robot inside a nice rectangular shaped enclosed area. Calibration is done for front and rear lidars separately. I first save the scan of the lidar at the initial position.
Then I start rotating the robot n times around its z axis of the robot with an angle increments 2*pi/k . The pose of the robot can then be recovered using two methods:
First, the odometry, which is assumed to be perfect for small and slow rotation with no slip (did some tests there and it looks that this assumption can be used)
Second, using a laser scan matcher. What it basically does is incrementally matching laser scans and updating the lidar's world pose, and with it the robots world's pose (by projecting it, using the lidar's to robot transform that I want to optimize)

The trick is that when the lidar's relative pose to the robot's center is not correct, the laser scan matcher will deliver the true motion of the lidar but when re-projecting and computing the robot's motion, one will get a diverging motion from the one described by odometry (supposed to be perfect).
This divergence will have a maximum at Pi and will come back to zero when doing the full rotation 2Pi (it was the way I could say odometry can be assumed to be perfect). 
This divergence is then used in order to estimate the true pose of the lidar.

The way this is done is by taking the initial laser scan, project it to the two new robot's frames after each rotation: robot_pose from odometry and robot_pose from laser scan matcher.
The optimization is then done to minimize this error by changing the lidar parameters (x, y and rot_z) relative to robot frame (base_link or robot_center).

The problem is that all these three parameters have an effect and contribute to this divergence. Optimizing x and y only always ends up converging but with some still visible error when checking the overlap between front laser and rear laser.
It could also be that a combined movement of rotation and translation would offer more information and constraints for the optimization (I already tested rotating by Pi, driving forwards once and then rotating again Pi).

[Matt Newville]

Hi Mehdi,

OK, yes, I understand that you are trying to optimize a measured signal by moving a few motors of your robot around.   I've done similar things myself a few times, though with very different motors and sensors.   What I am suggesting is that the motor resolution or minimum step size is important, especially compared to the changes the minimization routine will make in trying to find the best solution.  So, ask yourself, what is the smallest step the different axes can actually move?   Also ask yourself the possibly separate question of what is the smallest step the different axes can make for which the signal will actually change?

With that knowledge, PRINT OUT AND READ the values actually sent to the axes from your fitting function.    It is always a good idea to test objective function by sending it different Parameter values yourself and seeing if the output changes as you expect.   When you call your objective function by hand with different Parameter values, does the device move as expected?  Are the signals read correctly and change as you expect?

When that works as you expect, then try it with minimize() calling your objective function.  Again, read the Parameter values that minimize() sends and check if they are large enough to actually alter the measured signal?   If it is not, you'll need to increase epsfcn so that the initial and minimum steps of the Parameter values are large enough to actually cause the response to change.  Otherwise, the fit will never work.

I hope that helps.   But if you are having trouble with the lmfit module (and, why else would you be writing to this list?), always (really, always) include a small script that shows the actual problem with lmfit that you are actually having.

--Matt

 

--
You received this message because you are subscribed to the Google Groups "lmfit-py" group.
To unsubscribe from this group and stop receiving emails from it, send an email to lmfit-py+u...@googlegroups.com.
To post to this group, send email to lmfi...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/lmfit-py/98629ff5-7e07-4c69-a1bc-306e19b54b12%40googlegroups.com.

For more options, visit https://groups.google.com/d/optout.

--

--Matt Newville <newville at cars.uchicago.edu> 630-252-0431

[Mehdi Tlili]

Hi Matt,
yes that is exactly what I am trying to do. To simplify my task I decoupled the variables I want to optimize. I start with the yaw angle of my laser sensors relative to the robot's frame. I have two lasers in my robot that are optimized separately.
I just drive the robot 2 meters forward, and observe a deviation between on one side the supposed to be new laser pose estimated from the robot's motion + laser pose in robot's frame and on the other side the laser's pose estimated by matching the laser scans (true position).
What is really confusing me now is that the optimization works for one of the laser and doesn't for another laser. Where it doesn't work I printed the values and noticed that lmfit does not even try to change the value (using epsfcn=0.0001). This is what I get when I print the parameters and the mean of the error from the residual function (I am interested in optimizing front_laser_rot_z)

OrderedDict([('front_laser_rot_x', 3.141592653589793), ('front_laser_rot_y', -0.0), ('front_laser_rot_z', -0.65644682438400015), ('front_laser_trans_x', 0.39483264551099995), ('front_laser_trans_y', -0.11629361362399998), ('front_laser_trans_z', 0.1125), ('rear_laser_rot_x', 3.141592653589793), ('rear_laser_rot_y', -6.1629758220391534e-33), ('rear_laser_rot_z', 2.2785545254699997), ('rear_laser_trans_x', -0.39811236099900005), ('rear_laser_trans_y', 0.12254920721599999), ('rear_laser_trans_z', 0.1125)])
Residual error :  0.221663666888
OrderedDict([('front_laser_rot_x', 3.141592653589793), ('front_laser_rot_y', -0.0), ('front_laser_rot_z', -0.65644682438400015), ('front_laser_trans_x', 0.39483264551099995), ('front_laser_trans_y', -0.11629361362399998), ('front_laser_trans_z', 0.1125), ('rear_laser_rot_x', 3.141592653589793), ('rear_laser_rot_y', -6.1629758220391534e-33), ('rear_laser_rot_z', 2.2785545254699997), ('rear_laser_trans_x', -0.39811236099900005), ('rear_laser_trans_y', 0.12254920721599999), ('rear_laser_trans_z', 0.1125)])
Residual error :  0.221663666888
OrderedDict([('front_laser_rot_x', 3.141592653589793), ('front_laser_rot_y', -0.0), ('front_laser_rot_z', -0.65644682438400015), ('front_laser_trans_x', 0.39483264551099995), ('front_laser_trans_y', -0.11629361362399998), ('front_laser_trans_z', 0.1125), ('rear_laser_rot_x', 3.141592653589793), ('rear_laser_rot_y', -6.1629758220391534e-33), ('rear_laser_rot_z', 2.2785545254699997), ('rear_laser_trans_x', -0.39811236099900005), ('rear_laser_trans_y', 0.12254920721599999), ('rear_laser_trans_z', 0.1125)])
Residual error :  0.221663666888
OrderedDict([('front_laser_rot_x', 3.141592653589793), ('front_laser_rot_y', -0.0), ('front_laser_rot_z', -0.65644682438400015), ('front_laser_trans_x', 0.39483264551099995), ('front_laser_trans_y', -0.11629361362399998), ('front_laser_trans_z', 0.1125), ('rear_laser_rot_x', 3.141592653589793), ('rear_laser_rot_y', -6.1629758220391534e-33), ('rear_laser_rot_z', 2.2785545254699997), ('rear_laser_trans_x', -0.39811236099900005), ('rear_laser_trans_y', 0.12254920721599999), ('rear_laser_trans_z', 0.1125)])
Residual error :  0.221663666888
[[Variables]]
    front_laser_rot_x:     3.141593 (fixed)
    front_laser_rot_y:    -0 (fixed)
    front_laser_rot_z:    -0.65644682 +/- 0        (-0.00%) (init=-0.6564468)
    front_laser_trans_x:   0.3948326 (fixed)
    front_laser_trans_y:  -0.1162936 (fixed)
    front_laser_trans_z:   0.1125 (fixed)
    rear_laser_rot_x:      3.141593 (fixed)
    rear_laser_rot_y:     -6.162976e-33 (fixed)
    rear_laser_rot_z:      2.278555 (fixed)
    rear_laser_trans_x:   -0.3981124 (fixed)
    rear_laser_trans_y:    0.1225492 (fixed)
    rear_laser_trans_z:    0.1125 (fixed)

Here is my residual function:

def residual(params, master, master_csm_predictions,
             master_initial_points, initial_base_link, base_link_rotations):

    """A functions that computes the residual error of the laser pose from URDF."""
    # Get base_link to laser transform

    base_link_to_laser = CalibUtils.params_to_matrix(master + '_laser', params)
    # Now for each base_link_rotation predict the columns how they would appear in the rotated lasers
    nr_points = master_csm_predictions[0].shape[0]
    nr_predictions = len(master_csm_predictions)
    data_master = np.zeros((nr_predictions * nr_points, 2))
    model_master = np.zeros((nr_predictions * nr_points, 2))
    for idx, (base_link_rotation, master_csm_prediction) in enumerate(
            zip(base_link_rotations, master_csm_predictions)):

        # Pose of laser in the rotated/moved robot
        laser_rotated_pose = (initial_base_link.dot(base_link_rotation)).dot(base_link_to_laser)
        # Initial pose of the laser corresponding to initial_base_link
        laser_initial_pose = initial_base_link.dot(base_link_to_laser)

        # Transform matrix describing the 21motion if the laser translation/rotation

        laser_rotation = np.linalg.inv(laser_initial_pose).dot(laser_rotated_pose)
        # Now project the initially observed columns to predict their appearance in new pose

        projected_columns_observation = (np.linalg.inv(laser_rotation).dot(master_initial_points.T)).T

        # Put the model prediction and the data observation in their respective vectors

        data_master[idx * nr_points:(idx + 1) * nr_points, :] = copy.deepcopy(master_csm_prediction[:, :2])
        model_master[idx * nr_points:(idx + 1) * nr_points, :] = copy.deepcopy(
            projected_columns_observation[:, :2])
    print params.valuesdict()
    print "Residual error : ", np.mean(np.absolute(data_master - model_master))
    return data_master - model_master

data_master and model_master are nx2 vectors and I think lmfit can handle that.
I took this function and looped by myself over a range of angle between starting_value-0.5 and starting_value+0.5 (same as the range given in parameters) then I plotted the error (figure above). As you can see it goes nicely down until the optimal value around -0.77 but lmfit somehow is stuck at -0.65.
I don't have this problem with the other laser which is mounted in the same way on the back of the robot. Any suggestions?

[Mehdi Tlili]

And even weirder, when the error is from the other side (starting at -0.85) it converges with no problems.
Here is the result:

Residual error :  0.00377279291488
[[Variables]]
    front_laser_rot_x:     3.141593 (fixed)
    front_laser_rot_y:     6.162976e-33 (fixed)
    front_laser_rot_z:    -0.77248327 +/- 0.000340 (0.04%) (init=-0.8564468)

    front_laser_trans_x:   0.3948326 (fixed)
    front_laser_trans_y:  -0.1162936 (fixed)
    front_laser_trans_z:   0.1125 (fixed)
    rear_laser_rot_x:      3.141593 (fixed)
    rear_laser_rot_y:     -6.162976e-33 (fixed)

    rear_laser_rot_z:      2.478555 (fixed)

    rear_laser_trans_x:   -0.3981124 (fixed)
    rear_laser_trans_y:    0.1225492 (fixed)
    rear_laser_trans_z:    0.1125 (fixed)

And the error plot I iterate manually looks similar. What might be the cause?

[Matt Newville]

Mehdi,

I cannot reiterate enough the importance of including a small, complete example that shows the problem.   At this point in the conversation, I am more concerned that you are not understanding this advice than about any problem you may be having with your optimization.   Of course, I have no way of knowing what your "CalibUtils.params_to_matrix()" function does.   I also don't know how you called your objective function or defined your parameters.   Why are you not telling us this? 

As I said several times now, you probably need to adjust epsfcn for your response function to work.   Again: adjust epsfcn and print out the values of the changing parameters to ensure that they actually change enough to cause your a change in your response function.  I can't give more advice than that.

--Matt