Camera pose Optimization (first try)
split camera array in two: camera_rot and camera_trans
more conversion functions: kdl2cv and serialize
print_array in auxiliar.h
Added deserialization
Using transformed points in the frame of the first camera intead of the Checkerboard frame
Added computeReprojectionErrors for double*
more serialize/deserialize used in the optimization process
Testing calc_error. There is a problem with the quaternions calculation in KDL (I should use angle axis instead as a work around)
Fixed order for quaternions: w, x, y, z
Corrected version of calc_error. ToDo: fix optimization process
Corrected the transformation: T.inverser() * T0
Publish fixed transforms in the same moment (when the urdf is updated it unexpected results)
Stop timer before updating tree
Renamed function to get/setUrdfPose, and move implementation to .cpp file
Corrected getFk comment