Consolidated multiple maps that tracked source cartesian points with related info and maps that tracked joints with vertices.
Consolidated multiple maps that tracked source cartesian points with related info and maps that tracked joints with vertices.
Converted couts to logs.
Converted params to reference instead of pointers
Added constructor with RobotModel. Replaced random weight generator with a simple linear weight calculation between vectors.