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.

src/solver/solver_manager.cpp

@@ -79,8 +79,6 @@ void SolverManager::update()
     // UPDATE STATE BLOCKS (state, fix or local parameterization)
     for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
     {
-        if (state_blocks_.find(state_ptr)==state_blocks_.end())
-            continue;
         assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
 
         // state update
@@ -108,8 +106,8 @@ void SolverManager::update()
         }
     }
 
-    //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
-    //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
+    assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
+    assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
 }
 
 wolf::ProblemPtr SolverManager::getProblem()