corrected position calculation for NxN points

CMakeLists.txt

@@ -88,6 +88,7 @@ add_dependencies(msckf_vio
 target_link_libraries(msckf_vio
   ${catkin_LIBRARIES}
   ${SUITESPARSE_LIBRARIES}
+  ${OpenCV_LIBRARIES}
 )
 
 # Msckf Vio nodelet

include/msckf_vio/feature.hpp

@@ -177,6 +177,9 @@ struct Feature {
   // Position of NxN Patch in 3D space
   std::vector<Eigen::Vector3d> anchorPatch_3d;
 
+  // Anchor Isometry
+  Eigen::Isometry3d T_anchor_w;
+
   // 3d postion of the feature in the world frame.
   Eigen::Vector3d position;
 
@@ -335,8 +338,11 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p,
   // use undistorted position of point of interest
   // project it back into 3D space using pinhole model
   // save resulting NxN positions for this feature
-  anchorPatch_3d.push_back(Eigen::Vector3d(in_p.x/rho, in_p.y/rho, 1/rho));
-  printf("%f, %f, %f\n",in_p.x/rho, in_p.y/rho, 1/rho);
+
+  Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho);
+  Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
+  anchorPatch_3d.push_back(PositionInWorld);
+  printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
 }
 
 bool Feature::initializeAnchor(
@@ -412,6 +418,7 @@ bool Feature::initializePosition(
   // vector from the first camera frame in the buffer to this
   // camera frame.
   Eigen::Isometry3d T_c0_w = cam_poses[0];
+  T_anchor_w = T_c0_w;
   for (auto& pose : cam_poses)
     pose = pose.inverse() * T_c0_w;
 

include/msckf_vio/image_handler.h

@@ -1,13 +1,14 @@
 #ifndef MSCKF_VIO_IMAGE_HANDLER_H
 #define MSCKF_VIO_IMAGE_HANDLER_H
 
-#include <ros/ros.h>
-#include <string>
+#include <vector>
+#include <boost/shared_ptr.hpp>
 #include <opencv2/opencv.hpp>
 #include <opencv2/video.hpp>
+
+#include <ros/ros.h>
 #include <cv_bridge/cv_bridge.h>
-#include <Eigen/Geometry>
-#include <vector>
+
 
 namespace msckf_vio {
 /*

src/image_handler.cpp

@@ -1,4 +1,14 @@
-#include <msckf_vio/image_handler.h>
+#include <iostream>
+#include <algorithm>
+#include <set>
+#include <Eigen/Dense>
+
+#include <sensor_msgs/image_encodings.h>
+#include <random_numbers/random_numbers.h>
+
+#include <msckf_vio/CameraMeasurement.h>
+#include <msckf_vio/TrackingInfo.h>
+#include <msckf_vio/image_processor.h>
 
 namespace msckf_vio {
 namespace image_handler {