From 1949e4c43d4a8824b1592ba8cf40b800385aa325 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Fri, 19 Apr 2019 13:30:30 +0200
Subject: [PATCH] removed visu as result works so to not clutter the output

---
 include/msckf_vio/feature.hpp | 31 ++++++++++++++-----------------
 src/msckf_vio.cpp             | 18 +++++++++++++-----
 2 files changed, 27 insertions(+), 22 deletions(-)

diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index 7c8d803..7702ddc 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -384,8 +384,10 @@ bool Feature::estimate_FrameIrradiance(
   double b_A = 0;
   double a_l =frameExposureTime_ms;
   double b_l = 0;
-  printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
-  printf("exposure: %f, %f\n", a_A, a_l);
+  
+  //printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
+  //printf("exposure: %f, %f\n", a_A, a_l);
+  
   for (double anchorPixel : anchorPatch)
   {
     float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
@@ -400,26 +402,24 @@ bool Feature::FrameIrradiance(
                   CameraCalibration& cam0,
                   std::vector<float>& anchorPatch_measurement) const
 {
-  // project every point in anchorPatch_3d.
-  // int count = 0;
-  cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
+
+  // visu
+  /*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
   cv::Mat dottedFrame(current_image.size(), CV_8UC3);
   cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
+  */
 
+  // project every point in anchorPatch_3d.
   for (auto point : anchorPatch_3d)
   {
-    // testing
-    //if(cam_state_id == observations.begin()->first)
-      //if(count == 4)
-        //printf("\n\ncenter:\n");
 
     cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
 
-    //visualization of feature
-    cv::Point xs(p_in_c0.x, p_in_c0.y);
+    // visu
+    /*cv::Point xs(p_in_c0.x, p_in_c0.y);
     cv::Point ys(p_in_c0.x, p_in_c0.y);
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
-
+  */
     float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image);
     anchorPatch_measurement.push_back(irradiance);
     
@@ -432,11 +432,11 @@ bool Feature::FrameIrradiance(
 
   //visu
   //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
-  if(cam0.featureVisu.empty())
+  /*if(cam0.featureVisu.empty())
     cam0.featureVisu = dottedFrame.clone();
   else
     cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
-
+  */
 
 }
 
@@ -547,10 +547,7 @@ bool Feature::initializeAnchor(
   for(auto point : vec)
   {
     if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
-    {
-      printf("no good\n");
       return false;
-    }
   }
   for(auto point : vec)
     anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 49b4d08..70e167c 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -977,7 +977,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
   r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
       p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
 
-  printf("-----\n");
+  // visu
+  //printf("-----\n");
 
   //estimate photometric measurement
   std::vector<float> estimate_photo_z;
@@ -988,9 +989,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
   for(int i = 0; i < photo_z.size(); i++)
     photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
 
-
-  for(int i = 0; i < photo_z.size(); i++)
-    printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
+  // visu
+  //for(int i = 0; i < photo_z.size(); i++)
+  //  printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
 
   photo_z.clear();
   return;
@@ -1022,9 +1023,12 @@ void MsckfVio::PhotometricFeatureJacobian(
   VectorXd r_j = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
-  printf("_____FEATURE:_____\n");
 
+  // visu
+  /*
+  printf("_____FEATURE:_____\n");
   cam0.featureVisu.release();
+  */
   for (const auto& cam_id : valid_cam_state_ids) {
 
     Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
@@ -1042,6 +1046,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     r_j.segment<4>(stack_cntr) = r_i;
     stack_cntr += 4;
   }
+  // visu
+  /*
   if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000)
     imshow("feature", cam0.featureVisu);
   cvWaitKey(1);
@@ -1053,6 +1059,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     imwrite(ss.str(), cam0.featureVisu);
     image_save_time = ros::Time::now();
   }
+  */
+
   // Project the residual and Jacobians onto the nullspace
   // of H_fj.
   JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);