add build_node_hierarchy from keypoint_sort

[jonahpearl]

Remove dependency on the old keypoint_sort library. Are there other funcs we should grab from there?