fixed anchor initialization and added image resizing for incomming into msckf

include/msckf_vio/feature.hpp

@@ -823,7 +823,11 @@ bool Feature::VisualizePatch(
   cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 
-  cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
+  cv::Point2f p_f;
+  if(cam.id == 0)
+    p_f = cv::Point2f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
+  else if(cam.id == 1)
+    p_f = cv::Point2f(observations.find(cam_state_id)->second(2),observations.find(cam_state_id)->second(3));
   // move to real pixels
 
   p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
@@ -1042,8 +1046,9 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 
   auto anchor = observations.begin();
   if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
+  {
     return false;
-
+  }
   cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
   cv::Mat anchorImage_deeper;
   anchorImage.convertTo(anchorImage_deeper,CV_16S);
@@ -1066,17 +1071,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   
 
   // check if image has been pre-undistorted
-  if(cam.distortion_model.substr(0,3) == "pre-")
+  if(cam.distortion_model.substr(0,3) == "pre")
   {
-    std::cout << "is a pre" << std::endl;
     //project onto pixel plane
     undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
-
     // create vector of patch in pixel plane
     for(double u_run = -n; u_run <= n; u_run++)
       for(double v_run = -n; v_run <= n; v_run++)
         anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
 
+
     //project back into u,v
     for(int i = 0; i < N*N; i++)
       anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));

launch/msckf_vio_euroc.launch

@@ -19,11 +19,11 @@
 
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="1"/>
+      <param name="FILTER" value="2"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="3"/>

src/msckf_vio.cpp

@@ -425,6 +425,10 @@ void MsckfVio::imageCallback(
   // the origin.
   if (is_first_img) {
     is_first_img = false;
+    cam0.intrinsics *=0.5;
+    cam1.intrinsics *=0.5;
+    cam0.resolution *=0.5;
+    cam1.resolution *=0.5;
     state_server.imu_state.time = feature_msg->header.stamp.toSec();
   }
   static double max_processing_time = 0.0;
@@ -546,12 +550,26 @@ void MsckfVio::manageMovingWindow(
 
   cv::Mat new_cam0;
   cv::Mat new_cam1;
+  cam0.intrinsics *=2;
+  cam1.intrinsics *=2;
+  cam0.resolution *=2;
+  cam1.resolution *=2;
   image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
   image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  cam0.intrinsics *=0.5;
+  cam1.intrinsics *=0.5;
+  cam0.resolution *=0.5;
+  cam1.resolution *=0.5;
+  //resize
+
+  cv::Mat new_cam02;
+  cv::Mat new_cam12;
+  cv::resize(new_cam0, new_cam02, cv::Size(), 0.5, 0.5);
+  cv::resize(new_cam1, new_cam12, cv::Size(), 0.5, 0.5);
 
   // save image information into moving window
-  cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone();
+  cam0.moving_window[state_server.imu_state.id].image = new_cam02.clone();
-  cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone();
+  cam1.moving_window[state_server.imu_state.id].image = new_cam12.clone();
 
   cv::Mat xder;
   cv::Mat yder;
@@ -576,14 +594,15 @@ void MsckfVio::manageMovingWindow(
   yder/=96.;
 
 
-  /*
+/*  
   cv::Mat norm_abs_xderImage;
   cv::convertScaleAbs(xder, norm_abs_xderImage);
   cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
 
   cv::imshow("xder", norm_abs_xderImage);
   cvWaitKey(0);
-  */
+*/
+
   // save into moving window
   cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); 
   cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
@@ -1705,9 +1724,6 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
   dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
 
-  cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
-  cout << dI_dhj(1, 2) << ", " << dI_dhj(1, 3) << endl;
-
   // add jacobian
   
   //dh / d{}^Cp_{ij}
@@ -1784,8 +1800,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
     return false;
 
-  cout << "-\n" << r_photo << "-" << endl;
-
   //cout << "r\n" << r_photo  << endl;
   // calculate jacobian for patch
   int count = 0;
@@ -1929,7 +1943,9 @@ bool MsckfVio::PhotometricFeatureJacobian(
 
     // skip observation if measurement is not valid
     if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
+    {
       continue;
+    }
 
     // set size of stacking jacobians, once the returned jacobians are known
     if(first)