Draft: Add Map Database + covisibility query

TODOs:

• Run mapper in a different thread
• Queues to receive the lmdb from the VIO thread
• Queues to request and send covisibility map
• Show the Map in the UI (synchronized with the t_ns)
• Show map covisibility and observations
• Print LMDB for debug info
• Implement “traditional” covisibility query (bad performance)
• Check LMDBs (pointers, copies, etc)
• Add “strategy” to mergeLMDB to be able to override (or not) data
• Improve performance covisibility query
• Allow to add and remove data from the LMDB window
  • Remove submap added by covisibility query
  • Remove zero marg data columns and rows
• Make MapDatabase a templated class
• Implement paper covisibility query (be able to choose which one to use)
• Mapper optimizer thread to process marginalization data. This implies:
  • Add ORB descriptors when detecting in front-end
  • Adapt match_all code
  • Perform build_tracks
  • Adapt setup_opt just to add observations to the map
  • Globally optimize the map. This could be done with the method optimize(). Should be simple to adapt to the new “vio” mapper but without the above steps it is not worth it.

BUGs:

• When run with vio_covisibility_map_enable=True:
  ***** Assertion (size_t(mld.H.cols()) == mld.order.total_size) failed in void basalt::BundleAdjustmentBase::computeMargPriorError(const basalt::MargLinData_>&, basalt::BundleAdjustmentBase_>::Scalar&) const [with Scalar_ = float; basalt::BundleAdjustmentBase_>::Scalar = float]: /home/bruno/tesina/basalt/src/vi_estimator/ba_base.cpp:450: Aborted (core dumped)