Fix hyperlink

Author: Wallong

README.md

@@ -2,9 +2,11 @@
 ## Visual-Inertial Odometry Tightly Coupled with Wheel Encoder and GPS
 This repo couples wheel encoder data and GPS data on the basis of [VINS_Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). The project is tested on [KAIST](https://irap.kaist.ac.kr/dataset/) dataset and is suitable for automatic driving scenario.
 
+<img src="https://github.com/Wallong/VINS-GPS-Wheel/blob/master/support_files/image/kaist.png" width = 80% height = 80% div align=middle />
+
 The wheel encoder data is tightly coupled, referred to the paper[[1]](https://ieeexplore.ieee.org/abstract/document/8967607). GPS fusion adopts loose coupling, and the fusion method is consistent with [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion).
 
-Detailed derivations can be found in: https://blog.csdn.net/ewtewtewrt/article/details/
+Detailed derivations can be found in: https://blog.csdn.net/ewtewtewrt/article/details/117249295
 
 # Install
 ## 1. Prerequisites
@@ -37,7 +39,7 @@ Open four terminals, launch the vins_estimator, rviz and pubish the data file re
     roslaunch vins_estimator kaist.launch 
     rosrun multisensor_fusion multisensor_fusion_node (optional, for GPS)
     rosrun vins_estimator kaist_pub YOUR_PATH_TO_DATASET/KAIST/urban28/urban28-pankyo
-    roslaunch vins vins_rviz.launch
+    roslaunch vins_estimator vins_rviz.launch
 ```
 
 ## References

config/KAIST/KAIST_mono_imu_config.yaml

@@ -49,17 +49,17 @@ extrinsicTranslation_ic: !!opencv-matrix
 multiple_thread: 1
 
 #feature traker paprameters
-max_cnt: 150            # max feature number in feature tracking
+max_cnt: 200            # 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)
 show_track: 1           # publish tracking image as topic
 flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy
 
 #optimization parameters
-max_solver_time: 0.2  # max solver itration time (ms), to guarantee real time
-max_num_iterations: 20   # max solver itrations, to guarantee real time
-keyframe_parallax: 5.0 # keyframe selection threshold (pixel)
+max_solver_time: 0.15  # max solver itration time (ms), to guarantee real time
+max_num_iterations: 15   # max solver itrations, to guarantee real time
+keyframe_parallax: 5  # keyframe selection threshold (pixel)
 
 #imu parameters       The more accurate parameters you provide, the better performance
 acc_n: 0.1          # accelerometer measurement noise standard deviation. 

multisensor_fusion/src/multisensorFusionNode.cpp

@@ -31,8 +31,8 @@ int main(int argc, char *argv[])
 
     globalPath = &multisensorOptimization.globalPath;
 
-    ros::Subscriber sub_GPS = nh.subscribe("/gps/data_raw", 1000, GPS_callback);
-    ros::Subscriber sub_vio = nh.subscribe("/vins_estimator/odometry", 1000, vio_callback);
+    ros::Subscriber sub_GPS = nh.subscribe("/gps/data_raw", 100, GPS_callback);
+    ros::Subscriber sub_vio = nh.subscribe("/vins_estimator/odometry", 100, vio_callback);
 
     globalPathPub = nh.advertise<nav_msgs::Path>("global_path", 100);
     globalOdomPub = nh.advertise<nav_msgs::Odometry>("global_odometry", 100);

multisensor_fusion/src/optimization/multisensorOpt.cpp

@@ -90,8 +90,8 @@ void MultisensorOptimization::optimize()
             options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
             //options.minimizer_progress_to_stdout = true;
             //options.max_solver_time_in_seconds = SOLVER_TIME * 3;
-            options.max_num_iterations = 20;
-            options.max_solver_time_in_seconds = 0.3;
+            options.max_num_iterations = 5;
+            options.max_solver_time_in_seconds = 0.15;
             ceres::Solver::Summary summary;
             ceres::LossFunction *lossFunction;
             lossFunction = new ceres::HuberLoss(1.0);
@@ -222,7 +222,7 @@ void MultisensorOptimization::optimize()
             	    WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
             	}
             }
-            updateGlobalPath();
+            // updateGlobalPath();
             mPoseMap.unlock();
         }
         // std::chrono::milliseconds dura(1000);

support_files/image/kaist.png

@@ -0,0 +1,3 @@
+<launch>
+    <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find vins)/../config/vins_rviz_config.rviz" />
+</launch>

vins_estimator/launch/vins_rviz.launch

@@ -19,8 +19,7 @@ using namespace cv;
 using namespace Eigen;
 
 const int nDelayTimes = 2;
-string sData_path = "/home/dataset/EuRoC/MH-05/mav0/";
-string sConfig_path = "../config/";
+string sData_path = "/home/wallong/dataset/urban28/urban28-pankyo";
 
 bool LoadSensorData(string& sensor_data_file, unordered_map<string, string>* time_data_map)
 {
@@ -46,14 +45,13 @@ bool LoadSensorData(string& sensor_data_file, unordered_map<string, string>* tim
 
 int main(int argc, char *argv[])
 {
-    if(argc != 3)
+    if(argc != 2)
 	{
-		cerr << "./run_euroc PATH_TO_FOLDER/MH-05/mav0 PATH_TO_CONFIG/config \n" 
-			<< "For example: ./run_euroc /home/stevencui/dataset/EuRoC/MH-05/mav0/ ../config/"<< endl;
+		cerr << "./kaist_pub PATH_TO_FOLDER/YOUR_PATH_TO_DATASET/KAIST/urban28/urban28-pankyo \n" 
+			<< "For example: ./kaist_pub /home/wallong/dataset/urban28/urban28-pankyo"<< endl;
 		return -1;
 	}
     sData_path = argv[1];
-    sConfig_path = argv[2];
 
     ros::init(argc, argv, "KAIST_pub");
     ros::NodeHandle nh("~");
@@ -62,10 +60,10 @@ int main(int argc, char *argv[])
     ros::Publisher encoder_publisher;
     ros::Publisher gps_publisher;
 
-    imu_publisher = nh.advertise<sensor_msgs::Imu>("/imu/data_raw", 10, true);
-    img_publisher = nh.advertise<sensor_msgs::Image>("/stereo/left/image_raw", 5, true);
-    encoder_publisher = nh.advertise<custom_msgs::Encoder>("/encoder/data_raw", 10, true);
-    gps_publisher = nh.advertise<sensor_msgs::NavSatFix>("/gps/data_raw", 10, true);
+    imu_publisher = nh.advertise<sensor_msgs::Imu>("/imu/data_raw", 100, true);
+    img_publisher = nh.advertise<sensor_msgs::Image>("/stereo/left/image_raw", 100, true);
+    encoder_publisher = nh.advertise<custom_msgs::Encoder>("/encoder/data_raw", 100, true);
+    gps_publisher = nh.advertise<sensor_msgs::NavSatFix>("/gps/data_raw", 100, true);
 
     unordered_map<string, string> time_encoder_map;
     string encoder_data_path = sData_path + "/sensor_data/encoder.csv";
@@ -188,7 +186,7 @@ int main(int argc, char *argv[])
             gps_msg.altitude = std::stod(line_data_vec[3]);
             for (int i = 0; i < 9; i++)
             {
-                gps_msg.position_covariance[i] = std::stod(line_data_vec[i + 4]);
+                gps_msg.position_covariance[i] = std::stod(line_data_vec[i + 4]) / 100;
             }
             gps_publisher.publish(gps_msg);
         }