Apriltag: some improvements/changes

This MR has changes in:

• factor_autodiff_apriltag.h: The operator()was rewritten to remove some temporary variables and copies.
• gtest_factor_autodiff_apriltag.cpp: New test with non-zero values for extrinsics and frame pose. Also removed some wrong addConstrainedBy().
• processor_tracker_landmark_apriltag.cpp: After playing with the demo, the only change that remained is not providing features without orientation in detectNewFeatures() to avoid potential wrong frame and landmark initialization in future calls to reestimateLastFrame(). It could be implemented using a bool option in the processor parameters.
• processor_tracker.cpp a small bug fix also contained in !257 (merged).

TO DO: Ensure the proper frame coordinates for the error in FactorAutodiffApriltag::operator() to deal with covariance properly.

@mederic_fourmy, merge the current branch if you consider the contained changes are of your interest.

added enhancement label

Here you discard features which uncertain rotation measurement while we were just adding them with a big covariance on the rotation part since the translational part is still valuable information. Is it what is intended?

The idea behind this is the following: We are in ProcessNew(), so new features (first time the landmark is observed) without good are not provided if we don't have rotation information. This is because of a wrong orientation initialization eventually could produce a bad frame initialization in reestimateLastFrame() if the corresponding landmark is used to compute the frame pose.

It was a result of some online demo tests.. sometimes the initialization of some frames is very wrong and the estimation converges very slow. I think that it is because of the bug detected in processor_tracker.cpp but this could also be a problem.

This could be enabled/disabled by a boolean parameter in the processor.

I think that the initialization of landmarks and frames is still an open issue in this application (if we don't have any ProcessorMotion). Two comments about this:

The uncertain rotation issue, should be taken into account in reestimateLastFrame(). To do that, a flag in LandmarkApriltag should be added (and the associated machinery) to identify the landmarks that have any feature with rotation information. Then, the ones that haven't are not considered to reestimate the frame in last.

Also, recently created landmarks shouldn't be considered as well, since they are going to be reinitialized according to this last frame estimation.

merged