diff --git a/CMakeLists.txt b/CMakeLists.txt index 595f3ca..d47895e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,7 +24,7 @@ set (CMAKE_CXX_FLAGS "-DPCL_ONLY_CORE_POINT_TYPES=ON -DNO_EXPLICIT_INSTANTIATION # Eigen include_directories( "/usr/include/eigen3" ) # OpenCV -find_package( OpenCV 4.0 REQUIRED ) +find_package( OpenCV 3.4 REQUIRED ) include_directories( ${OpenCV_INCLUDE_DIRS} ) # Sophus find_package( Sophus REQUIRED ) @@ -44,7 +44,8 @@ add_definitions( ${PCL_DEFINITIONS} ) set( THIRD_PARTY_LIBS ${OpenCV_LIBS} ${PCL_LIBRARIES} - libSophus.so + ${Sophus_LIBRARIES} # If "make install" is good, use this line + libSophus.so # If "make install" failed, copy files manully to usr/lib and usr/include, and use this line g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension ${CSPARSE_LIBRARY} ) diff --git a/README.md b/README.md index 7aeca58..3c37548 100644 --- a/README.md +++ b/README.md @@ -168,11 +168,18 @@ It's based on Eigen, and contains datatypes for Lie Group and Lie Algebra (SE3/S Download this lib here: https://github.com/strasdat/Sophus. Do cmake and make. Since I failed to make install it, I manually moved “/Sophus/sophus” to “/usr/include/sophus”, and moved “libSophus.so” to “usr/lib”. Then, in my CMakeLists.txt, I add this: `set (THIRD_PARTY_LIBS libSophus.so )`. +If there is an error of "unit_complex_.real() = 1.;" +replace it and its following line with "unit_complex_ = std::complex(1,0);" + + **(4) g2o** -Download here: https://github.com/RainerKuemmerle/g2o. Checkout to the last version in year 2017. Do cmake, make, make install. -If the csparse library is not found during cmake, please install the following package: +First install either of the following two packages: > $ sudo apt-get install libsuitesparse +> $ sudo apt-get install libsuitesparse-dev + +Download here: https://github.com/RainerKuemmerle/g2o. +Checkout to the last version in year 2017. Do cmake, make, make install. # 4. How to Run > $ mkdir build && mkdir lib && mkdir bin diff --git a/config/config.yaml b/config/config.yaml index 19ee7cb..9ec8504 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -3,15 +3,24 @@ # ===== Settings for displaying MAX_NUM_IMAGES: 300 PCL_WAIT_FOR_KEY_PRESS: "false" # If true, PCL Viewer will stop and wait for any of your keypress before continueing. + +# ===== Save/Load trajectory DRAW_GROUND_TRUTH_TRAJ: "true" # Ground truth traj's color is set as green. Estimated is set as white. -GROUND_TRUTH_TRAJ_FILENAME: /home/feiyu/Desktop/slam/my_vo/my2/data/test_data/cam_traj_truth.txt -FILENAME_FOR_RESULT_TRAJECTORY: /home/feiyu/Desktop/slam/my_vo/my2/data/test_data/cam_traj.txt + +GROUND_TRUTH_TRAJ_FILENAME: /home/feiyu/Documents/Projects/EECS432_CV_VO/data/test_data/cam_traj_truth.txt +# Format of ground-truth data: +# In each row, there are 12 numbers representing the transformation matrix T=[R|t]. +# Order of the 12 numbers: tx, ty, tz, R[:,0], R[:,1], R[:,2] +# Or say: T[0,3], T[1,3], T[2,3], T[0,0],T[1,0],T[2,0],T[0,1],T[1,1],T[2,1],T[0,2],T[1,2],T[2,2] +# See this "readPoseFromFile" in "io.cpp" for more details. + +FILENAME_FOR_RESULT_TRAJECTORY: /home/feiyu/Documents/Projects/EECS432_CV_VO/data/test_data/cam_traj.txt # ===== Dataset and camera intrinsics # -- fr1_desk dataset # https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats -# dataset_dir: /home/feiyu/Desktop/slam/my_vo/my2/data/dataset_images_fr1_desk +# dataset_dir: /home/feiyu/Documents/Projects/EECS432_CV_VO/data/dataset_images_fr1_desk # num_images: 100 # camera_info.fx: 517.3 # camera_info.fy: 516.5 @@ -19,7 +28,7 @@ FILENAME_FOR_RESULT_TRAJECTORY: /home/feiyu/Desktop/slam/my_vo/my2/data/test_dat # camera_info.cy: 249.7 # -- fr1_xyz dataset -# dataset_dir: /home/feiyu/Desktop/slam/my_vo/my2/data/dataset_images_fr1_xyz +# dataset_dir: /home/feiyu/Documents/Projects/EECS432_CV_VO/data/dataset_images_fr1_xyz # num_images: 100 # camera_info.fx: 517.3 # camera_info.fy: 516.5 @@ -27,7 +36,7 @@ FILENAME_FOR_RESULT_TRAJECTORY: /home/feiyu/Desktop/slam/my_vo/my2/data/test_dat # camera_info.cy: 249.7 # -- New Tsukuba Stereo dataset used in matlab tutorial -dataset_dir: /home/feiyu/Desktop/slam/my_vo/my2/data/dataset_images_matlab +dataset_dir: /home/feiyu/Documents/Projects/EECS432_CV_VO/data/dataset_images_matlab num_images: 150 camera_info.fx: 615 camera_info.fy: 615 diff --git a/include/my_basics/io.h b/include/my_basics/io.h index 016a7ec..50fe7f2 100644 --- a/include/my_basics/io.h +++ b/include/my_basics/io.h @@ -18,7 +18,7 @@ cv::Mat readCameraIntrinsics(const string &path_of_config_file, bool print_res=f // -- Read/Write camera pose to file void writePoseToFile(const string filename, vector list_T); // Format: x, y, z, 1st row of R, 2nd row of R, 3rd row of R -vector readPoseToFile(const string filename); // Read pose from file +vector readPoseFromFile(const string filename); // Read pose from file } // namespace my_basics diff --git a/include/my_slam/frame.h b/include/my_slam/frame.h index 4cb34e1..8e6f9c3 100644 --- a/include/my_slam/frame.h +++ b/include/my_slam/frame.h @@ -56,7 +56,7 @@ class Frame my_geometry::Camera::Ptr camera_; // -- Current pose - Mat T_w_c_; // transform from camera to world + Mat T_w_c_; // transform from world to camera public: Frame() {} diff --git a/python_tools/calibrate_camera.py b/python_tools/calibrate_camera.py index daee057..90bdba5 100644 --- a/python_tools/calibrate_camera.py +++ b/python_tools/calibrate_camera.py @@ -6,7 +6,7 @@ import glob # for getting files' names in the disk # import pickle # for saving variables to disk -IMAGE_FOLDER='/home/feiyu/Desktop/slam/my_vo/my2/data/fr1_rgb_calibration/' +IMAGE_FOLDER='/home/feiyu/Documents/Projects/EECS432_CV_VO/data/fr1_rgb_calibration/' # termination criteria diff --git a/python_tools/undistort_all_images.py b/python_tools/undistort_all_images.py index 60ec969..fb2202b 100644 --- a/python_tools/undistort_all_images.py +++ b/python_tools/undistort_all_images.py @@ -5,8 +5,8 @@ import glob, os if 1: # fr1 dataset - INPUT_FOLDER = '/home/feiyu/Desktop/slam/my_vo/my2/data/dataset_images_fr1_xyz/' - OUTPUT_FOLDER = '/home/feiyu/Desktop/slam/my_vo/my2/data/undist/' + INPUT_FOLDER = '/home/feiyu/Documents/Projects/EECS432_CV_VO/data/dataset_images_fr1_xyz/' + OUTPUT_FOLDER = '/home/feiyu/Documents/Projects/EECS432_CV_VO/data/undist/' camera_intrinsics = np.array([ [517.3, 0, 325.1], diff --git a/src/io.cpp b/src/io.cpp index 8a10ad4..ab4cbdb 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -88,7 +88,7 @@ void writePoseToFile(const string filename, vector list_T) } // Read pose from file -vector readPoseToFile(const string filename) +vector readPoseFromFile(const string filename) { // Output vector list_T; diff --git a/src_main/run_vo.cpp b/src_main/run_vo.cpp index eaebde2..0b79b1f 100644 --- a/src_main/run_vo.cpp +++ b/src_main/run_vo.cpp @@ -55,7 +55,7 @@ int main(int argc, char **argv) vector image_paths; if (DEBUG_MODE) { - string folder = "/home/feiyu/Desktop/slam/my_vo/my2/data/test_data/"; + string folder = "/home/feiyu/Documents/Projects/EECS432_CV_VO/data/test_data/"; vector tmp{ "image0001.jpg", "image0013.jpg", "image0015.jpg"}; for (string &filename : tmp) @@ -219,7 +219,7 @@ bool drawResultByPcl(const my_slam::VisualOdometry::Ptr vo, my_slam::Frame::Ptr if (DRAW_GROUND_TRUTH_TRAJ) { static string GROUND_TRUTH_TRAJ_FILENAME = my_basics::Config::get("GROUND_TRUTH_TRAJ_FILENAME"); - static vector truth_poses = readPoseToFile(GROUND_TRUTH_TRAJ_FILENAME); + static vector truth_poses = readPoseFromFile(GROUND_TRUTH_TRAJ_FILENAME); cv::Mat truth_T = truth_poses[frame->id_], truth_R_vec, truth_t; getRtFromT(truth_T, truth_R_vec, truth_t);