Merge branch '19-building-a-new-visual-odometry-system' of...

Merge branch '19-building-a-new-visual-odometry-system' of https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/vision into 19-building-a-new-visual-odometry-system

include/vision/factor/factor_pixel_hp.h

@@ -131,10 +131,10 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
     using namespace Eigen;
 
     // frames w: world; r: robot; c: camera
-    Matrix<T, 3, 1> p_wr(_frame_p);
-    Quaternion<T>   q_wr(_frame_o);
-    Matrix<T, 3, 1> p_rc(_sensor_p);
-    Quaternion<T>   q_rc(_sensor_o);
+    Map<const Matrix<T, 3, 1> > p_wr(_frame_p);
+    Map<const Quaternion<T> >   q_wr(_frame_o);
+    Map<const Matrix<T, 3, 1> > p_rc(_sensor_p);
+    Map<const Quaternion<T> >   q_rc(_sensor_o);
 
     // camera pose in world frame: transforms from camera to world
     Matrix<T, 3, 1> p_wc = p_wr + q_wr * p_rc;

src/processor/processor_visual_odometry.cpp

@@ -133,10 +133,6 @@ void ProcessorVisualOdometry::preProcess()
         capture_image_incoming_->setTracksOrigin(tracks_init);
         capture_image_incoming_->setTracksPrev(tracks_init);
 
-        // print a bar with the number of active tracks in incoming
-        std::string s(capture_image_incoming_->getKeyPoints().size(), '#');
-        WOLF_INFO("TRACKS: ", capture_image_incoming_->getKeyPoints().size(), " ", s);
-
         auto __attribute__((unused)) dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
         WOLF_DEBUG( "dt_preprocess (ms): " , dt_preprocess );
 
@@ -310,11 +306,6 @@ void ProcessorVisualOdometry::preProcess()
     capture_image_incoming_->setTracksOrigin(tracks_origin_incoming);
 
     auto __attribute__((unused)) dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
-
-    // print a bar with the number of active tracks in incoming
-    std::string s(capture_image_incoming_->getKeyPoints().size(), '#');
-    WOLF_INFO("TRACKS: ", capture_image_incoming_->getKeyPoints().size(), " ", s);
-
     WOLF_DEBUG( "dt_preprocess (ms): " , dt_preprocess );
 
     return;
@@ -480,16 +471,16 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
 
     Eigen::Vector3d point3d;
     point3d = pinhole::backprojectPoint(
-    		getSensor()->getIntrinsic()->getState(),
-    		(std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
-			point2d);
+            getSensor()->getIntrinsic()->getState(),
+            (std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
+            point2d);
 
-    //double distance = params_bundle_adjustment_->distance; // arbitrary value
+    // double distance = params_bundle_adjustment_->distance; // arbitrary value
     double distance = 1;
     Eigen::Vector4d vec_homogeneous_c;
     vec_homogeneous_c = {point3d(0),point3d(1),point3d(2),point3d.norm()/distance};
 
-    //TODO: lmk from camera to world coordinate frame.
+    // lmk from camera to world coordinate frame.
     Transform<double,3,Isometry> T_w_r
         = Translation<double,3>(feat_pi->getFrame()->getP()->getState())
         * Quaterniond(_feat->getFrame()->getO()->getState().data());
@@ -533,6 +524,16 @@ void ProcessorVisualOdometry::postProcess()
             ++track_it;
     }
 
+    // print a bar with the number of active tracks in incoming
+    unsigned int n = capture_image_incoming_->getKeyPoints().size();
+    std::string s(n/2, '#');
+    WOLF_INFO("TRACKS: ", n, " ", s);
+
+    n = getProblem()->getMap()->getLandmarkList().size();
+    s = std::string(n/2, '-');
+    WOLF_INFO("LMARKS: ", n, " ", s);
+
+
 }
 
 bool ProcessorVisualOdometry::voteForKeyFrame() const