Implement Lidar data cost function

[miguelriemoliveira]

FYI @afonsocastro For now just testing with the current cost function to see if it makes sense ...

[miguelriemoliveira]

Hi @afonsocastro ,

I made the following test: built a json file with a bad first guess [images are not very nice but for that we have #50 ].

then ran an optimization with all sensors and about 40 collections. the goal was to see if the pose of the lidars converged to the real pose. Results are these:

Bottom line, I am not sure this is working, the Lasers did not converge to the propper place did they?

[miguelriemoliveira]

... what else could we try? An optimization without cameras? not sure it would / should provide more information ... any ideas ?

[eupedrosa]

Maybe we should try an optimization without cameras and see what happens. The lasers may not align on the z-axis but they should align on xy-axis.

[miguelriemoliveira]

Try out new orthogonal residuals for all the points

[miguelriemoliveira]

Example of launchig the code

test/sensor_pose_json_v2/main_ros_visualization.py -json ~/datasets/calibration_26nov2019/data_collected.json -cradius .5 -csize 0.101 -cnumx 9 -cnumy 6 -vo -si -csf "lambda x: not str(x) in ['5', '6', '7', '12', '13', '14', '15'] and int(x)<7" -ssf "lambda name: name=='top_left_camera' or name=='right_laser'"

[afonsocastro]

Done