Support for initial orientation

[whoenig]

Currently, we try to automatically infer yaw (N_YAW=20), which can cause issues for symmetric marker configurations. Instead, we should use the known initial pose. This affects both the Pose mode and Hybrid mode.

On the way, the code should be cleaned up:

1. Use radius search, rather than k-search + manual filtering;
2. The initial guess should include the initial pose, not just position (e.g., to support very long rigid bodies properly).