fix ar_demo image interface

ar_demo/launch/realsense_ar.launch

@@ -0,0 +1,11 @@
+<launch>
+	<include file="$(find vins_estimator)/launch/realsense_fisheye.launch"/>
+
+    <node pkg="ar_demo" type="ar_demo_node" name="ar_demo_node" output="screen">
+        <remap from="~image_raw" to="/camera/fisheye/image_raw"/>
+        <remap from="~camera_pose" to="/vins_estimator/camera_pose"/>
+        <remap from="~pointcloud" to="/vins_estimator/point_cloud"/>
+        <param name="calib_file" type="string" value="$(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml"/>
+        <param name="use_undistored_img" type="bool" value="false"/>
+    </node>
+</launch>

ar_demo/src/ar_demo_node.cpp

@@ -25,6 +25,8 @@
 #include <queue>
 #include <cmath>
 #include <algorithm> 
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
 
 using namespace std;
 using namespace Eigen;
@@ -380,9 +382,25 @@ void callback(const ImageConstPtr& img_msg, const nav_msgs::Odometry::ConstPtr p
 
    project_object(camera_p, camera_q);
 
-   cv_bridge::CvImagePtr bridge_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
+   cv_bridge::CvImageConstPtr ptr;
+   if (img_msg->encoding == "8UC1")
+   {
+       sensor_msgs::Image img;
+       img.header = img_msg->header;
+       img.height = img_msg->height;
+       img.width = img_msg->width;
+       img.is_bigendian = img_msg->is_bigendian;
+       img.step = img_msg->step;
+       img.data = img_msg->data;
+       img.encoding = "mono8";
+       ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
+   }
+   else
+       ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
+
+   //cv_bridge::CvImagePtr bridge_ptr = cv_bridge::toCvCopy(ptr, sensor_msgs::image_encodings::MONO8);
    cv::Mat AR_image;
-   AR_image = bridge_ptr->image.clone();
+   AR_image = ptr->image.clone();
    cv::cvtColor(AR_image, AR_image, cv::COLOR_GRAY2RGB);
    draw_object(AR_image);
 
@@ -501,14 +519,14 @@ int main( int argc, char** argv )
         sub_img = n.subscribe("image_raw", 100, img_callback);
     }
 
-    Axis[0] = Vector3d(0, 2, -1.2);
+    Axis[0] = Vector3d(0, 1.5, -1.2);
     Axis[1]= Vector3d(-10, 5, 0);
     Axis[2] = Vector3d(3, 3, 3);
     Axis[3] = Vector3d(-2, 2, 0);
     Axis[4]= Vector3d(5, 10, -5);
     Axis[5] = Vector3d(0, 10, -1);
 
-    Cube_center[0] = Vector3d(-2, 0, -1.2 + box_length / 2.0);
+    Cube_center[0] = Vector3d(0, 1.5, -1.2 + box_length / 2.0);
     //Cube_center[0] = Vector3d(0, 3, -1.2 + box_length / 2.0);
     Cube_center[1] = Vector3d(4, -2, -1.2 + box_length / 2.0);
     Cube_center[2] = Vector3d(0, -2, -1.2 + box_length / 2.0);

config/realsense/realsense_fisheye_config.yaml

@@ -41,7 +41,7 @@ extrinsicTranslation: !!opencv-matrix
    data: [0.00188568, 0.00123801, 0.01044055]
 
 #feature traker paprameters
-max_cnt: 150            # max feature number in feature tracking
+max_cnt: 120            # max feature number in feature tracking
 min_dist: 30            # min distance between two features 
 freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
 F_threshold: 1.0        # ransac threshold (pixel)
@@ -78,4 +78,4 @@ rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per
 #visualization parameters
 save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
 visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4      # size of camera marker in RVIZ
+visualize_camera_size: 0.4      # size of camera marker in RVIZ