minor changes to visualization, jakobi tests

include/msckf_vio/feature.hpp

@@ -172,7 +172,8 @@ struct Feature {
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  const std::vector<double> photo_r) const;
+                  const std::vector<double> photo_r,
+                  std::stringstream& ss) const;
   /*
   * @brief projectPixelToPosition uses the calcualted pixels
   *     of the anchor patch to generate 3D positions of all of em
@@ -410,7 +411,8 @@ bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  const std::vector<double> photo_r) const
+                  const std::vector<double> photo_r,
+                  std::stringstream& ss) const
 {
 
   double rescale = 1;
@@ -421,14 +423,15 @@ bool Feature::VisualizePatch(
   cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
   cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
 
+  // visualize the true anchor points (the surrounding of the original measurements)
   for(auto point : anchorPatch_real)
   {
     // visu - feature
     cv::Point xs(point.x, point.y);
     cv::Point ys(point.x, point.y);
-    cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
+    cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
   }  
-  cam0.featureVisu = dottedFrame.clone();
+  cam0.featureVisu = dottedFrame.clone(); 
 
   // visu - feature
   cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
@@ -487,23 +490,52 @@ bool Feature::VisualizePatch(
                       cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), 
                       CV_FILLED);
       
+
+  // visualize position of used observations and resulting feature position
+  cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
+  cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale);
+
+  // draw world zero
+  cv::line(positionFrame,
+          cv::Point(20,20),
+          cv::Point(20,30),
+          cv::Scalar(0,0,255),
+          CV_FILLED);
+
+  cv::line(positionFrame,
+          cv::Point(20,20),
+          cv::Point(30,20),
+          cv::Scalar(255,0,0),
+          CV_FILLED);
+  // for every observation, get cam state
+  for(auto obs : observations)
+  {
+      cam_state.find(obs->first);
+      cv::line(positionFrame,
+        cv::Point(20,20),
+        cv::Point(30,20),
+        cv::Scalar(255,0,0),
+        CV_FILLED);
+  }
+  // draw, x y position and arrow with direction - write z next to it
+
   cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
 
   cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
+  cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
 
   // write feature position
   std::stringstream pos_s;
   pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
-  cv::putText(cam0.featureVisu, pos_s.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
+  cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
-
   // create line?
 
   //save image
 
-  std::stringstream ss;
-  ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
-  cv::imwrite(ss.str(), cam0.featureVisu);
+  std::stringstream loc;
+  loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
+  cv::imwrite(loc.str(), cam0.featureVisu);
 
   //cv::imshow("patch", cam0.featureVisu);
   //cvWaitKey(1);

include/msckf_vio/msckf_vio.h

@@ -107,6 +107,15 @@ class MsckfVio {
      */
     void imuCallback(const sensor_msgs::ImuConstPtr& msg);
 
+    /*
+     * @brief truthCallback
+     *      Callback function for ground truth navigation information
+     * @param TransformStamped msg
+     */    
+    void truthCallback(
+        const geometry_msgs::TransformStampedPtr& msg);
+
+
     /*
      * @brief imageCallback
      *    Callback function for feature measurements
@@ -144,11 +153,23 @@ class MsckfVio {
     bool resetCallback(std_srvs::Trigger::Request& req,
         std_srvs::Trigger::Response& res);
 
+    // Saves the exposure time and the camera measurementes
     void manageMovingWindow(
         const sensor_msgs::ImageConstPtr& cam0_img,
         const sensor_msgs::ImageConstPtr& cam1_img,
         const CameraMeasurementConstPtr& feature_msg);
 
+
+    // Debug related Functions
+    // Propagate the true state
+    void batchTruthProcessing(
+        const double& time_bound);
+
+    void processTruthtoIMU(const double& time,
+        const Eigen::Vector4d& m_rot,
+        const Eigen::Vector3d& m_trans);
+
+
     // Filter related functions
     // Propogate the state
     void batchImuProcessing(
@@ -202,12 +223,16 @@ class MsckfVio {
     void onlineReset();
 
     // Photometry flag
-    // visualization flag
     bool PHOTOMETRIC;
+
+    // debug flag
     bool PRINTIMAGES;
+    bool GROUNDTRUTH;
 
     bool nan_flag;
 
+    double last_time_bound;
+
     // Patch size for Photometry
     int N;
 
@@ -227,6 +252,8 @@ class MsckfVio {
     // transfer delay between IMU and Image messages.
     std::vector<sensor_msgs::Imu> imu_msg_buffer;
 
+    // Ground Truth message data
+    std::vector<geometry_msgs::TransformStamped> truth_msg_buffer;
     // Moving Window buffer
     movingWindow cam0_moving_window;
     movingWindow cam1_moving_window;
@@ -268,6 +295,7 @@ class MsckfVio {
 
     // Subscribers and publishers
     ros::Subscriber imu_sub;
+    ros::Subscriber truth_sub;
     ros::Publisher odom_pub;
     ros::Publisher feature_pub;
     tf::TransformBroadcaster tf_pub;