added tum launch files, removed anchor procedure being called multiple times through a flag

config/camchain-imucam-tum.yaml

@@ -0,0 +1,36 @@
+cam0:
+  T_cam_imu:
+      [-0.9995378259923383,   0.02917807204183088,  -0.008530798463872679, 0.047094247958417004,
+      0.007526588843243184,   -0.03435493139706542, -0.9993813532126198,   -0.04788273017221637,
+      -0.029453096117288798,  -0.9989836729399656,  0.034119442089241274,  -0.0697294754693238,
+      0.0,                    0.0,                  0.0,                   1.0]
+  camera_model: pinhole
+  distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
+    -0.001662284667857643]
+  distortion_model: equidistant
+  intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506]
+  resolution: [1024, 1024]
+  rostopic: /cam0/image_raw
+cam1:
+  T_cam_imu:
+    [-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335,
+    0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889,
+    -0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645,
+    0.0, 0.0, 0.0, 1.0]
+  T_cn_cnm1:
+    [0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335,
+    0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977,
+    0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572,
+    0.0, 0.0, 0.0, 1.0]
+  camera_model: pinhole
+  distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
+    -0.002347858896562788]
+  distortion_model: equidistant
+  intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
+  resolution: [1024, 1024]
+  rostopic: /cam1/image_raw
+T_imu_body:
+  [1.0000, 0.0000, 0.0000, 0.0000,
+  0.0000, 1.0000, 0.0000, 0.0000,
+  0.0000, 0.0000, 1.0000, 0.0000,
+  0.0000, 0.0000, 0.0000, 1.0000]

include/msckf_vio/feature.hpp

@@ -59,11 +59,11 @@ struct Feature {
 
   // Constructors for the struct.
   Feature(): id(0), position(Eigen::Vector3d::Zero()),
-    is_initialized(false) {}
+    is_initialized(false), is_anchored(false) {}
 
   Feature(const FeatureIDType& new_id): id(new_id),
     position(Eigen::Vector3d::Zero()),
-    is_initialized(false) {}
+    is_initialized(false), is_anchored(false) {}
 
   /*
    * @brief cost Compute the cost of the camera observations
@@ -217,7 +217,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
   // A indicator to show if the 3d postion of the feature
   // has been initialized or not.
   bool is_initialized;
-
+  bool is_anchored;
   // Noise for a normalized feature measurement.
   static double observation_noise;
 
@@ -380,6 +380,8 @@ 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);
   for (double anchorPixel : anchorPatch)
   {
     float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
@@ -396,6 +398,8 @@ bool Feature::FrameIrradiance(
                   std::vector<float>& anchorPatch_measurement) const
 {
   //project every point in anchorPatch_3d.
+  if(!is_anchored)
+    printf("not anchored!\n");
   for (auto point : anchorPatch_3d)
   {
     cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); 
@@ -471,12 +475,19 @@ bool Feature::initializeAnchor(
   cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image;
   auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2];
   auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3];
-
+  printf("initializing anchor\n");
+  if(u - n < 0 || u + n >= cam.resolution(0) || v - n < 0 || v + n >= cam.resolution(1))
+  {
+    printf("no good: \n");
+    printf("%f, %f\n", u, v);
+    return false;
+  }
   //for NxN patch pixels around feature
   for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1)
   {
     for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1)
     {
+      printf("ADDING\n");
       // add irradiance information
       anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)u_run,(int)v_run));
 
@@ -489,8 +500,8 @@ bool Feature::initializeAnchor(
       anchorPatch_3d.push_back(Npose);
     }
   }
-
+  printf("set to true!!!\n");
-  //TODO test if NxN patch can be selected
+  is_anchored = true;
   return true;
 }
 

launch/image_processor_tum.launch

@@ -0,0 +1,34 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="image_processor"
+      args="standalone msckf_vio/ImageProcessorNodelet"
+      output="screen"
+       >
+
+      <rosparam command="load" file="$(arg calibration_file)"/>
+      <param name="grid_row" value="4"/>
+      <param name="grid_col" value="4"/>
+      <param name="grid_min_feature_num" value="3"/>
+      <param name="grid_max_feature_num" value="4"/>
+      <param name="pyramid_levels" value="3"/>
+      <param name="patch_size" value="15"/>
+      <param name="fast_threshold" value="10"/>
+      <param name="max_iteration" value="30"/>
+      <param name="track_precision" value="0.01"/>
+      <param name="ransac_threshold" value="3"/>
+      <param name="stereo_threshold" value="5"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+    </node>
+  </group>
+
+</launch>

launch/msckf_vio_tum.launch

@@ -0,0 +1,64 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="fixed_frame_id" default="world"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_tum.launch">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="calibration_file" value="$(arg calibration_file)"/>
+  </include>
+
+  <!-- Msckf Vio Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="vio"
+      args='standalone msckf_vio/MsckfVioNodelet'
+      output="screen">
+
+      <!-- Calibration parameters -->
+      <rosparam command="load" file="$(arg calibration_file)"/>
+
+      <param name="publish_tf" value="true"/>
+      <param name="frame_rate" value="20"/>
+      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
+      <param name="child_frame_id" value="odom"/>
+      <param name="max_cam_state_size" value="20"/>
+      <param name="position_std_threshold" value="8.0"/>
+
+      <param name="rotation_threshold" value="0.2618"/>
+      <param name="translation_threshold" value="0.4"/>
+      <param name="tracking_rate_threshold" value="0.5"/>
+
+      <!-- Feature optimization config -->
+      <param name="feature/config/translation_threshold" value="-1.0"/>
+
+      <!-- These values should be standard deviation -->
+      <param name="noise/gyro" value="0.005"/>
+      <param name="noise/acc" value="0.05"/>
+      <param name="noise/gyro_bias" value="0.001"/>
+      <param name="noise/acc_bias" value="0.01"/>
+      <param name="noise/feature" value="0.035"/>
+
+      <param name="initial_state/velocity/x" value="0.0"/>
+      <param name="initial_state/velocity/y" value="0.0"/>
+      <param name="initial_state/velocity/z" value="0.0"/>
+
+      <!-- These values should be covariance -->
+      <param name="initial_covariance/velocity" value="0.25"/>
+      <param name="initial_covariance/gyro_bias" value="0.01"/>
+      <param name="initial_covariance/acc_bias" value="0.01"/>
+      <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
+      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+      <remap from="~features" to="image_processor/features"/>
+
+    </node>
+  </group>
+
+</launch>

src/image_processor.cpp

@@ -390,7 +390,6 @@ void ImageProcessor::predictFeatureTracking(
     const cv::Matx33f& R_p_c,
     const cv::Vec4d& intrinsics,
     vector<cv::Point2f>& compensated_pts) {
-
   // Return directly if there are no input features.
   if (input_pts.size() == 0) {
     compensated_pts.clear();
@@ -421,7 +420,6 @@ void ImageProcessor::trackFeatures() {
     cam0_curr_img_ptr->image.rows / processor_config.grid_row;
   static int grid_width =
     cam0_curr_img_ptr->image.cols / processor_config.grid_col;
-
   // Compute a rough relative rotation which takes a vector
   // from the previous frame to the current frame.
   Matx33f cam0_R_p_c;
@@ -611,7 +609,6 @@ void ImageProcessor::stereoMatch(
     const vector<cv::Point2f>& cam0_points,
     vector<cv::Point2f>& cam1_points,
     vector<unsigned char>& inlier_markers) {
-
   if (cam0_points.size() == 0) return;
 
   if(cam1_points.size() == 0) {
@@ -700,8 +697,8 @@ void ImageProcessor::addNewFeatures() {
     cam0_curr_img_ptr->image.rows / processor_config.grid_row;
   static int grid_width =
     cam0_curr_img_ptr->image.cols / processor_config.grid_col;
-
   // Create a mask to avoid redetecting existing features.
+
   Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1));
 
   for (const auto& features : *curr_features_ptr) {
@@ -721,7 +718,6 @@ void ImageProcessor::addNewFeatures() {
       mask(row_range, col_range) = 0;
     }
   }
-
   // Detect new features.
   vector<KeyPoint> new_features(0);
   detector_ptr->detect(curr_img, new_features, mask);
@@ -736,7 +732,6 @@ void ImageProcessor::addNewFeatures() {
     new_feature_sieve[
       row*processor_config.grid_col+col].push_back(feature);
   }
-
   new_features.clear();
   for (auto& item : new_feature_sieve) {
     if (item.size() > processor_config.grid_max_feature_num) {
@@ -749,7 +744,6 @@ void ImageProcessor::addNewFeatures() {
   }
 
   int detected_new_features = new_features.size();
-
   // Find the stereo matched points for the newly
   // detected features.
   vector<cv::Point2f> cam0_points(new_features.size());
@@ -777,7 +771,6 @@ void ImageProcessor::addNewFeatures() {
       static_cast<double>(detected_new_features) < 0.1)
     ROS_WARN("Images at [%f] seems unsynced...",
         cam0_curr_img_ptr->header.stamp.toSec());
-
   // Group the features into grids
   GridFeatures grid_new_features;
   for (int code = 0; code <
@@ -799,7 +792,6 @@ void ImageProcessor::addNewFeatures() {
     new_feature.cam1_point = cam1_point;
     grid_new_features[code].push_back(new_feature);
   }
-
   // Sort the new features in each grid based on its response.
   for (auto& item : grid_new_features)
     std::sort(item.second.begin(), item.second.end(),
@@ -849,73 +841,6 @@ void ImageProcessor::pruneGridFeatures() {
   return;
 }
 
-void ImageProcessor::undistortPoints(
-    const vector<cv::Point2f>& pts_in,
-    const cv::Vec4d& intrinsics,
-    const string& distortion_model,
-    const cv::Vec4d& distortion_coeffs,
-    vector<cv::Point2f>& pts_out,
-    const cv::Matx33d &rectification_matrix,
-    const cv::Vec4d &new_intrinsics) {
-
-  if (pts_in.size() == 0) return;
-
-  const cv::Matx33d K(
-      intrinsics[0], 0.0, intrinsics[2],
-      0.0, intrinsics[1], intrinsics[3],
-      0.0, 0.0, 1.0);
-
-  const cv::Matx33d K_new(
-      new_intrinsics[0], 0.0, new_intrinsics[2],
-      0.0, new_intrinsics[1], new_intrinsics[3],
-      0.0, 0.0, 1.0);
-
-  if (distortion_model == "radtan") {
-    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
-                        rectification_matrix, K_new);
-  } else if (distortion_model == "equidistant") {
-    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
-                                 rectification_matrix, K_new);
-  } else {
-    ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
-                  distortion_model.c_str());
-    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
-                        rectification_matrix, K_new);
-  }
-
-  return;
-}
-
-vector<cv::Point2f> ImageProcessor::distortPoints(
-    const vector<cv::Point2f>& pts_in,
-    const cv::Vec4d& intrinsics,
-    const string& distortion_model,
-    const cv::Vec4d& distortion_coeffs) {
-
-  const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
-                      0.0, intrinsics[1], intrinsics[3],
-                      0.0, 0.0, 1.0);
-
-  vector<cv::Point2f> pts_out;
-  if (distortion_model == "radtan") {
-    vector<cv::Point3f> homogenous_pts;
-    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
-    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
-                      distortion_coeffs, pts_out);
-  } else if (distortion_model == "equidistant") {
-    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  } else {
-    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
-                  distortion_model.c_str());
-    vector<cv::Point3f> homogenous_pts;
-    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
-    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
-                      distortion_coeffs, pts_out);
-  }
-
-  return pts_out;
-}
-
 void ImageProcessor::integrateImuData(
     Matx33f& cam0_R_p_c, Matx33f& cam1_R_p_c) {
   // Find the start and the end limit within the imu msg buffer.
@@ -967,7 +892,6 @@ void ImageProcessor::integrateImuData(
 void ImageProcessor::rescalePoints(
     vector<Point2f>& pts1, vector<Point2f>& pts2,
     float& scaling_factor) {
-
   scaling_factor = 0.0f;
 
   for (int i = 0; i < pts1.size(); ++i) {
@@ -1232,7 +1156,6 @@ void ImageProcessor::twoPointRansac(
 }
 
 void ImageProcessor::publish() {
-
   // Publish features.
   CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement);
   feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;

src/msckf_vio.cpp

@@ -306,7 +306,6 @@ void MsckfVio::imageCallback(
     const sensor_msgs::ImageConstPtr& cam1_img,
     const CameraMeasurementConstPtr& feature_msg)
 {
-
   // Return if the gravity vector has not been set.
   if (!is_gravity_set) return;
 
@@ -344,7 +343,7 @@ void MsckfVio::imageCallback(
 
   // Add new images to moving window
   start_time = ros::Time::now();
-  //manageMovingWindow(cam0_img, cam1_img, feature_msg);
+  manageMovingWindow(cam0_img, cam1_img, feature_msg);
   double manage_moving_window_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -398,16 +397,16 @@ void MsckfVio::manageMovingWindow(
     const CameraMeasurementConstPtr& feature_msg) {
 
     //save exposure Time into moving window
-    cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000;
+    cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000;
-    cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000;
+    cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000;
     if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
       cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
     if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
       cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
-    if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
+    if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
-      cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
+      cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
-    if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
+    if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
-      cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
+      cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
 
     // Get the current image.
     cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
@@ -981,6 +980,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");
+
   //estimate photometric measurement
   std::vector<float> estimate_photo_z;
   feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
@@ -990,9 +991,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
   for(int i = 0; i < photo_z.size(); i++)
     photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
 
-  // printf("-----\n");
+
-  // for(int i = 0; i < photo_z.size(); i++)
+  for(int i = 0; i < photo_z.size(); i++)
-  //  printf("%.4f - %.4f\n", photo_z[i], estimate_photo_z[i]);
+    printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
 
   photo_z.clear();
   return;
@@ -1341,6 +1342,9 @@ void MsckfVio::removeLostFeatures() {
           continue;
         }
       }
+    }
+    if(!feature.is_anchored)
+    {
       if(!feature.initializeAnchor(cam0_moving_window, cam0))
       {
         invalid_feature_ids.push_back(feature.id);
@@ -1477,7 +1481,6 @@ void MsckfVio::pruneCamStateBuffer() {
       feature.observations.erase(involved_cam_state_ids[0]);
       continue;
     }
-
     if (!feature.is_initialized) {
       // Check if the feature can be initialize.
       if (!feature.checkMotion(state_server.cam_states)) {
@@ -1494,6 +1497,9 @@ void MsckfVio::pruneCamStateBuffer() {
           continue;
         }
       }
+    }
+    if(!feature.is_anchored)
+    {
       if(!feature.initializeAnchor(cam0_moving_window, cam0))
       {
         for (const auto& cam_id : involved_cam_state_ids)
@@ -1501,7 +1507,6 @@ void MsckfVio::pruneCamStateBuffer() {
         continue;
       }
     }
-
     jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
   }
 
@@ -1512,7 +1517,6 @@ void MsckfVio::pruneCamStateBuffer() {
       21+6*state_server.cam_states.size());
   VectorXd r = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
-  ros::Time start_time = ros::Time::now();
   for (auto& item : map_server) {
     auto& feature = item.second;
     // Check how many camera states to be removed are associated
@@ -1539,8 +1543,6 @@ void MsckfVio::pruneCamStateBuffer() {
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
-  double anchorPrune_processing_time = (ros::Time::now()-start_time).toSec();
-  printf("FeatureJacobian Time: %f\n", anchorPrune_processing_time);
 
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);