added anchor information generation

include/msckf_vio/feature.hpp

@@ -114,6 +114,19 @@ struct Feature {
   inline bool checkMotion(
       const CamStateServer& cam_states) const;
 
+  /*
+   * @brief InitializeAnchor generates the NxN patch around the
+   *        feature in the Anchor image
+   * @param cam_states: A map containing all recorded images 
+   *        currently presented in the camera state vector
+   * @return the irradiance of the Anchor NxN Patch 
+   * @return True if the Anchor can be estimated
+   */
+
+  inline bool initializeAnchor(
+      const movingWindow& cam0_moving_window);
+
+
   /*
    * @brief InitializePosition Intialize the feature position
    *    based on all current available measurements.
@@ -145,7 +158,7 @@ struct Feature {
       std::pair<const StateIDType, Eigen::Vector4d> > > observations;
 
   // NxN Patch of Anchor Image
-  std::vector<double> patch;
+  std::vector<double> anchorPatch;
   // 3d postion of the feature in the world frame.
   Eigen::Vector3d position;
 
@@ -292,6 +305,29 @@ bool Feature::checkMotion(
   else return false;
 }
 
+bool Feature::initializeAnchor(
+   const movingWindow& cam0_moving_window)
+{
+
+  int N = 5;
+  int n = (int)(N-1)/2;
+
+  auto anchor = observations.begin();
+  if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end())
+    return false;
+
+  cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second;
+  auto u = anchor->second(0)*anchorImage.rows/2 + anchorImage.rows/2;
+  auto v = anchor->second(1)*anchorImage.cols/2 + anchorImage.cols/2;
+  int count = 0;
+
+  for(int u_run = (int)u - n; u_run <= (int)u + n; u_run++)
+    for(int v_run = v - n; v_run <= v + n; v_run++)
+      anchorPatch.push_back(anchorImage.at<uint8_t>(u_run,v_run));
+
+  return true;
+}
+
 bool Feature::initializePosition(
     const CamStateServer& cam_states) {
   // Organize camera poses and feature observations properly.