From 49374a43237d27e3e5e6c60a5adac0ce31b9f919 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Fri, 14 Jun 2019 12:37:06 +0200
Subject: [PATCH] added direct rho estimation

---
 src/msckf_vio.cpp | 29 +++++++++++++++++++++++++++--
 1 file changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index acafb60..1e8f4f2 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -1410,6 +1410,30 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   if(PRINTIMAGES)
   {
+
+  ofstream myfile;
+  myfile.open("/home/raphael/dev/octave/log2octave");
+  myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
+         << "# name: Hx\n"
+         << "# type: matrix\n"
+         << "# rows: " << H_xi.rows() << "\n"
+         << "# columns: " << H_xi.cols() << "\n"
+         << H_xi << endl;
+
+  myfile << "# name: Hy\n"
+         << "# type: matrix\n"
+         << "# rows: " << H_yi.rows() << "\n"  
+         << "# columns: " << H_yi.cols() << "\n"
+         << H_yi << endl;
+
+
+  myfile << "# name: r\n"
+         << "# type: matrix\n"
+         << "# rows: " << r_i.rows() << "\n"  
+         << "# columns: " << 1 << "\n"
+         << r_i << endl;
+
+  myfile.close();
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
   }
@@ -1548,6 +1572,7 @@ void MsckfVio::featureJacobian(
   myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
   myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
   myfile.close();
+
   cout << "---------- LOGGED -------- " << endl; 
   nh.setParam("/play_bag", false);
 
@@ -1711,7 +1736,7 @@ void MsckfVio::removeLostFeatures() {
         invalid_feature_ids.push_back(feature.id);
         continue;
       } else {
-        if(!feature.initializePosition(state_server.cam_states)) {
+        if(!feature.initializeRho(state_server.cam_states)) {
           invalid_feature_ids.push_back(feature.id);
           continue;
         }
@@ -1881,7 +1906,7 @@ void MsckfVio::pruneCamStateBuffer() {
           feature.observations.erase(cam_id);
         continue;
       } else {
-        if(!feature.initializePosition(state_server.cam_states)) {
+        if(!feature.initializeRho(state_server.cam_states)) {
           for (const auto& cam_id : involved_cam_state_ids)
             feature.observations.erase(cam_id);
           continue;