Lidar objective function - simple optimization tests

[miguelriemoliveira]

Try to figure out if the current cost function for 2D lasers is working and sufficient.

Right now we have the following residuals:

• extrema points
• beam (orthogonal)
• inner points

Simplest test: only right laser as sensor, anchored sensor right laser, only one collection.

There seems to be some problem since the chessboard should move up to get less score from the inner points ... will investigate.

[miguelriemoliveira]

Hi @afonsocastro,

I need your help. The laser orthogonal distance is returning negative values (see figure bellow). I know you and @eupedrosa had a big discussion on whether we could or could not have the errors in absolute. What was the result of that discussion? Residuals can be absolute, right?

Residuals:
                          error
5_right_laser_inner_0  0.250932
5_right_laser_inner_1  0.026654
5_right_laser_inner_2  0.033147
5_right_laser_inner_3  0.020602
5_right_laser_inner_4  0.010148
5_right_laser_inner_5  0.038313
5_right_laser_inner_6  0.000932
5_right_laser_inner_7  0.045203
5_right_laser_inner_8  0.029275
5_right_laser_inner_9  0.357613
5_right_laser_beam_0  -0.280164
5_right_laser_beam_1   0.060513
5_right_laser_beam_2  -0.007723
5_right_laser_beam_3   0.085132
5_right_laser_beam_4  -0.010917
5_right_laser_beam_5   0.014135
5_right_laser_beam_6   0.050294
5_right_laser_beam_7   0.127568
5_right_laser_beam_8   0.015961
5_right_laser_beam_9  -0.085599
5_right_laser_beam_10 -0.046741
5_right_laser_beam_11  0.063330
5_right_laser_beam_12  0.074628
5_right_laser_beam_13  0.077165
5_right_laser_beam_14 -0.049046
5_right_laser_beam_15 -0.133990
5_right_laser_beam_16 -0.037653
5_right_laser_beam_17  0.141129
5_right_laser_beam_18 -0.060240
5_right_laser_beam_19  0.149856
5_right_laser_beam_20  0.071439
5_right_laser_beam_21 -0.195469
5_right_laser_beam_22  0.019157
5_right_laser_beam_23  0.055342
5_right_laser_beam_24  0.003112
5_right_laser_beam_25  0.024284
5_right_laser_beam_26  0.065795
5_right_laser_beam_27  0.069174
5_right_laser_beam_28 -0.045543
5_right_laser_beam_29 -0.098317
5_right_laser_beam_30 -0.229107
5_right_laser_beam_31 -0.017872
5_right_laser_beam_32  0.021338
5_right_laser_beam_33  0.090481
5_right_laser_beam_34 -0.097897
5_right_laser_beam_35  0.166265
5_right_laser_beam_36  0.003030
5_right_laser_beam_37 -0.007535
5_right_laser_beam_38 -0.032527
5_right_laser_beam_39  0.068290
5_right_laser_beam_40 -0.077855

[miguelriemoliveira]

Redesigned the inner points distance (there are some things about Afonso's implementation I did not understand.

Tried several stuff to get better results:

• Using only the vertical inner points
• Using absolute beam distance
• Using several collections
• Discarding leftmost and rightmost detected inner points, which are often outliers

Still cannot converge to a satisfactory result. I believe this funcion has local minima because of the image bellow:

The fact is the first guess is very wrong ... that could help. But I would still like to have a better objective function for lidars ...

[miguelriemoliveira]

For collection 3 converges to this ... strange

[eupedrosa]

So many things to discuss. @miguelriemoliveira, what a productive Christmas.

The residuals can be negative as long as the derivative is in agreement. We are using numerical differentiation so, no problem there.

If I am not mistaken, the laser labels are not validated, right? We could fit a line to make sure it is a line. Then, we could use that line to interpolate points or even extrapolate the edge points. In a way, the optimization already tries to fit the points to a line in the chess plane.

[miguelriemoliveira]

Hi @eupedrosa , thanks, but the holiday season is not over yet : -).

The residuals can be negative as long as the derivative is in agreement. We are using numerical differentiation so, no problem there.

For me it would be much easier to use the absolute value ... is that ok?

If I am not mistaken, the laser labels are not validated, right? We could fit a line to make sure it is a line. Then, we could use that line to interpolate points or even extrapolate the edge points. In a way, the optimization already tries to fit the points to a line in the chess plane.

That's a good idea, and perhaps an alternative to #110. Can you please create an issue for that?

[eupedrosa]

For me it would be much easier to use the absolute value ... is that ok?

Yes, it is ok.

That's a good idea, and perhaps an alternative to #110. Can you please create an issue for that?

I will open the issue.

[miguelriemoliveira]

Thanks.

[miguelriemoliveira]

Still the same problem.

Reduced the complexity of the problem:

• Only using residual extrema left (not extrema right)
• Not using inner residuals
• Tried without beam residuals (same result)

The solution never converges to the left limit of the chessboard ... bug bug ...

Tomorrow I will study this, even if I have to draw lines to the selected points in the limits.

[miguelriemoliveira]

Found the problem ... the limit points also contain the point 0,0. In this image the magenta capsules are all the limit points. 0,0 is also there!

This cause the optimization to converge to positioning the left extrema at 0,0

[miguelriemoliveira]

Without the limit point at 0,0 now it converges as it should ...

[miguelriemoliveira]

@afonsocastro this was a nasty bug! You owe me a coffee!

[miguelriemoliveira]

OK, so now I am working on getting the optimization to converge with multiple collections. Using the lidar only it seems to work perfectly, e.g. for 3 collections:

Average cost of laser is 0.6 cm.

Avg residuals per msg_type:
 {'Image': nan, 'LaserScan': 0.0064529082363795169}

However, if I add the top_left_camera to the optimization and anchor it, the optimization does not work the same way. The optimization converges to this:

and the avg error is about 10 times more (9 cm)

Avg residuals per msg_type: {'Image': 0.38787829849551864, 'LaserScan': 0.091845670421972253}

[miguelriemoliveira]

So what does it mean? Adding the camera made the optimization loose a degree of freedom. Since the camera is anchored, the pose of the chessboards is also fixed, or the residual related with the camera will rise.

But 9 centimeters error is too much, there should a better solution ... I am confused ...

Could it be that the intrinsic parameters have to be there? Could a wrong fx or fy cause that a good solution does not exist?

[miguelriemoliveira]

Using hte intrinsics does not help at all ...

What is strange about these results is that from the image above and the dark blue collection, it seems that the laser must move back (negative xx) to make the measurements better fit the chessboard, but this image:

shows that the other two collections (brown and cyan) need the laser to move forward (x positive) to put the measurements of the chessboard.

So I would say the algorithm is doing what is expected, and converges to a middles ground in an attempt to reduce the all the residuals ... but what does this mean?

[eupedrosa]

Did you try the optimization without optimizing the intrinsic?

[miguelriemoliveira]

Here is the dataset I am using.

dataset_23_dez_2019.tar.gz

The sparse matrix is displayed here (but you can run the code and take a look at the sparse_matrix.csv file)

https://sheet.zoho.com/sheet/editor.do?doc=a368b6008820e647005921abac8e7fe1aab7928580538b23abd66c31a19916a808e29126208662b3270e97684b916c97f310b1aba00b4988b0feba778bd92d99

I run only with 3 collections, i.e.:

roslaunch interactive_calibration atlascar2_view_optimization.launch read_first_guess:=false read_optimized:=false

test/sensor_pose_json_v2/main.py -json ~/datasets/dataset_23_dez_2019/data_collected.json -ssf "lambda name: name not in ['left_laser','top_right_camera']" -csf "lambda x: int(x) in [1,11,22]" -vo -si

[miguelriemoliveira]

Using orthogonal error instead of beam resulted in the same problem ...

[miguelriemoliveira]

Dividing all image residuals by 100, e.g. this code on 212 of objective_function.py

local_residuals = [x/100 for x in local_residuals]

gets good results:

Residuals are 3 pixels (must multiply back by 100) and 0.6 cm average

Avg residuals per msg_type:
 {'Image': 0.029769675054920747, 'LaserScan': 0.0067155425162651059}

104 (ad hoc weights) made it work ... will try with many collections ...

[miguelriemoliveira]

I think it also worked using many collections

Residuals are similar, 3 pixels and 0.7 cms (around 25 collections)

Avg residuals per msg_type: {'Image': 0.03488272638259074, 'LaserScan': 0.0074347133892096523}

And what's better, the height of the laser scans seems reasonable. We are not using inner points residuals here ...

[miguelriemoliveira]

So I guess we should explore #104 . Next I should try with a reasonable first guess (including of the anchored sensor) to see if the pose of the laser w.r.t the car chassis makes any sense.

[miguelriemoliveira]

Using inner residuals the av errors are similar, the result places the laser beam much higher in the chessboard, which seems more realistic ...

I am not sure this test about using the inner points is conclusive ...

[miguelriemoliveira]

Closing this issue. Conclusions taken from all these tests:

• inner points, beam, orthogonal (not used), and extrema all seem to be working as intended (refactored some code by @afonsocastro )
• Chessboard poses inferred by the cameras are not consistent with those from the lidars (top_left_camera and right_laser tests). This inconsistency makes it so that either the residuals of the cameras or the one of the lidars increases. We must balance this compromise, probably using weights (#104).
  • Still not clear why this inconsistency occurs, camera's intrinsics or problem with dataset .