fixed stop - go functionality via bagcontrol

Merge branch 'photometry-jakobi-htest' of https://gitlab.com/Hippocampus/msckf into photometry-jakobi-htest
removed constant logging
2019-06-28 09:40:59 +02:00 · 2019-06-25 12:08:41 +02:00 · 2019-06-19 09:29:22 +02:00 · 2019-06-14 12:37:06 +02:00 · 2019-06-12 09:25:38 +00:00 · 2019-06-12 09:26:34 +02:00
30 changed files with 360 additions and 2450 deletions
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@ -1,16 +0,0 @@
-{
-    "configurations": [
-        {
-            "name": "Linux",
-            "includePath": [
-                "${workspaceFolder}/**"
-            ],
-            "defines": [],
-            "compilerPath": "/usr/bin/gcc",
-            "cStandard": "c11",
-            "cppStandard": "c++14",
-            "intelliSenseMode": "clang-x64"
-        }
-    ],
-    "version": 4
-}
--- a/.vscode/ipch/778a17e566a4909e/mmap_address.bin
+++ b/.vscode/ipch/778a17e566a4909e/mmap_address.bin
--- a/.vscode/ipch/ccf983af1f87ec2b/mmap_address.bin
+++ b/.vscode/ipch/ccf983af1f87ec2b/mmap_address.bin
--- a/.vscode/ipch/e40aedd19a224f8d/mmap_address.bin
+++ b/.vscode/ipch/e40aedd19a224f8d/mmap_address.bin
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -1,6 +0,0 @@
-{
-    "files.associations": {
-        "core": "cpp",
-        "sparsecore": "cpp"
-    }
-}
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/README.md
+++ b/README.md
@ -1,22 +1,12 @@
 # MSCKF\_VIO


-The `MSCKF_VIO` package is a stereo-photometric version of MSCKF. The software takes in synchronized stereo images and IMU messages and generates real-time 6DOF pose estimation of the IMU frame.
+The `MSCKF_VIO` package is a stereo version of MSCKF. The software takes in synchronized stereo images and IMU messages and generates real-time 6DOF pose estimation of the IMU frame.

-This approach is based on the paper written by Ke Sun et al.
-[https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036) and their Stereo MSCKF implementation, which tightly fuse the matched feature information of a stereo image pair into a 6DOF Pose.
-The approach implemented in this repository follows the semi-dense msckf approach tightly fusing the photometric
-information around the matched featues into the covariance matrix, as described and derived in the master thesis [Pose Estimation using a Stereo-Photometric Multi-State Constraint Kalman Filter](http://raphael.maenle.net/resources/sp-msckf/maenle_master_thesis.pdf).
-
-It's positioning is comparable to the approach from Ke Sun et al. with the photometric approach, with a higher
-computational load, especially with larger image patches around the feature. A video explaining the approach can be
-found on [https://youtu.be/HrqQywAnenQ](https://youtu.be/HrqQywAnenQ): 
-<br/>
-[![Stereo-Photometric MSCKF](https://img.youtube.com/vi/HrqQywAnenQ/0.jpg)](https://www.youtube.com/watch?v=HrqQywAnenQ)
-
-<br/>
-This software should be deployed using ROS Kinetic on Ubuntu 16.04 or 18.04.
+The software is tested on Ubuntu 16.04 with ROS Kinetic.

+Video: [https://www.youtube.com/watch?v=jxfJFgzmNSw&t](https://www.youtube.com/watch?v=jxfJFgzmNSw&t=3s)<br/>
+Paper Draft: [https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036)

 ## License

@ -38,6 +28,16 @@ cd your_work_space
 catkin_make --pkg msckf_vio --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```

+## Calibration
+
+An accurate calibration is crucial for successfully running the software. To get the best performance of the software, the stereo cameras and IMU should be hardware synchronized. Note that for the stereo calibration, which includes the camera intrinsics, distortion, and extrinsics between the two cameras, you have to use a calibration software. **Manually setting these parameters will not be accurate enough.** [Kalibr](https://github.com/ethz-asl/kalibr) can be used for the stereo calibration and also to get the transformation between the stereo cameras and IMU. The yaml file generated by Kalibr can be directly used in this software. See calibration files in the `config` folder for details. The two calibration files in the `config` folder should work directly with the EuRoC and [fast flight](https://github.com/KumarRobotics/msckf_vio/wiki) datasets. The convention of the calibration file is as follows:
+
+`camx/T_cam_imu`: takes a vector from the IMU frame to the camx frame.
+`cam1/T_cn_cnm1`: takes a vector from the cam0 frame to the cam1 frame.
+
+The filter uses the first 200 IMU messages to initialize the gyro bias, acc bias, and initial orientation. Therefore, the robot is required to start from a stationary state in order to initialize the VIO successfully.
+
+
 ## EuRoC and UPenn Fast flight dataset example usage

 First obtain either the [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) or the [UPenn fast flight](https://github.com/KumarRobotics/msckf_vio/wiki/Dataset) dataset.
@ -75,8 +75,6 @@ To visualize the pose and feature estimates you can use the provided rviz config

 ## ROS Nodes

-The general structure is similar to the structure of the MSCKF implementation this repository is derived from.
-
 ### `image_processor` node

 **Subscribed Topics**
--- a/config/camchain-imucam-euroc.yaml
+++ b/config/camchain-imucam-euroc.yaml
@ -9,7 +9,7 @@ cam0:
                     0,                   0,                   0,   1.000000000000000]
  camera_model: pinhole
  distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
-  distortion_model: pre-radtan
+  distortion_model: radtan
  intrinsics: [458.654, 457.296, 367.215, 248.375]
  resolution: [752, 480]
  timeshift_cam_imu: 0.0
@ -26,7 +26,7 @@ cam1:
                     0,                   0,                   0,   1.000000000000000]
  camera_model: pinhole
  distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
-  distortion_model: pre-radtan
+  distortion_model: radtan
  intrinsics: [457.587, 456.134, 379.999, 255.238]
  resolution: [752, 480]
  timeshift_cam_imu: 0.0
--- a/config/camchain-imucam-tum-scaled.yaml
+++ b/config/camchain-imucam-tum-scaled.yaml
@ -1,36 +0,0 @@
-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.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
-    0.00020293673591811182]
-  distortion_model: pre-equidistant
-  intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
-  resolution: [512, 512]
-  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.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
-    0.0003299517423931039]
-  distortion_model: pre-equidistant
-  intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
-  resolution: [512, 512]
-  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]
--- a/include/msckf_vio/cam_state.h
+++ b/include/msckf_vio/cam_state.h
@ -18,8 +18,6 @@ namespace msckf_vio {

 struct Frame{
  cv::Mat image;
-  cv::Mat dximage;
-  cv::Mat dyimage;
  double exposureTime_ms;
 };

@ -41,7 +39,6 @@ struct CameraCalibration{
  cv::Vec4d distortion_coeffs;
  movingWindow moving_window;
  cv::Mat featureVisu;
-  int id;
 };


--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@ -137,7 +137,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
  inline bool checkMotion(
      const CamStateServer& cam_states) const;

-
  /*
   * @brief InitializeAnchor generates the NxN patch around the
   *        feature in the Anchor image
@ -172,6 +171,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
                  Eigen::Vector3d& in_p) const;


+
 /*
 *  @brief project PositionToCamera Takes a 3d position in a world frame
 *         and projects it into the passed camera frame using pinhole projection
@ -184,16 +184,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
                  const CameraCalibration& cam,
                  Eigen::Vector3d& in_p) const;

-  double CompleteCvKernel(
-                const cv::Point2f pose,
-                const StateIDType& cam_state_id,
-                CameraCalibration& cam,
-                std::string type) const;
-
-  double cvKernel(
-            const cv::Point2f pose,
-            std::string type) const;
-
  double Kernel(
            const cv::Point2f pose,
            const cv::Mat frame,
@ -208,7 +198,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
  bool estimate_FrameIrradiance(
                  const CAMState& cam_state,
                  const StateIDType& cam_state_id,
-                  CameraCalibration& cam,
+                  CameraCalibration& cam0,
                  std::vector<double>& anchorPatch_estimate,
                  IlluminationParameter& estimatedIllumination) const;

@ -216,17 +206,14 @@ bool MarkerGeneration(
                  ros::Publisher& marker_pub,
                  const CamStateServer& cam_states) const;

-bool VisualizeKernel(
-                  const CAMState& cam_state,
-                  const StateIDType& cam_state_id,
-                  CameraCalibration& cam0) const;
-
  bool VisualizePatch(
                  const CAMState& cam_state,
                  const StateIDType& cam_state_id,
-                  CameraCalibration& cam,
+                  CameraCalibration& cam0,
                  const Eigen::VectorXd& photo_r,
-                  std::stringstream& ss) const;
+                  std::stringstream& ss,
+                  cv::Point2f gradientVector,
+                  cv::Point2f residualVector) const;
  /*
  * @brief AnchorPixelToPosition uses the calcualted pixels
  *     of the anchor patch to generate 3D positions of all of em
@ -277,14 +264,6 @@ inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
  bool is_initialized;
  bool is_anchored;

-
-  cv::Mat abs_xderImage;
-  cv::Mat abs_yderImage;
-
-  cv::Mat xderImage;
-  cv::Mat yderImage;
-
-  cv::Mat anchorImage_blurred;
  cv::Point2f anchor_center_pos;
  cv::Point2f undist_anchor_center_pos;
  // Noise for a normalized feature measurement.
@ -494,38 +473,6 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
  else return false;
 }

-double Feature::CompleteCvKernel(
-                const cv::Point2f pose,
-                const StateIDType& cam_state_id,
-                CameraCalibration& cam,
-                std::string type) const
-{
-
-  double delta = 0;
-
-  if(type == "Sobel_x")
-    delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at<short>(pose.y, pose.x))/255.;
-  else if (type == "Sobel_y")
-    delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at<short>(pose.y, pose.x))/255.;
-
-  return delta;
-}
-
-double Feature::cvKernel(
-                const cv::Point2f pose,
-                std::string type) const
-{
-
-  double delta = 0;
-
-  if(type == "Sobel_x")
-    delta = ((double)xderImage.at<short>(pose.y, pose.x))/255.;
-  else if (type == "Sobel_y")
-    delta = ((double)yderImage.at<short>(pose.y, pose.x))/255.;
-  return delta;
-
-}
-
 double Feature::Kernel(
                const cv::Point2f pose,
                const cv::Mat frame,
@ -542,14 +489,14 @@ int offs = (int)(kernel.rows()-1)/2;

 for(int i = 0; i < kernel.rows(); i++)
  for(int j = 0; j < kernel.cols(); j++)
-    delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255. * (float)kernel(j,i);
+    delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i);

 return delta;
 }
 bool Feature::estimate_FrameIrradiance(
                  const CAMState& cam_state,
                  const StateIDType& cam_state_id,
-                  CameraCalibration& cam,
+                  CameraCalibration& cam0,
                  std::vector<double>& anchorPatch_estimate,
                  IlluminationParameter& estimated_illumination) const
 {
@ -558,11 +505,11 @@ bool Feature::estimate_FrameIrradiance(
  // muliply by a and add b of this frame

  auto anchor = observations.begin();
-  if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
+  if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end())
    return false;

-  double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms;
-  double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms;
+  double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms;
+  double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms;


  double a_A = anchorExposureTime_ms;
@ -697,55 +644,21 @@ bool Feature::MarkerGeneration(
  marker_pub.publish(ma);
 }

-
-bool Feature::VisualizeKernel(
-                  const CAMState& cam_state,
-                  const StateIDType& cam_state_id,
-                  CameraCalibration& cam0) const
-{
-  auto anchor = observations.begin();
-  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
-
-  //cv::Mat xderImage;
-  //cv::Mat yderImage;
-
-  //cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3);
-  //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
-
-
-  // cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
-  // cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
-
-  /*
-  for(int i = 1; i < anchorImage.rows-1; i++)
-    for(int j = 1; j < anchorImage.cols-1; j++)
-      xderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x"));
-  
-
-  for(int i = 1; i < anchorImage.rows-1; i++)
-    for(int j = 1; j < anchorImage.cols-1; j++)
-      yderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y"));
-  */
-  //cv::imshow("anchor", anchorImage);
-  cv::imshow("xder2", xderImage);
-  cv::imshow("yder2", yderImage);
-
-  cvWaitKey(0);
-} 
-
 bool Feature::VisualizePatch(
                  const CAMState& cam_state,
                  const StateIDType& cam_state_id,
-                  CameraCalibration& cam,
+                  CameraCalibration& cam0,
                  const Eigen::VectorXd& photo_r,
-                  std::stringstream& ss) const
+                  std::stringstream& ss,
+                  cv::Point2f gradientVector,
+                  cv::Point2f residualVector) const
 {

  double rescale = 1;

  //visu - anchor
  auto anchor = observations.begin();
-  cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
+  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
  cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
  cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);

@ -757,10 +670,10 @@ bool Feature::VisualizePatch(
    cv::Point ys(point.x, point.y);
    cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
  }  
-  cam.featureVisu = dottedFrame.clone(); 
+  cam0.featureVisu = dottedFrame.clone(); 

  // visu - feature
-  cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image;
+  cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
  cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
  
  // set position in frame 
@ -768,7 +681,7 @@ bool Feature::VisualizePatch(
  std::vector<double> projectionPatch;
  for(auto point : anchorPatch_3d)
  {
-    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point);
+    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
    projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
    // visu - feature
    cv::Point xs(p_in_c0.x, p_in_c0.y);
@ -776,7 +689,7 @@ bool Feature::VisualizePatch(
    cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
  }  

-  cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu);
+  cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);


  // patches visualization
@ -823,14 +736,10 @@ 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;
-  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));
+  cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
  // move to real pixels
+  p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);

-  p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
  for(int i = 0; i<N; i++)
  {
    for(int j = 0; j<N ; j++)
@ -847,23 +756,22 @@ bool Feature::VisualizePatch(
  // residual grid projection, positive - red, negative - blue colored 
  namer.str(std::string());
  namer << "residual";
-  std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl;
  cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), 
    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);

  for(int i = 0; i<N; i++)
    for(int j = 0; j<N; j++)
-      if(photo_r(2*(i*N+j))>0)
+      if(photo_r(i*N+j)>0)
        cv::rectangle(irradianceFrame, 
                      cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                      cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*255, 255),  
+                      cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255),  
                      CV_FILLED);
      else
        cv::rectangle(irradianceFrame, 
                      cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                      cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255), 
+                      cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255), 
                      CV_FILLED);

  // gradient arrow
@ -876,15 +784,14 @@ bool Feature::VisualizePatch(
  */

  // residual gradient direction
-      /*
  cv::arrowedLine(irradianceFrame,
                  cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))),
                  cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y),
                  cv::Scalar(0, 255, 175),
                  3);
-       */           
                  
-  cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu);
+
+  cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
      
 /*
  // visualize position of used observations and resulting feature position
@ -916,15 +823,15 @@ bool Feature::VisualizePatch(

  // draw, x y position and arrow with direction - write z next to it

-  cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale);
+  cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);


-  cv::hconcat(cam.featureVisu, positionFrame, cam.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(cam.featureVisu, ss.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?

@ -932,16 +839,16 @@ bool Feature::VisualizePatch(

  std::stringstream loc;
 // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
-  //cv::imwrite(loc.str(), cam.featureVisu);
+  //cv::imwrite(loc.str(), cam0.featureVisu);

-  cv::imshow("patch", cam.featureVisu);
-  cvWaitKey(1);
+  cv::imshow("patch", cam0.featureVisu);
+  cvWaitKey(0);
 }

 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
 {

-  return ((float)image.at<uint8_t>(pose.y, pose.x))/255.;
+  return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
 }

 cv::Point2f Feature::pixelDistanceAt(
@ -967,18 +874,14 @@ cv::Point2f Feature::pixelDistanceAt(
                                cam.distortion_coeffs, 
                                pure); 

-  // transfrom position to camera frame
-  // to get distance multiplier
-  Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
-  const Eigen::Vector3d& t_c0_w = cam_state.position;
-  Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
-
-  // returns the distance between the pixel points in space
+  // returns the absolute pixel distance at pixels one metres away
  cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));

  return distance;
 }

+
+
 cv::Point2f Feature::projectPositionToCamera(
                  const CAMState& cam_state,
                  const StateIDType& cam_state_id,
@ -988,40 +891,26 @@ cv::Point2f Feature::projectPositionToCamera(
  Eigen::Isometry3d T_c0_w;

  cv::Point2f out_p;
-  cv::Point2f my_p;
-  // transfrom position to camera frame

-  // cam0 position
+  // transfrom position to camera frame
  Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
  const Eigen::Vector3d& t_c0_w = cam_state.position;
-
-  // project point according to model
-  if(cam.id == 0)
-  {
  Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
+
  out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));

-  }
-  // if camera is one, calcualte the cam1 position from cam0 position first
-  else if(cam.id == 1)
-  {  
-    // cam1 position
-    Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
-    Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0;
-    Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
+ // if(cam_state_id == observations.begin()->first)
+      //printf("undist:\n  \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y);
 
-    Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w);
-    out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
-  }
-
-  // undistort point according to camera model
-  if (cam.distortion_model.substr(0,3) == "pre-")
-    my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]);
-  else
-    my_p = image_handler::distortPoint(out_p, 
+  cv::Point2f my_p = image_handler::distortPoint(out_p, 
                                                 cam.intrinsics, 
                                                 cam.distortion_model, 
                                                 cam.distortion_coeffs);
+
+  // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z());
+  // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2));
+  // printf("Photo projection: %f, %f\n", my_p[0].x,  my_p[0].y);
+  
  return my_p;
 }

@ -1046,48 +935,19 @@ 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);
-  //TODO remove this?
-
-
-  cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3);
-  cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3);
-
-  xderImage/=8.;
-  yderImage/=8.;
-
-  cv::convertScaleAbs(xderImage, abs_xderImage);
-  cv::convertScaleAbs(yderImage, abs_yderImage);
-  
-  cv::GaussianBlur(anchorImage, anchorImage_blurred, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT);

  auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
  auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
  
+  //testing
+  undist_anchor_center_pos = cv::Point2f(u,v);

-  // check if image has been pre-undistorted
-  if(cam.distortion_model.substr(0,3) == "pre")
-  {
-    //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));
+  //for NxN patch pixels around feature
+  int count = 0;

-
-    //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]));
-  }
-
-  else
-  {
  // get feature in undistorted pixel space
  // this only reverts from 'pure' space into undistorted pixel space using camera matrix
  cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
@ -1107,14 +967,13 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
                                cam.distortion_coeffs,
                                anchorPatch_ideal);

-  }

  // save anchor position for later visualisaztion
  anchor_center_pos = anchorPatch_real[(N*N-1)/2];

  // save true pixel Patch position
  for(auto point : anchorPatch_real)
-    if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1)
+    if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
      return false;

  for(auto point : anchorPatch_real)
@ -1125,11 +984,9 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
    anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));

  is_anchored = true;
-
  return true;
 }

-
 bool Feature::initializeRho(const CamStateServer& cam_states) {

  // Organize camera poses and feature observations properly.
--- a/include/msckf_vio/image_handler.h
+++ b/include/msckf_vio/image_handler.h
@ -16,16 +16,6 @@ namespace msckf_vio {
 */
 namespace image_handler {

-cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
-cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
-
-void undistortImage(
-  cv::InputArray src,
-  cv::OutputArray dst,
-  const std::string& distortion_model,
-  const cv::Vec4d& intrinsics,
-  const cv::Vec4d& distortion_coeffs);
-
 void undistortPoints(
    const std::vector<cv::Point2f>& pts_in,
    const cv::Vec4d& intrinsics,
@ -55,7 +45,6 @@ void undistortPoint(
    cv::Point2f& pt_out,
    const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(),
    const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0));
-
 }
 }
 #endif
--- a/include/msckf_vio/image_processor.h
+++ b/include/msckf_vio/image_processor.h
@ -320,8 +320,6 @@ private:
    return;
  }

-  bool STREAMPAUSE;
-
  // Indicate if this is the first image message.
  bool is_first_img;

--- a/include/msckf_vio/msckf_vio.h
+++ b/include/msckf_vio/msckf_vio.h
@ -14,7 +14,7 @@
 #include <string>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
-#include <math.h>
+
 #include <boost/shared_ptr.hpp>
 #include <opencv2/opencv.hpp>
 #include <opencv2/video.hpp>
@ -38,8 +38,6 @@
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>

-#define PI 3.14159265
-
 namespace msckf_vio {
 /*
 * @brief MsckfVio Implements the algorithm in
@ -197,116 +195,58 @@ class MsckfVio {
    // for a single feature observed at a single camera frame.
    void measurementJacobian(const StateIDType& cam_state_id,
        const FeatureIDType& feature_id,
-        Eigen::Matrix<double, 4, 6>& H_x,
-        Eigen::Matrix<double, 4, 3>& H_f,
-        Eigen::Vector4d& r);
+        Eigen::Matrix<double, 2, 6>& H_x,
+        Eigen::Matrix<double, 2, 3>& H_f,
+        Eigen::Vector2d& r);
    // This function computes the Jacobian of all measurements viewed
    // in the given camera states of this feature.
-    bool featureJacobian(
-        const FeatureIDType& feature_id,
+    void featureJacobian(const FeatureIDType& feature_id,
        const std::vector<StateIDType>& cam_state_ids,
        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);


-    void twodotMeasurementJacobian(
-        const StateIDType& cam_state_id,
-        const FeatureIDType& feature_id,
-        Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
-
-    bool ConstructJacobians(
-        Eigen::MatrixXd& H_rho,
-        Eigen::MatrixXd& H_pl,
-        Eigen::MatrixXd& H_pA,
-        const Feature& feature,
-        const StateIDType&  cam_state_id,
-        Eigen::MatrixXd& H_xl,
-        Eigen::MatrixXd& H_yl);
-
-    bool PhotometricPatchPointResidual(
-        const StateIDType& cam_state_id,
-        const Feature& feature, 
-        Eigen::VectorXd& r);
-
-    bool PhotometricPatchPointJacobian(
-        const CAMState& cam_state,
-        const StateIDType& cam_state_id,
-        const Feature& feature,
-        Eigen::Vector3d point,
-        int count,
-        Eigen::Matrix<double, 2, 1>& H_rhoj,
-        Eigen::Matrix<double, 2, 6>& H_plj,
-        Eigen::Matrix<double, 2, 6>& H_pAj,
-        Eigen::Matrix<double, 2, 4>& dI_dhj);
-
-    bool PhotometricMeasurementJacobian(
+    void PhotometricMeasurementJacobian(
    const StateIDType& cam_state_id,
    const FeatureIDType& feature_id,
    Eigen::MatrixXd& H_x,
    Eigen::MatrixXd& H_y,
    Eigen::VectorXd& r);

-
-    bool twodotFeatureJacobian(
+    void PhotometricFeatureJacobian(
    const FeatureIDType& feature_id,
    const std::vector<StateIDType>& cam_state_ids,
    Eigen::MatrixXd& H_x, Eigen::VectorXd& r);

-    bool PhotometricFeatureJacobian(
-        const FeatureIDType& feature_id,
-        const std::vector<StateIDType>& cam_state_ids,
-        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
-
-    void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
    void measurementUpdate(const Eigen::MatrixXd& H,
        const Eigen::VectorXd& r);
-    void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
-
    bool gatingTest(const Eigen::MatrixXd& H,
-        const Eigen::VectorXd&r, const int& dof, int filter=0);
+        const Eigen::VectorXd&r, const int& dof);
    void removeLostFeatures();
    void findRedundantCamStates(
        std::vector<StateIDType>& rm_cam_state_ids);
-
-    void pruneLastCamStateBuffer();
    void pruneCamStateBuffer();
    // Reset the system online if the uncertainty is too large.
    void onlineReset();

    // Photometry flag
-    int FILTER;
+    bool PHOTOMETRIC;

    // debug flag
-    bool STREAMPAUSE;
    bool PRINTIMAGES;
    bool GROUNDTRUTH;

    bool nan_flag;
    bool play;
    double last_time_bound;
-    double time_offset;

    // Patch size for Photometry
    int N;
-    // Image rescale
-    int SCALE; 
+
    // Chi squared test table.
    static std::map<int, double> chi_squared_test_table;

-    double eval_time;
-
-    IMUState timed_old_imu_state;
-    IMUState timed_old_true_state;
-
-    IMUState old_imu_state;
-    IMUState old_true_state;
-
-    // change in position
-    Eigen::Vector3d delta_position;
-    Eigen::Vector3d delta_orientation;
-
    // State vector
    StateServer state_server;
-    StateServer photometric_state_server;

    // Ground truth state vector 
    StateServer true_state_server;
@ -369,7 +309,6 @@ class MsckfVio {
    // Subscribers and publishers
    ros::Subscriber imu_sub;
    ros::Subscriber truth_sub;
-    ros::Publisher truth_odom_pub;
    ros::Publisher odom_pub;
    ros::Publisher marker_pub;
    ros::Publisher feature_pub;
--- a/launch/image_processor_tinytum.launch
+++ b/launch/image_processor_tinytum.launch
@ -1,38 +0,0 @@
-<launch>
-
-  <arg name="robot" default="firefly_sbx"/>
-  <arg name="calibration_file"
-    default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
-
-  <!-- Image Processor Nodelet  -->
-  <group ns="$(arg robot)">
-    <node pkg="nodelet" type="nodelet" name="image_processor"
-      args="standalone msckf_vio/ImageProcessorNodelet"
-      output="screen"
-       >
-
-      <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
-
-      <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="5"/>
-      <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>
--- a/launch/image_processor_tum.launch
+++ b/launch/image_processor_tum.launch
@ -11,9 +11,6 @@
      output="screen"
       >

-      <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
-
      <rosparam command="load" file="$(arg calibration_file)"/>
      <param name="grid_row" value="4"/>
      <param name="grid_col" value="4"/>
--- a/launch/msckf_vio_debug_tum.launch
+++ b/launch/msckf_vio_debug_tum.launch
@ -25,7 +25,7 @@
      <param name="PrintImages" value="true"/>
      <param name="GroundTruth" value="false"/>

-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="7"/>

      <!-- Calibration parameters -->
      <rosparam command="load" file="$(arg calibration_file)"/>
--- a/launch/msckf_vio_euroc.launch
+++ b/launch/msckf_vio_euroc.launch
@ -17,18 +17,6 @@
      args='standalone msckf_vio/MsckfVioNodelet'
      output="screen">

-
-      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="1"/>
-
-      <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="true"/>
-      <param name="GroundTruth" value="false"/>
-
-      <param name="patch_size_n" value="5"/>
-      <param name="image_scale" value ="1"/>
-
      <!-- Calibration parameters -->
      <rosparam command="load" file="$(arg calibration_file)"/>

--- a/launch/msckf_vio_tinytum.launch
+++ b/launch/msckf_vio_tinytum.launch
@ -1,77 +0,0 @@
-<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-scaled.yaml"/>
-
-  <!-- Image Processor Nodelet  -->
-  <include file="$(find msckf_vio)/launch/image_processor_tinytum.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">
-
-      <param name="FILTER" value="0"/>
-
-      <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="false"/>
-      <param name="GroundTruth" value="false"/>
-
-      <param name="patch_size_n" value="3"/>
-      <param name="image_scale" value ="1"/>
-
-      <!-- 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="12"/>
-      <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"/>
-      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
-
-      <remap from="~imu" to="/imu0"/>
-      <remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
-      <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>
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@ -17,16 +17,14 @@
      args='standalone msckf_vio/MsckfVioNodelet'
      output="screen">

-
-      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="1"/>
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="true"/>

      <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
      <param name="GroundTruth" value="false"/>

-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="1"/>
      <!-- Calibration parameters -->
      <rosparam command="load" file="$(arg calibration_file)"/>

@ -73,6 +71,4 @@
    </node>
  </group>

-  <!--node name="player" pkg="bagcontrol" type="control.py" /-->
-
 </launch>
--- a/73
+++ b/73
@ -1,73 +0,0 @@
-# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>
-# name: Hx
-# type: matrix
-# rows: 18
-# columns: 49
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 313.795 -58.7912 139.778 -46.7616 144.055 86.9644 0 -314.123 55.6434 -140.648 46.7616 -144.055 -86.9644 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 441.06 -94.50069999999999 174.424 -53.7653 204.822 120.248 0 -441.685 90.1101 -175.657 53.7653 -204.822 -120.248 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 225.35 -54.5629 77.60599999999999 -21.1425 105.886 60.3706 0 -225.756 52.3373 -78.2406 21.1425 -105.886 -60.3706 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 175.128 20.6203 175.127 -79.63939999999999 73.245 62.1868 0 -174.573 -22.5235 -175.576 79.63939999999999 -73.245 -62.1868 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 296.962 43.5469 311.307 -143.667 123.399 108.355 0 -295.905 -46.7952 -312.063 143.667 -123.399 -108.355 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 126.283 117.889 311.864 -161.264 38.8521 71.8019 0 -124.464 -119.541 -312.118 161.264 -38.8521 -71.8019 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49.2502 63.7166 155.071 -80.81950000000001 12.7732 32.1826 0 -48.2934 -64.4113 -155.157 80.81950000000001 -12.7732 -32.1826 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 69.59699999999999 154.579 335.384 -179.355 9.212580000000001 62.0364 0 -67.35599999999999 -155.735 -335.462 179.355 -9.212580000000001 -62.0364 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -66.6965 304.947 500.218 -285.589 -71.31010000000001 55.5058 0 70.8009 -305.077 -499.831 285.589 71.31010000000001 -55.5058 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 323.404 -62.2043 141.092 -46.6015 148.737 89.1108 0 0 0 0 0 0 0 0 -324.336 57.8552 -141.991 46.6015 -148.737 -89.1108 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 454.208 -99.3095 175.986 -53.4094 211.276 123.174 0 0 0 0 0 0 0 0 -455.779 93.0992 -177.158 53.4094 -211.276 -123.174 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 231.884 -57.0266 78.2559 -20.8926 109.118 61.8183 0 0 0 0 0 0 0 0 -232.824 53.8025 -78.80719999999999 20.8926 -109.118 -61.8183 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 181.715 18.8525 177.045 -80.08499999999999 76.3716 63.8254 0 0 0 0 0 0 0 0 -181.07 -20.839 -177.959 80.08499999999999 -76.3716 -63.8254 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 308.249 40.5812 314.711 -144.494 128.766 111.207 0 0 0 0 0 0 0 0 -306.972 -43.8825 -316.328 144.494 -128.766 -111.207 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 133.309 116.865 315.474 -162.598 42.0763 73.8353 0 0 0 0 0 0 0 0 -130.603 -117.454 -316.931 162.598 -42.0763 -73.8353 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 52.4426 63.3607 156.902 -81.5288 14.2139 33.1222 0 0 0 0 0 0 0 0 -50.9976 -63.4393 -157.607 81.5288 -14.2139 -33.1222 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 75.5508 154.234 339.369 -180.995 11.859 63.8956 0 0 0 0 0 0 0 0 -72.1041 -153.816 -340.865 180.995 -11.859 -63.8956 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -62.0551 306.357 506.351 -288.542 -69.50409999999999 57.4562 0 0 0 0 0 0 0 0 68.6262 -303.028 -508.423 288.542 69.50409999999999 -57.4562 1 0 0 0 0 0 0 0
-
-
-# name: Hy
-# type: matrix
-# rows: 18
-# columns: 14
- 1.56267 0 0 0 0 0 0 0 0 0 0.252873 0 0 0.110387
- 0 1.56267 0 0 0 0 0 0 0 0 0.453168 0 0 0.162961
- 0 0 1.56267 0 0 0 0 0 0 0 0.638441 0 0 0.0873758
- 0 0 0 1.56267 0 0 0 0 0 0 0.21031 0 0 0.0321517
- 0 0 0 0 1.56267 0 0 0 0 0 0.375554 0 0 0.0505151
- 0 0 0 0 0 1.56267 0 0 0 0 0.638441 0 0 -0.0336034
- 0 0 0 0 0 0 1.56267 0 0 0 0.157733 0 0 -0.0232704
- 0 0 0 0 0 0 0 1.56267 0 0 0.212814 0 0 -0.0688343
- 0 0 0 0 0 0 0 0 1.56267 0 0.360532 0 0 -0.187745
- 1.56267 0 0 0 0 0 0 0 0 0 0 0.252873 0 0.322811
- 0 1.56267 0 0 0 0 0 0 0 0 0 0.453168 0 0.467319
- 0 0 1.56267 0 0 0 0 0 0 0 0 0.638441 0 0.245907
- 0 0 0 1.56267 0 0 0 0 0 0 0 0.21031 0 0.135725
- 0 0 0 0 1.56267 0 0 0 0 0 0 0.375554 0 0.225277
- 0 0 0 0 0 1.56267 0 0 0 0 0 0.638441 0 0.012926
- 0 0 0 0 0 0 1.56267 0 0 0 0 0.157733 0 -0.0108962
- 0 0 0 0 0 0 0 1.56267 0 0 0 0.212814 0 -0.06974909999999999
- 0 0 0 0 0 0 0 0 1.56267 0 0 0.360532 0 -0.318846
-
-
-# name: r
-# type: matrix
-# rows: 18
-# columns: 1
- 0.0354809
- 0.0153183
- 0.0570191
- -0.0372801
- 0.0878601
- 0.06811780000000001
- -0.00426164
- 5.162985999041026e-321
- 6.927779999999998e-310
- 0
- 2.121999999910509e-314
- 0
- 0
- 0
- 3.6073900000086e-313
- 4.446590812571219e-323
- 3.952525166729972e-323
- 3.952525166729972e-323
-
-
--- a/src/bagcontrol.py
+++ b/src/bagcontrol.py
@ -1,64 +0,0 @@
-#!/usr/bin/env python
-import rosbag
-import rospy
-from sensor_msgs.msg import Imu, Image
-from geometry_msgs.msg import TransformStamped
-import time
-import signal
-import sys
-
-
-def signal_handler(sig, frame):
-        print('gracefully exiting the program.')
-        bag.close()
-        sys.exit(0)
-
-def main():
-    global bag
-
-    cam0_topic = '/cam0/image_raw'
-    cam1_topic = '/cam1/image_raw'
-    imu0_topic = '/imu0'
-    grnd_topic = '/vrpn_client/raw_transform'
-
-    rospy.init_node('controlbag')
-    rospy.set_param('play_bag', False)
-
-    cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
-    cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
-    imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
-    grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
-
-    signal.signal(signal.SIGINT, signal_handler)
-
-    bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
-    for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
-        
-        # pause if parameter set to false
-        flag = False
-        while not rospy.get_param('/play_bag'):
-            time.sleep(0.01)
-            if not flag:
-                print("stopped playback")
-                flag = not flag
-
-        if flag:
-            print("resume playback")
-
-        if topic == cam0_topic:
-            cam0_pub.publish(msg)
-
-        elif topic == cam1_topic:
-            cam1_pub.publish(msg)
-
-        elif topic == imu0_topic:
-            imu0_pub.publish(msg)
-
-        elif topic ==grnd_topic:
-            grnd_pub.publish(msg)
-
-        #print msg
-    bag.close()
-
-if __name__== "__main__":
-    main()
--- a/src/control.py
+++ b/src/control.py
@ -1,64 +0,0 @@
-#!/usr/bin/env python
-import rosbag
-import rospy
-from sensor_msgs.msg import Imu, Image
-from geometry_msgs.msg import TransformStamped
-import time
-import signal
-import sys
-
-
-def signal_handler(sig, frame):
-        print('gracefully exiting the program.')
-        bag.close()
-        sys.exit(0)
-
-def main():
-    global bag
-
-    cam0_topic = '/cam0/image_raw'
-    cam1_topic = '/cam1/image_raw'
-    imu0_topic = '/imu0'
-    grnd_topic = '/vrpn_client/raw_transform'
-
-    rospy.init_node('controlbag')
-    rospy.set_param('play_bag', False)
-
-    cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
-    cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
-    imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
-    grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
-
-    signal.signal(signal.SIGINT, signal_handler)
-
-    bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
-    for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
-        
-        # pause if parameter set to false
-        flag = False
-        while not rospy.get_param('/play_bag'):
-            time.sleep(0.01)
-            if not flag:
-                print("stopped playback")
-                flag = not flag
-
-        if flag:
-            print("resume playback")
-
-        if topic == cam0_topic:
-            cam0_pub.publish(msg)
-
-        elif topic == cam1_topic:
-            cam1_pub.publish(msg)
-
-        elif topic == imu0_topic:
-            imu0_pub.publish(msg)
-
-        elif topic ==grnd_topic:
-            grnd_pub.publish(msg)
-
-        #print msg
-    bag.close()
-
-if __name__== "__main__":
-    main()
--- a/src/image_handler.cpp
+++ b/src/image_handler.cpp
@ -14,39 +14,6 @@ namespace msckf_vio {
 namespace image_handler {


-cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
-{
-  return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]);
-}
-
-cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
-{
-  return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]);
-}
-
-void undistortImage(
-  cv::InputArray src,
-  cv::OutputArray dst,
-  const std::string& distortion_model,
-  const cv::Vec4d& intrinsics,
-  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);
-
-  if (distortion_model == "pre-equidistant")
-      cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K);
-  else if (distortion_model == "equidistant")
-      src.copyTo(dst);
-
-  else if (distortion_model == "pre-radtan")
-      cv::undistort(src, dst, K, distortion_coeffs, K);
-  else if (distortion_model == "radtan")
-      src.copyTo(dst);
-
-}

 void undistortPoint(
    const cv::Point2f& pt_in,
@ -76,37 +43,10 @@ void undistortPoint(
  if (distortion_model == "radtan") {
    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                        rectification_matrix, K_new);
-  } 
-  // equidistant
-  else if (distortion_model == "equidistant") {
+  } else if (distortion_model == "equidistant") {
    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                 rectification_matrix, K_new);
-  }
-  // fov
-  else if (distortion_model == "fov") {
-    for(int i = 0; i < pts_in.size(); i++)
-    {
-      float omega = distortion_coeffs[0];
-      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
-      float ru = tan(rd * omega)/(2 * tan(omega / 2));
-
-      cv::Point2f newPoint(
-              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
-              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
-
-      pts_out.push_back(newPoint);
-    }
-   }
-
-  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
-  {
-    std::vector<cv::Point2f> temp_pts_out;
-    for(int i = 0; i < pts_in.size(); i++)
-      temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
-
-    pts_out = temp_pts_out;
-  } 
-   else {
+  } 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,
@ -140,35 +80,10 @@ void undistortPoints(
  if (distortion_model == "radtan") {
    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                        rectification_matrix, K_new);
-  } 
-  else if (distortion_model == "equidistant") {
+  } else if (distortion_model == "equidistant") {
    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                 rectification_matrix, K_new);
-  } 
-  else if (distortion_model == "fov") {
-    for(int i = 0; i < pts_in.size(); i++)
-    {
-      float omega = distortion_coeffs[0];
-      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
-      float ru = tan(rd * omega)/(2 * tan(omega / 2));
-
-      cv::Point2f newPoint(
-              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
-              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
-
-      pts_out.push_back(newPoint);
-    }
-  }
-  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
-  {
-    std::vector<cv::Point2f> temp_pts_out;
-    for(int i = 0; i < pts_in.size(); i++)
-      temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
-
-    pts_out = temp_pts_out;
-
-  }  
-  else {
+  } 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,
@ -196,31 +111,7 @@ std::vector<cv::Point2f> distortPoints(
                      distortion_coeffs, pts_out);
  } else if (distortion_model == "equidistant") {
    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  }
-  else if (distortion_model == "fov") {
-    for(int i = 0; i < pts_in.size(); i++)
-    {
-      // based on 'straight lines have to be straight'
-      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
-      float omega = distortion_coeffs[0];
-      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
-      
-      cv::Point2f newPoint(
-              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
-              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
-
-      pts_out.push_back(newPoint);
-    }
-  }
-  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
-  {
-    std::vector<cv::Point2f> temp_pts_out;
-    for(int i = 0; i < pts_in.size(); i++)
-      temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
-
-    pts_out = temp_pts_out;
-  }   
-   else {
+  } else {
    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                  distortion_model.c_str());
    std::vector<cv::Point3f> homogenous_pts;
@ -253,31 +144,7 @@ cv::Point2f distortPoint(
                      distortion_coeffs, pts_out);
  } else if (distortion_model == "equidistant") {
    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  }
-  else if (distortion_model == "fov") {
-    for(int i = 0; i < pts_in.size(); i++)
-    {
-      // based on 'straight lines have to be straight'
-      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
-      float omega = distortion_coeffs[0];
-      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
-      
-      cv::Point2f newPoint(
-              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
-              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
-
-      pts_out.push_back(newPoint);
-    }
-  }
-  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
-  {
-    std::vector<cv::Point2f> temp_pts_out;
-    for(int i = 0; i < pts_in.size(); i++)
-      pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
-
-    pts_out = temp_pts_out;
-  }    
-   else {
+  } else {
    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                  distortion_model.c_str());
    std::vector<cv::Point3f> homogenous_pts;
--- a/src/image_processor.cpp
+++ b/src/image_processor.cpp
@ -42,9 +42,6 @@ ImageProcessor::~ImageProcessor() {
 }

 bool ImageProcessor::loadParameters() {
-
-  // debug parameters
-  nh.param<bool>("StreamPause", STREAMPAUSE, false);
  // Camera calibration parameters
  nh.param<string>("cam0/distortion_model",
      cam0.distortion_model, string("radtan"));
@ -214,9 +211,7 @@ void ImageProcessor::stereoCallback(
    const sensor_msgs::ImageConstPtr& cam0_img,
    const sensor_msgs::ImageConstPtr& cam1_img) {

-    // stop playing bagfile if printing images
-  //if(STREAMPAUSE)
-   // nh.setParam("/play_bag_image", false);
+  //cout << "==================================" << endl;

  // Get the current image.
  cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
@ -224,27 +219,12 @@ void ImageProcessor::stereoCallback(
  cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
      sensor_msgs::image_encodings::MONO8);

-  ros::Time start_time = ros::Time::now();
-
-
-  cv::Mat new_cam0;
-  cv::Mat new_cam1;
-
-  image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
-  image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-  
-  new_cam0.copyTo(cam0_curr_img_ptr->image);
-  new_cam1.copyTo(cam1_curr_img_ptr->image);
-
-  //ROS_INFO("Publishing: %f",
-  //    (ros::Time::now()-start_time).toSec());
-
  // Build the image pyramids once since they're used at multiple places
  createImagePyramids();

  // Detect features in the first frame.
  if (is_first_img) {
-    start_time = ros::Time::now();
+    ros::Time start_time = ros::Time::now();
    initializeFirstFrame();
    //ROS_INFO("Detection time: %f",
    //    (ros::Time::now()-start_time).toSec());
@ -257,7 +237,7 @@ void ImageProcessor::stereoCallback(
    //    (ros::Time::now()-start_time).toSec());
  } else {
    // Track the feature in the previous image.
-    start_time = ros::Time::now();
+    ros::Time start_time = ros::Time::now();
    trackFeatures();
    //ROS_INFO("Tracking time: %f",
    //    (ros::Time::now()-start_time).toSec());
@ -265,7 +245,6 @@ void ImageProcessor::stereoCallback(
    // Add new features into the current image.
    start_time = ros::Time::now();
    addNewFeatures();
-    
    //ROS_INFO("Addition time: %f",
    //    (ros::Time::now()-start_time).toSec());

@ -288,18 +267,16 @@ void ImageProcessor::stereoCallback(
  //    (ros::Time::now()-start_time).toSec());

  // Publish features in the current image.
-  start_time = ros::Time::now();
+  ros::Time start_time = ros::Time::now();
  publish();
  //ROS_INFO("Publishing: %f",
  //    (ros::Time::now()-start_time).toSec());

  // Update the previous image and previous features.
-  
  cam0_prev_img_ptr = cam0_curr_img_ptr;
  prev_features_ptr = curr_features_ptr;
  std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);

-
  // Initialize the current features to empty vectors.
  curr_features_ptr.reset(new GridFeatures());
  for (int code = 0; code <
@ -307,10 +284,6 @@ void ImageProcessor::stereoCallback(
    (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
  }

-    // stop playing bagfile if printing images
-  //if(STREAMPAUSE)
-  //  nh.setParam("/play_bag_image", true);
-
  return;
 }

@ -607,7 +580,6 @@ void ImageProcessor::trackFeatures() {
    ++after_ransac;
  }

-
  // Compute the tracking rate.
  int prev_feature_num = 0;
  for (const auto& item : *prev_features_ptr)
@ -647,14 +619,9 @@ void ImageProcessor::stereoMatch(
    image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model,
                    cam0.distortion_coeffs, cam0_points_undistorted,
                    R_cam0_cam1);
-<<<<<<< HEAD
-    cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
-                                cam1_distortion_model, cam1_distortion_coeffs);
-=======

    cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1.intrinsics,
                                cam1.distortion_model, cam1.distortion_coeffs);
->>>>>>> photometry-jakobi
  }

  // Track features using LK optical flow method.
@ -692,8 +659,6 @@ void ImageProcessor::stereoMatch(

  // Further remove outliers based on the known
  // essential matrix.
-
-
  vector<cv::Point2f> cam0_points_undistorted(0);
  vector<cv::Point2f> cam1_points_undistorted(0);
  image_handler::undistortPoints(
@ -703,7 +668,6 @@ void ImageProcessor::stereoMatch(
      cam1_points, cam1.intrinsics, cam1.distortion_model,
      cam1.distortion_coeffs, cam1_points_undistorted);

-
  double norm_pixel_unit = 4.0 / (
      cam0.intrinsics[0]+cam0.intrinsics[1]+
      cam1.intrinsics[0]+cam1.intrinsics[1]);
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
--- a/src/shrinkImage.py
+++ b/src/shrinkImage.py
@ -1,75 +0,0 @@
-#!/usr/bin/env python
-from __future__ import print_function
-
-import sys
-import rospy
-import cv2
-from std_msgs.msg import String
-from sensor_msgs.msg import Image
-from cv_bridge import CvBridge, CvBridgeError
-
-class image_converter:
-
-  def __init__(self):
-    self.image0_pub = rospy.Publisher("/cam0/new_image_raw",Image, queue_size=10)
-    self.image1_pub = rospy.Publisher("/cam1/new_image_raw",Image, queue_size=10)
-
-    self.bridge = CvBridge()
-    self.image0_sub = rospy.Subscriber("/cam0/image_raw",Image,self.callback_cam0)
-    self.image1_sub = rospy.Subscriber("/cam1/image_raw",Image,self.callback_cam1)
-
-  def callback_cam0(self,data):
-    try:
-      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
-    except CvBridgeError as e:
-      print(e)
-
-    imgScale = 0.25
-    newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
-    newimg = cv2.resize(cv_image,(int(newX),int(newY)))
-
-    newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
-    newdata = data
-    newdata.height = newpub.height
-    newdata.width = newpub.width
-    newdata.step = newpub.step
-    newdata.data = newpub.data
-    try:
-      self.image0_pub.publish(newdata)
-    except CvBridgeError as e:
-      print(e)
-
-  def callback_cam1(self,data):
-    try:
-      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
-    except CvBridgeError as e:
-      print(e)
-
-    imgScale = 0.25
-    newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
-    newimg = cv2.resize(cv_image,(int(newX),int(newY)))
-
-    newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
-    newdata = data
-    newdata.height = newpub.height
-    newdata.width = newpub.width
-    newdata.step = newpub.step
-    newdata.data = newpub.data
-
-    try:
-      self.image1_pub.publish(newdata)
-    except CvBridgeError as e:
-      print(e)
-
-
-def main(args):
-  ic = image_converter()
-  rospy.init_node('image_converter', anonymous=True)
-  try:
-    rospy.spin()
-  except KeyboardInterrupt:
-    print("Shutting down")
-  cv2.destroyAllWindows()
-
-if __name__ == '__main__':
-    main(sys.argv)
Author	SHA1	Message	Date
Raphael Maenle	2dac361ba2	fixed stop - go functionality via bagcontrol	2019-06-28 09:40:59 +02:00
Raphael Maenle	0712a98c7f	Merge branch 'photometry-jakobi-htest' of https://gitlab.com/Hippocampus/msckf into photometry-jakobi-htest	2019-06-25 12:08:41 +02:00
g-spacewhale	6b8dce9876	removed constant logging	2019-06-19 09:29:22 +02:00
g-spacewhale	49374a4323	added direct rho estimation	2019-06-14 12:37:06 +02:00
Raphael Maenle	3e480560e8	Update msckf_vio.cpp	2019-06-12 09:25:38 +00:00
g-spacewhale	b3df525060	made residual change more direct. starts diverging but then sometimes appriximates correctly after some camera movement	2019-06-12 09:26:34 +02:00
g-spacewhale	a8d4580812	removed some groundtruth measurement processings	2019-06-11 17:03:05 +02:00
g-spacewhale	a0577dfb9d	added some feature calculations to this branch	2019-06-11 16:45:17 +02:00
g-spacewhale	8cfbe06945	minor calcualtion changes - works at 7-8 fps bagfiles speed	2019-05-28 14:12:22 +02:00
g-spacewhale	cab56d9494	tested bug: filter converges longer without distortion effects; reason probably general formulation inconsistencies	2019-05-22 10:57:31 +02:00
g-spacewhale	5e9149eacc	constructed jacobian based on residual: r = z(measurement pix. pos) - h(rho, x_l, x_a) - based on the projection of the feature point into space based on the anchor frame - followed by projection onto H_rho nullspace; works well enough	2019-05-21 14:50:53 +02:00
g-spacewhale	e4dbe2f060	removed error in photom. res. calculation	2019-05-21 14:23:49 +02:00
g-spacewhale	6b208dbc44	added changed formulation, no positive result	2019-05-16 15:53:07 +02:00