[SfM] Major bug fix on BlockOrder in BACeres

[0xfaded]

Ceres accepts a ParameterBlockOrdering hint which must assign
every parameter block to exactly one group.

BundleAdjustmentCeres can be called multiple times, and on each
call resetProblem() is called internally to clear the parameter blocks.

However, resetProblem() did not clear the ParameterBlockOrdering.
If the BundleAdjustmentCeres object was used more than once,
Ceres would fail with an error.

In practice, the BundleAdjustmentCeres object is used in a loop that
repeatedly calls adjust() while removing outliers on each iteration.

If the first iteration succeeded with no outliers, the loop would
correctly exit. However, if any outliers are present, the entire
bundle adjustment routine fails leading to the warning:

"Bundle Adjustment failed, the solution is not usable."

This incorrect Ceres usage seems to have been the main cause
of bundle adjustment failures. The error given by Ceres was:

FAILURE (The program has 3855 parameter blocks, but the parameter block
ordering has 7621 parameter blocks.)

Includes fix for rig error functor which was previously unreachable due
to bug. Without fix, rigs cause SfM to crash.

Description

Features list

Implementation remarks

[fabiencastan]

As pointed:
// AngleAxisRotatePoint: Inplace rotation is not supported. pt and result must point to different memory locations, otherwise the result will be undefined.
https://github.com/ceres-solver/ceres-solver/blob/5cb5b35a930c1702278083c75769dbb4e5801045/include/ceres/rotation.h#L217

[fabiencastan]

Thanks for this fix. This is a very nice contribution!