├── CHANGELIST.md ├── CMakeLists.txt ├── README.md ├── apps ├── global_cloud_align.cpp ├── global_cloud_align.h ├── keyframe_associations.cpp └── keyframe_associations.h ├── cmake-modules ├── FindG2O.cmake └── FindSuiteSparse.cmake ├── include └── rgbdtools │ ├── features │ ├── feature_detector.h │ ├── gft_detector.h │ ├── orb_detector.h │ └── star_detector.h │ ├── graph │ ├── keyframe_association.h │ ├── keyframe_graph_detector.h │ ├── keyframe_graph_solver.h │ └── keyframe_graph_solver_g2o.h │ ├── header.h │ ├── map_util.h │ ├── registration │ ├── motion_estimation.h │ ├── motion_estimation_icp_prob_model.h │ └── motion_estimation_pairwise_ransac.h │ ├── rgbd_frame.h │ ├── rgbd_keyframe.h │ ├── rgbd_util.h │ ├── rgbdtools.h │ └── types.h └── src ├── features ├── feature_detector.cpp ├── gft_detector.cpp ├── orb_detector.cpp └── star_detector.cpp ├── graph ├── keyframe_graph_detector.cpp ├── keyframe_graph_solver.cpp └── keyframe_graph_solver_g2o.cpp ├── map_util.cpp ├── registration ├── motion_estimation.cpp ├── motion_estimation_icp_prob_model.cpp └── motion_estimation_pairwise_ransac.cpp ├── rgbd_frame.cpp ├── rgbd_keyframe.cpp └── rgbd_util.cpp /CHANGELIST.md: -------------------------------------------------------------------------------- 1 | rgbdtools changelist 2 | ======================== 3 | 4 | latest 5 | ------------------------ 6 | * Updated to new g2o interface 7 | * Fixed ccny_rgbd_tools issue #17: assertion failure with grayscale images 8 | * Fixed many stray references to ROS which prevented standalone linking 9 | * Added new app skeleton for aligning point cloud maps globally (histogram-based) 10 | 11 | 0.1.0 (4/17/2013) 12 | ------------------------ 13 | * Initial release 14 | * Migrated over from ccny_rgbd 15 | 16 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(rgbdtools) 4 | set(CMAKE_CXX_FLAGS_RELEASE "-O3") 5 | set(CMAKE_BUILD_TYPE Release) 6 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) 7 | set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib) 8 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake-modules/") 9 | 10 | include_directories(${PROJECT_SOURCE_DIR}/include) 11 | 12 | #################################################### 13 | # Dependencies: 14 | find_package(PkgConfig) 15 | 16 | # Dependencies - Boost: 17 | find_package(Boost REQUIRED) 18 | include_directories(${Boost_INCLUDE_DIRS}) 19 | 20 | # Dependencies - OpenCV: 21 | FIND_PACKAGE( OpenCV REQUIRED ) 22 | include_directories(${OpenCV_INCLUDE_DIRS}) 23 | link_directories(${OpenCV_LIBRARY_DIRS}) 24 | 25 | # Dependencies - PCL: 26 | find_package(PCL REQUIRED) 27 | include_directories(${PCL_INCLUDE_DIRS}) 28 | link_directories(${PCL_LIBRARY_DIRS}) 29 | 30 | # Dependencies - Suitesparse: 31 | FIND_PACKAGE(SuiteSparse REQUIRED) 32 | include_directories(${SUITESPARSE_INCLUDE_DIRS}) 33 | 34 | # Dependencies - G2O: 35 | # Use script to find g2o. If it's not installed, set G2O_INCLUDE_DIRS manually 36 | # using -DG2O_INCLUDE_DIRS. 37 | find_package(G2O REQUIRED) 38 | include_directories(${G2O_INCLUDE_DIR}) 39 | 40 | #################################################### 41 | # Build rgbdtools library. 42 | 43 | add_library (rgbdtools SHARED 44 | src/rgbd_frame.cpp 45 | src/rgbd_keyframe.cpp 46 | src/rgbd_util.cpp 47 | src/map_util.cpp 48 | src/features/feature_detector.cpp 49 | src/features/orb_detector.cpp 50 | src/features/gft_detector.cpp 51 | src/features/star_detector.cpp 52 | src/registration/motion_estimation.cpp 53 | src/registration/motion_estimation_icp_prob_model.cpp 54 | src/registration/motion_estimation_pairwise_ransac.cpp 55 | src/graph/keyframe_graph_detector.cpp 56 | src/graph/keyframe_graph_solver.cpp 57 | src/graph/keyframe_graph_solver_g2o.cpp) 58 | 59 | target_link_libraries (rgbdtools 60 | ${Boost_LIBRARIES} 61 | ${OpenCV_LIBRARIES} 62 | ${PCL_LIBRARIES} 63 | ${SUITESPARSE_LIBRARIES} 64 | ${G2O_LIBRARIES}) 65 | 66 | install(TARGETS rgbdtools DESTINATION lib) 67 | install(DIRECTORY include/rgbdtools DESTINATION include) 68 | 69 | #################################################### 70 | # Build global cloud align applications. 71 | 72 | add_executable(global_cloud_align apps/global_cloud_align.cpp) 73 | target_link_libraries (global_cloud_align rgbdtools) 74 | install(TARGETS global_cloud_align DESTINATION bin) 75 | 76 | #################################################### 77 | # Build keyframe associations applications. 78 | 79 | add_executable(keyframe_associations apps/keyframe_associations.cpp) 80 | target_link_libraries (keyframe_associations rgbdtools) 81 | install(TARGETS keyframe_associations DESTINATION bin) 82 | 83 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | rgbdtools 2 | =================================== 3 | 4 | Ivan Dryanovski 5 | 6 | 7 | Copyright (C) 2013, City University of New York 8 | CCNY Robotics Lab 9 | 10 | 11 | Overview 12 | ----------------------------------- 13 | 14 | The stack contains tools for visual odometry and mapping using RGB-D cameras. 15 | 16 | For ROS applications built using this library, see [ccny_rgbd_tools](https://github.com/ccny-ros-pkg/ccny_rgbd_tools.git). 17 | 18 | This code is at an experimental stage, and licensed under the GPLv3 license. 19 | 20 | Installing 21 | ----------------------------------- 22 | 23 | If you are using this library as part of ROS, then refer to the 24 | [ccny_rgbd_tools](https://github.com/ccny-ros-pkg/ccny_rgbd_tools.git) 25 | installation instructions. You do not need to install anything manually. 26 | 27 | If you would like to build this library stand-alone, then follow these 28 | steps: 29 | 30 | If you don't have git installed, do so: 31 | 32 | sudo apt-get install git-core 33 | 34 | Download the stack from our repository: 35 | 36 | git clone https://github.com/ccny-ros-pkg/rgbdtools.git 37 | 38 | Install the required dependencies: 39 | 40 | * [g2o](https://github.com/RainerKuemmerle/g2o) 41 | 42 | If you are using ROS, you use the `libg2o` deb package. 43 | 44 | Next, configure and build: 45 | 46 | mkdir build 47 | cd build 48 | cmake .. 49 | make 50 | 51 | References 52 | ----------------------------------- 53 | 54 | If you use this system in your reasearch, please cite the following paper: 55 | 56 | Ivan Dryanovski, Roberto G. Valenti, Jizhong Xiao. 57 | *Fast Visual Odometry and Mapping from RGB-D Data*. 58 | 2013 International Conference on Robotics and Automation (ICRA2013). 59 | 60 | More info 61 | ----------------------------------- 62 | 63 | Videos: 64 | * Visual odometry & 3D mapping: http://youtu.be/YE9eKgek5pI 65 | * Feature viewer: http://youtu.be/kNkrPuBu8JA 66 | -------------------------------------------------------------------------------- /apps/global_cloud_align.cpp: -------------------------------------------------------------------------------- 1 | #include "global_cloud_align.h" 2 | 3 | using namespace rgbdtools; 4 | 5 | int main(int argc, char** argv) 6 | { 7 | if (argc != 3) 8 | { 9 | printUsage(argv); 10 | return -1; 11 | } 12 | 13 | std::string filename_in = argv[1]; 14 | std::string filename_out = argv[2]; 15 | 16 | // read in 17 | printf("Reading cloud\n"); 18 | PointCloudT::Ptr cloud; 19 | cloud.reset(new rgbdtools::PointCloudT()); 20 | pcl::PCDReader reader; 21 | reader.read(filename_in, *cloud); 22 | 23 | alignGlobalCloud(cloud); 24 | 25 | return 0; 26 | } 27 | 28 | void printUsage(char** argv) 29 | { 30 | std::cerr << "error: usage is " << argv[0] 31 | << " [filename_in] [filename_out]" 32 | << std::endl; 33 | } 34 | 35 | void alignGlobalCloud(const PointCloudT::Ptr& cloud) 36 | { 37 | double vgf_res = 0.01; 38 | 39 | // filter cloud 40 | printf("Filtering cloud\n"); 41 | PointCloudT::Ptr cloud_f; 42 | cloud_f.reset(new PointCloudT()); 43 | pcl::VoxelGrid vgf; 44 | vgf.setInputCloud(cloud); 45 | vgf.setLeafSize(vgf_res, vgf_res, vgf_res); 46 | vgf.filter(*cloud_f); 47 | 48 | /* 49 | // rotate 45 deg 50 | tf::Transform t; 51 | t.setOrigin(tf::Vector3(0,0,0)); 52 | tf::Quaternion q; 53 | q.setRPY(0, 0, M_PI/6.0); 54 | t.setRotation(q); 55 | pcl::transformPointCloud(*cloud_f, *cloud_f, eigenFromTf(t)); 56 | */ 57 | 58 | // show map 59 | printf("Showing map\n"); 60 | cv::Mat map_r; 61 | create2DProjectionImage(*cloud_f, map_r); 62 | cv::imshow("map_r", map_r); 63 | cv::waitKey(0); 64 | 65 | // Create the normal estimation class, and pass the input dataset to it 66 | pcl::NormalEstimation ne; 67 | ne.setInputCloud(cloud_f); 68 | 69 | // Create an empty kdtree representation, and pass it to the normal estimation object. 70 | // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). 71 | printf("Creating kd-tree\n"); 72 | pcl::search::KdTree::Ptr tree; 73 | tree.reset(new pcl::search::KdTree()); 74 | ne.setSearchMethod(tree); 75 | 76 | // Output datasets 77 | pcl::PointCloud::Ptr cloud_normals; 78 | cloud_normals.reset(new pcl::PointCloud); 79 | 80 | // Use all neighbors in a sphere of radius x cm 81 | ne.setRadiusSearch(0.05); 82 | 83 | // Compute the features 84 | printf("Estimating normals\n"); 85 | ne.compute (*cloud_normals); 86 | 87 | /* 88 | for (unsigned int i = 0; i < cloud_f->points.size(); ++i) 89 | { 90 | const PointT& p_in = cloud_f->points[i]; 91 | pcl::PointXYZRGBNormal& p_out = cloud_normals->points[i]; 92 | 93 | p_out.x = p_in.x; 94 | p_out.y = p_in.y; 95 | p_out.z = p_in.z; 96 | p_out.rgb = p_in.rgb; 97 | } 98 | 99 | // write out 100 | printf("Writing out\n"); 101 | pcl::PCDWriter writer; 102 | writer.write ("/home/idryanov/cloud_00_n.pcd", *cloud_normals); 103 | */ 104 | 105 | // create expected histogram 106 | double hist_resolution = 0.25; 107 | cv::Mat hist_exp; 108 | buildExpectedPhiHistorgtam(hist_exp, hist_resolution, 5.0); 109 | cv::Mat hist_exp_img; 110 | createImageFromHistogram(hist_exp, hist_exp_img); 111 | cv::imshow("hist_exp_img", hist_exp_img); 112 | 113 | // create histogram 114 | printf("creating histogram\n"); 115 | cv::Mat hist; 116 | buildPhiHistogram(*cloud_normals, hist, hist_resolution); 117 | 118 | // show histogram 119 | printf("showing histogram\n"); 120 | cv::Mat hist_img; 121 | createImageFromHistogram(hist, hist_img); 122 | cv::imshow("Histogram Phi", hist_img); 123 | cv::waitKey(0); 124 | 125 | // find alignement 126 | double best_angle; 127 | alignHistogram(hist, hist_exp, hist_resolution, best_angle); 128 | printf("best_angle: %f\n", best_angle); 129 | 130 | // show best aligned histogram 131 | cv::Mat hist_best; 132 | shiftHistogram(hist, hist_best, best_angle/hist_resolution); 133 | cv::Mat hist_best_img; 134 | createImageFromHistogram(hist_best, hist_best_img); 135 | cv::imshow("hist_best_img", hist_best_img); 136 | cv::waitKey(0); 137 | 138 | // derotate 139 | AffineTransform best_tf; 140 | XYZRPYToEigenAffine(0, 0, 0, 0, 0, best_angle * M_PI/180.0, best_tf); 141 | 142 | /* 143 | // derotate 144 | tf::Transform t1; 145 | t1.setOrigin(tf::Vector3(0,0,0)); 146 | tf::Quaternion q1; 147 | q1.setRPY(0, 0, best_angle * M_PI/180.0); 148 | t1.setRotation(q1); 149 | */ 150 | 151 | pcl::transformPointCloud(*cloud_f, *cloud_f, best_tf); 152 | 153 | // show map 154 | cv::Mat map_f; 155 | create2DProjectionImage(*cloud_f, map_f); 156 | cv::imshow("map_f", map_f); 157 | cv::waitKey(0); 158 | 159 | printf("Done\n"); 160 | } -------------------------------------------------------------------------------- /apps/global_cloud_align.h: -------------------------------------------------------------------------------- 1 | #ifndef RGBDTOOLS_GLOBAL_CLOUD_ALIGN_H 2 | #define RGBDTOOLS_GLOBAL_CLOUD_ALIGN_H 3 | 4 | #include "rgbdtools/rgbdtools.h" 5 | 6 | void printUsage(char** argv); 7 | 8 | void alignGlobalCloud(const rgbdtools::PointCloudT::Ptr& cloud); 9 | 10 | #endif // RGBDTOOLS_GLOBAL_CLOUD_ALIGN_H -------------------------------------------------------------------------------- /apps/keyframe_associations.cpp: -------------------------------------------------------------------------------- 1 | #include "keyframe_associations.h" 2 | 3 | using namespace rgbdtools; 4 | 5 | int main(int argc, char** argv) 6 | { 7 | // **** handle input ********************************************** 8 | 9 | if (argc != 3) 10 | { 11 | printUsage(argv); 12 | return -1; 13 | } 14 | 15 | std::string keyframe_path = argv[1]; 16 | std::string output_path = argv[2]; 17 | 18 | std::string bf_output_path = output_path + "/bf/"; 19 | std::string tree_output_path = output_path + "/tree/"; 20 | 21 | // **** load keyframes and prepare features *********************** 22 | 23 | rgbdtools::KeyframeVector keyframes; 24 | loadKeyframes(keyframes, keyframe_path); 25 | 26 | rgbdtools::KeyframeGraphDetector graph_detector; 27 | graph_detector.setVerbose(true); 28 | graph_detector.prepareFeaturesForRANSAC(keyframes); 29 | 30 | // **** brute force *********************************************** 31 | 32 | bruteForceAssociations(graph_detector, keyframes, bf_output_path); 33 | 34 | // load BF matrix and show 35 | cv::Mat bf_assoc = cv::imread(bf_output_path + "/bf_assoc.png", -1); 36 | cv::namedWindow("BF Associations", 0); 37 | cv::imshow("BF Associations", bf_assoc*255); 38 | cv::waitKey(1); 39 | 40 | // **** tree based ************************************************ 41 | 42 | printf("--------------------------------------------------------------\n"); 43 | printf("K\tN\tTOT\tFN(C)\tFN(A)\tR(C)\tR(A)\tDUR[s]\n"); 44 | printf("--------------------------------------------------------------\n"); 45 | 46 | cv::namedWindow("Tree Associations", 0); 47 | cv::namedWindow("Tree Candidates", 0); 48 | 49 | for (int n = 5; n <= 20; n+=5) 50 | for (int k = 2; k <= 10; k+=1) 51 | { 52 | treeAssociations(graph_detector, keyframes, tree_output_path, bf_assoc, k, n); 53 | } 54 | 55 | printf("--------------------------------------------------------------\n"); 56 | 57 | return 0; 58 | } 59 | 60 | void bruteForceAssociations( 61 | rgbdtools::KeyframeGraphDetector& graph_detector, 62 | rgbdtools::KeyframeVector& keyframes, 63 | const std::string& bf_output_path) 64 | { 65 | graph_detector.setCandidateGenerationMethod( 66 | rgbdtools::KeyframeGraphDetector::CANDIDATE_GENERATION_BRUTE_FORCE); 67 | graph_detector.setPairwiseMatchingMethod( 68 | rgbdtools::KeyframeGraphDetector::PAIRWISE_MATCHING_BFSAC); 69 | graph_detector.setPairwiseMatcherIndex( 70 | rgbdtools::KeyframeGraphDetector::PAIRWISE_MATCHER_LINEAR); 71 | 72 | graph_detector.setVerbose(true); 73 | graph_detector.setOutputPath(bf_output_path + "/sac_images/"); 74 | graph_detector.setSACSaveResults(true); 75 | 76 | rgbdtools::KeyframeAssociationVector associations; 77 | 78 | const clock_t start = clock(); 79 | graph_detector.buildAssociationMatrix(keyframes, associations); 80 | float dur_s = (clock() - start)/(float)CLOCKS_PER_SEC; 81 | 82 | printf("--------------------------------------------------------------\n"); 83 | printf("BF:%.1f\n", dur_s); 84 | printf("--------------------------------------------------------------\n"); 85 | 86 | // save BF matrices to file 87 | cv::imwrite(bf_output_path + "/bf_corr.png", graph_detector.getCorrespondenceMatrix()); 88 | cv::imwrite(bf_output_path + "/bf_assoc.png", graph_detector.getAssociationMatrix()); 89 | } 90 | 91 | void treeAssociations( 92 | rgbdtools::KeyframeGraphDetector& graph_detector, 93 | rgbdtools::KeyframeVector& keyframes, 94 | const std::string& tree_output_path, 95 | const cv::Mat& bf_assoc, 96 | int k, int n) 97 | { 98 | // set parameters relating to tree matchin 99 | graph_detector.setCandidateGenerationMethod( 100 | rgbdtools::KeyframeGraphDetector::CANDIDATE_GENERATION_SURF_TREE); 101 | graph_detector.setPairwiseMatchingMethod( 102 | rgbdtools::KeyframeGraphDetector::PAIRWISE_MATCHING_RANSAC); 103 | graph_detector.setPairwiseMatcherIndex( 104 | rgbdtools::KeyframeGraphDetector::PAIRWISE_MATCHER_KDTREE); 105 | graph_detector.setNCandidates(n); 106 | graph_detector.setKNearestNeighbors(k); 107 | 108 | // set output parameters 109 | graph_detector.setVerbose(false); 110 | graph_detector.setSACSaveResults(false); 111 | 112 | std::stringstream ss_tree_output_path_kn; 113 | ss_tree_output_path_kn << tree_output_path << n << "_" << k << "/"; 114 | std::string tree_output_path_kn = ss_tree_output_path_kn.str(); 115 | 116 | graph_detector.setOutputPath(tree_output_path_kn + "/sac_images/"); 117 | 118 | // build the associations 119 | rgbdtools::KeyframeAssociationVector associations; 120 | const clock_t start = clock(); 121 | graph_detector.buildAssociationMatrix(keyframes, associations); 122 | float dur_s = (clock() - start)/(float)CLOCKS_PER_SEC; 123 | 124 | // show & save matrices 125 | cv::Mat tree_cand = graph_detector.getCandidateMatrix(); 126 | cv::Mat tree_corr = graph_detector.getCorrespondenceMatrix(); 127 | cv::Mat tree_assoc = graph_detector.getAssociationMatrix(); 128 | 129 | // save BF matrices to file 130 | cv::imwrite(tree_output_path_kn + "/tree_cand.png", tree_cand); 131 | cv::imwrite(tree_output_path_kn + "/tree_corr.png", tree_corr); 132 | cv::imwrite(tree_output_path_kn + "/tree_assoc.png", tree_assoc); 133 | 134 | cv::imshow("Tree Associations", tree_assoc*255); 135 | cv::imshow("Tree Candidates", tree_cand*255); 136 | cv::waitKey(1); 137 | 138 | // print out results 139 | int fp, fn, tot; 140 | compareAssociationMatrix(bf_assoc, tree_assoc, fp, fn, tot); 141 | double fnr = (double)fn / double(tot); 142 | 143 | int fp_c, fn_c, tot_c; 144 | compareAssociationMatrix(bf_assoc, tree_cand, fp_c, fn_c, tot_c); 145 | double fnr_c = (double)fn_c / double(tot_c); 146 | 147 | printf("%d\t%d\t%d\t%d\t%d\t%.3f\t%.3f\t%.1f\n", 148 | k, n, tot, fn_c, fn, 1-fnr_c, 1-fnr, dur_s); 149 | } 150 | 151 | void printUsage(char** argv) 152 | { 153 | std::cerr << "error: usage is " << argv[0] 154 | << " [keyframe_path] [output_path]" 155 | << std::endl; 156 | } 157 | -------------------------------------------------------------------------------- /apps/keyframe_associations.h: -------------------------------------------------------------------------------- 1 | #ifndef RGBDTOOLS_KEYFRAME_ASSOCIATIONS_H 2 | #define RGBDTOOLS_KEYFRAME_ASSOCIATIONS_H 3 | 4 | #include 5 | #include 6 | #include "rgbdtools/rgbdtools.h" 7 | 8 | void printUsage(char** argv); 9 | 10 | void bruteForceAssociations( 11 | rgbdtools::KeyframeGraphDetector& graph_detector, 12 | rgbdtools::KeyframeVector& keyframes, 13 | const std::string& bf_output_path); 14 | 15 | void treeAssociations( 16 | rgbdtools::KeyframeGraphDetector& graph_detector, 17 | rgbdtools::KeyframeVector& keyframes, 18 | const std::string& tree_output_path, 19 | const cv::Mat& bf_assoc, 20 | int k, int n); 21 | 22 | #endif // RGBDTOOLS_KEYFRAME_ASSOCIATIONS_H -------------------------------------------------------------------------------- /cmake-modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Locate the g2o libraries 2 | # A general framework for graph optimization. 3 | # 4 | # This module defines 5 | # G2O_FOUND, if false, do not try to link against g2o 6 | # G2O_LIBRARIES, path to the libg2o 7 | # G2O_INCLUDE_DIR, where to find the g2o header files 8 | # 9 | # Niko Suenderhauf 10 | 11 | IF(UNIX) 12 | 13 | IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 14 | # in cache already 15 | SET(G2O_FIND_QUIETLY TRUE) 16 | ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 17 | 18 | MESSAGE(STATUS "Searching for g2o ...") 19 | 20 | FIND_PATH(G2O_INCLUDE_DIR 21 | NAMES core math_groups types 22 | PATHS 23 | /usr/local 24 | /usr 25 | PATH_SUFFIXES include/g2o include 26 | ) 27 | IF (G2O_INCLUDE_DIR) 28 | MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}") 29 | ENDIF (G2O_INCLUDE_DIR) 30 | 31 | FIND_LIBRARY(G2O_CORE_LIBRARIES 32 | NAMES g2o_core 33 | PATHS 34 | /usr/local 35 | /usr 36 | PATH_SUFFIXES lib 37 | ) 38 | FIND_LIBRARY(G2O_CLI_LIBRARIES 39 | NAMES g2o_cli 40 | PATHS 41 | /usr/local 42 | /usr 43 | PATH_SUFFIXES lib 44 | ) 45 | 46 | FIND_LIBRARY(G2O_SLAM2D_LIBRARIES 47 | NAMES g2o_types_slam2d 48 | PATHS 49 | /usr/local 50 | /usr 51 | PATH_SUFFIXES lib 52 | ) 53 | 54 | FIND_LIBRARY(G2O_SLAM3D_LIBRARIES 55 | NAMES g2o_types_slam3d 56 | PATHS 57 | /usr/local 58 | /usr 59 | PATH_SUFFIXES lib 60 | ) 61 | 62 | FIND_LIBRARY(G2O_SOLVER_CSPARSE 63 | NAMES g2o_solver_csparse 64 | PATHS 65 | /usr/local 66 | /usr 67 | PATH_SUFFIXES lib 68 | ) 69 | 70 | FIND_LIBRARY(G2O_CSPARSE_EXTENSION 71 | NAMES g2o_csparse_extension 72 | PATHS 73 | /usr/local 74 | /usr 75 | PATH_SUFFIXES lib 76 | ) 77 | 78 | SET(G2O_LIBRARIES ${G2O_CORE_LIBRARIES} 79 | ${G2O_CLI_LIBRARIES} 80 | ${G2O_SLAM2D_LIBRARIES} 81 | ${G2O_SLAM3D_LIBRARIES} 82 | ${G2O_SOLVER_CSPARSE} 83 | ${G2O_CSPARSE_EXTENSION}) 84 | 85 | IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 86 | SET(G2O_FOUND "YES") 87 | IF(NOT G2O_FIND_QUIETLY) 88 | MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}") 89 | ENDIF(NOT G2O_FIND_QUIETLY) 90 | ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 91 | IF(NOT G2O_LIBRARIES) 92 | IF(G2O_FIND_REQUIRED) 93 | message(FATAL_ERROR "Could not find libg2o!") 94 | ENDIF(G2O_FIND_REQUIRED) 95 | ENDIF(NOT G2O_LIBRARIES) 96 | 97 | IF(NOT G2O_INCLUDE_DIR) 98 | IF(G2O_FIND_REQUIRED) 99 | message(FATAL_ERROR "Could not find g2o include directory!") 100 | ENDIF(G2O_FIND_REQUIRED) 101 | ENDIF(NOT G2O_INCLUDE_DIR) 102 | ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 103 | 104 | ENDIF(UNIX) 105 | 106 | -------------------------------------------------------------------------------- /cmake-modules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find SUITESPARSE 2 | # Once done this will define 3 | # 4 | # SUITESPARSE_FOUND - system has SUITESPARSE 5 | # SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory 6 | # SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE 7 | # SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package) 8 | # SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package) 9 | # SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs 10 | # SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs 11 | # SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly 12 | 13 | IF (SUITESPARSE_INCLUDE_DIRS) 14 | # Already in cache, be silent 15 | SET(SUITESPARSE_FIND_QUIETLY TRUE) 16 | ENDIF (SUITESPARSE_INCLUDE_DIRS) 17 | 18 | if( WIN32 ) 19 | # Find cholmod part of the suitesparse library collection 20 | 21 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 22 | PATHS "C:\\libs\\win32\\SuiteSparse\\Include" ) 23 | 24 | # Add cholmod include directory to collection include directories 25 | IF ( CHOLMOD_INCLUDE_DIR ) 26 | list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) 27 | ENDIF( CHOLMOD_INCLUDE_DIR ) 28 | 29 | 30 | # find path suitesparse library 31 | FIND_PATH( SUITESPARSE_LIBRARY_DIRS 32 | amd.lib 33 | PATHS "C:\\libs\\win32\\SuiteSparse\\libs" ) 34 | 35 | # if we found the library, add it to the defined libraries 36 | IF ( SUITESPARSE_LIBRARY_DIRS ) 37 | list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd ) 38 | ENDIF( SUITESPARSE_LIBRARY_DIRS ) 39 | 40 | else( WIN32 ) 41 | IF( APPLE) 42 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 43 | PATHS /opt/local/include/ufsparse ) 44 | 45 | FIND_PATH( SUITESPARSE_LIBRARY_DIR 46 | NAMES libSuiteSparse.dylib 47 | PATHS /opt/local/lib ) 48 | 49 | list ( APPEND SUITESPARSE_LIBRARY_DIRS ${SUITESPARSE_LIBRARY_DIR} ) 50 | 51 | list ( APPEND SUITESPARSE_LIBRARIES SuiteSparse) 52 | 53 | ELSE(APPLE) 54 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 55 | PATHS /usr/local/include 56 | /usr/include 57 | /usr/include/suitesparse/ 58 | ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod 59 | PATH_SUFFIXES cholmod/ CHOLMOD/ ) 60 | 61 | 62 | FIND_PATH( SUITESPARSE_LIBRARY_DIR 63 | NAMES libcholmod.so 64 | PATHS /usr/lib 65 | /usr/lib64 66 | /usr/local/lib ) 67 | 68 | 69 | ENDIF(APPLE) 70 | 71 | # Add cholmod include directory to collection include directories 72 | IF ( CHOLMOD_INCLUDE_DIR ) 73 | list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) 74 | ENDIF( CHOLMOD_INCLUDE_DIR ) 75 | 76 | 77 | # if we found the library, add it to the defined libraries 78 | IF ( SUITESPARSE_LIBRARY_DIR ) 79 | 80 | # Skipped, as this is set for apple in the block above 81 | if (NOT APPLE) 82 | list ( APPEND SUITESPARSE_LIBRARIES amd) 83 | list ( APPEND SUITESPARSE_LIBRARIES btf) 84 | list ( APPEND SUITESPARSE_LIBRARIES camd) 85 | list ( APPEND SUITESPARSE_LIBRARIES ccolamd) 86 | list ( APPEND SUITESPARSE_LIBRARIES cholmod) 87 | list ( APPEND SUITESPARSE_LIBRARIES colamd) 88 | # list ( APPEND SUITESPARSE_LIBRARIES csparse) 89 | list ( APPEND SUITESPARSE_LIBRARIES cxsparse) 90 | list ( APPEND SUITESPARSE_LIBRARIES klu) 91 | # list ( APPEND SUITESPARSE_LIBRARIES spqr) 92 | list ( APPEND SUITESPARSE_LIBRARIES umfpack) 93 | endif() 94 | 95 | # Metis and spqr are optional 96 | FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY 97 | NAMES metis 98 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 99 | IF (SUITESPARSE_METIS_LIBRARY) 100 | list ( APPEND SUITESPARSE_LIBRARIES metis) 101 | ENDIF(SUITESPARSE_METIS_LIBRARY) 102 | 103 | if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp") 104 | SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid") 105 | else() 106 | SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid") 107 | endif() 108 | 109 | if(SUITESPARSE_SPQR_VALID) 110 | FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY 111 | NAMES spqr 112 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 113 | IF (SUITESPARSE_SPQR_LIBRARY) 114 | list ( APPEND SUITESPARSE_LIBRARIES spqr) 115 | ENDIF (SUITESPARSE_SPQR_LIBRARY) 116 | endif() 117 | 118 | ENDIF( SUITESPARSE_LIBRARY_DIR ) 119 | 120 | endif( WIN32 ) 121 | 122 | 123 | IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 124 | IF(WIN32) 125 | list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig ) 126 | ENDIF(WIN32) 127 | SET(SUITESPARSE_FOUND TRUE) 128 | ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 129 | SET( SUITESPARSE_FOUND FALSE ) 130 | ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 131 | 132 | -------------------------------------------------------------------------------- /include/rgbdtools/features/feature_detector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file feature_detector.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_FEATURE_DETECTOR_H 25 | #define RGBDTOOLS_FEATURE_DETECTOR_H 26 | 27 | #include 28 | #include 29 | 30 | #include "rgbdtools/types.h" 31 | #include "rgbdtools/rgbd_frame.h" 32 | 33 | namespace rgbdtools { 34 | 35 | /** @brief Base class for sparse feature extractors 36 | */ 37 | class FeatureDetector 38 | { 39 | public: 40 | 41 | /** @brief Default constructor 42 | */ 43 | FeatureDetector(); 44 | 45 | /** @brief Default destructor 46 | */ 47 | virtual ~FeatureDetector(); 48 | 49 | /** @brief Main function to call to detect the sparse features 50 | * in an RGBDFrame and fill out the corresponding information 51 | * @param frame the input frame 52 | */ 53 | void findFeatures(RGBDFrame& frame); 54 | 55 | /** @brief Returns the smoothing size. 56 | * 57 | * Smoothing is performed using Gaussian bluring in a window of size 58 | * smooth*2 + 1 59 | * 60 | * If smooth is set to 0, then no blurring will take place 61 | * 62 | * @return smoothing window size 63 | */ 64 | inline int getSmooth() const; 65 | 66 | /** @brief Returns the maximum allowed z-depth (in meters) for features 67 | * @return maximum allowed z-depth (in meters) for features 68 | */ 69 | inline double getMaxRange() const; 70 | 71 | /** @brief Returns the maximum allowed std_dev(z) (in meters) for features 72 | * @return maximum allowed std_dev(z) (in meters) for features 73 | */ 74 | inline double getMaxStDev() const; 75 | 76 | /** @brief Sets the smoothing size. 77 | * 78 | * Smoothing is performed using Gaussian bluring in a window of size 79 | * smooth*2 + 1 80 | * 81 | * If smooth is set to 0, then no blurring will take place 82 | * 83 | * @param smooth smoothing window size 84 | */ 85 | void setSmooth(int smooth); 86 | 87 | /** @brief Sets the maximum allowed z-depth (in meters) for features 88 | * @param max_range maximum allowed z-depth (in meters) for features 89 | */ 90 | void setMaxRange(double max_range); 91 | 92 | /** @brief Sets the maximum allowed std_dev(z) (in meters) for features 93 | * @param max_stdev maximum allowed std_dev(z) (in meters) for features 94 | */ 95 | void setMaxStDev(double max_stdev); 96 | 97 | void setComputeDescriptors(bool compute_descriptors); 98 | 99 | protected: 100 | 101 | boost::mutex mutex_; ///< state mutex 102 | 103 | bool compute_descriptors_; ///< whether to calculate feature descriptors 104 | 105 | /** @brief Implementation of the feature detector. 106 | * @param frame the input frame 107 | * @param input_img the image for feature detection, derived from the 108 | * RGB image of the frame after (optional) blurring 109 | */ 110 | virtual void findFeatures(RGBDFrame& frame, const cv::Mat& input_img) = 0; 111 | 112 | private: 113 | 114 | int smooth_; ///< blurring size (blur winddow = smooth*2 + 1) 115 | double max_range_; ///< maximum allowed z-depth (in meters) for features 116 | double max_stdev_; ///< maximum allowed std_dev(z) (in meters) for features 117 | }; 118 | 119 | typedef boost::shared_ptr FeatureDetectorPtr; 120 | 121 | } // namespace rgbdtools 122 | 123 | #endif // RGBDTOOLS_FEATURE_DETECTOR_H 124 | -------------------------------------------------------------------------------- /include/rgbdtools/features/gft_detector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gft_detector.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_GFT_DETECTOR_H 25 | #define RGBDTOOLS_GFT_DETECTOR_H 26 | 27 | #include 28 | #include 29 | 30 | #include "rgbdtools/features/feature_detector.h" 31 | 32 | namespace rgbdtools { 33 | 34 | /** @brief GoodFeaturesToTrack detector 35 | */ 36 | class GftDetector: public FeatureDetector 37 | { 38 | public: 39 | 40 | /** @brief Default constructor 41 | */ 42 | GftDetector(); 43 | 44 | /** @brief Default destructor 45 | */ 46 | ~GftDetector(); 47 | 48 | /** @brief Implementation of the feature detector. 49 | * @param frame the input frame 50 | * @param input_img the image for feature detection, derived from the 51 | * RGB image of the frame after (optional) blurring 52 | */ 53 | void findFeatures(RGBDFrame& frame, const cv::Mat& input_img); 54 | 55 | /** @brief Set the desired number of features 56 | * @param n_features desired number of features 57 | */ 58 | void setNFeatures(int n_features); 59 | 60 | /** @brief Set the minimum distance (in pixels) between the features 61 | * @param min_distance minimum distance (in pixels) between the features 62 | */ 63 | void setMinDistance(double min_distance); 64 | 65 | private: 66 | 67 | int n_features_; ///< the number of desired features 68 | double min_distance_; ///< the minimum distance (in pixels) between the features 69 | 70 | boost::shared_ptr gft_detector_; ///< OpenCV GTF detector object 71 | 72 | cv::OrbDescriptorExtractor orb_descriptor_; ///< OpenCV feature detector object 73 | }; 74 | 75 | typedef boost::shared_ptr GftDetectorPtr; 76 | 77 | } // namespace rgbdtools 78 | 79 | #endif // RGBDTOOLS_GFT_DETECTOR_H 80 | -------------------------------------------------------------------------------- /include/rgbdtools/features/orb_detector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file orb_detector.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_ORB_DETECTOR_H 25 | #define RGBDTOOLS_ORB_DETECTOR_H 26 | 27 | #include 28 | #include 29 | 30 | #include "rgbdtools/features/feature_detector.h" 31 | 32 | namespace rgbdtools { 33 | 34 | /** @brief ORB detector 35 | */ 36 | class OrbDetector: public FeatureDetector 37 | { 38 | public: 39 | 40 | /** @brief Default constructor 41 | */ 42 | OrbDetector(); 43 | 44 | /** @brief Default destructor 45 | */ 46 | ~OrbDetector(); 47 | 48 | /** @brief Implementation of the feature detector. 49 | * @param frame the input frame 50 | * @param input_img the image for feature detection, derived from the 51 | * RGB image of the frame after (optional) blurring 52 | */ 53 | void findFeatures(RGBDFrame& frame, const cv::Mat& input_img); 54 | 55 | /** @brief Sets the detection threshold 56 | * @param threshold the detection threshold 57 | */ 58 | void setThreshold(int threshold); 59 | 60 | /** @brief Sets the number of desired features 61 | * @param n_features number of desired features 62 | */ 63 | void setNFeatures(int n_features); 64 | 65 | private: 66 | 67 | int n_features_; ///< number of desired features 68 | double threshold_; ///< threshold for detection 69 | 70 | cv::OrbDescriptorExtractor orb_descriptor_; ///< OpenCV feature detector object 71 | boost::shared_ptr orb_detector_; ///< OpenCV descriptor extractor object 72 | }; 73 | 74 | typedef boost::shared_ptr OrbDetectorPtr; 75 | 76 | } // namespace rgbdtools 77 | 78 | #endif // RGBDTOOLS_ORB_DETECTOR_H 79 | -------------------------------------------------------------------------------- /include/rgbdtools/features/star_detector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file star_detector.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_STAR_DETECTOR_H 25 | #define RGBDTOOLS_STAR_DETECTOR_H 26 | 27 | #include 28 | #include 29 | 30 | #include "rgbdtools/features/feature_detector.h" 31 | 32 | namespace rgbdtools { 33 | 34 | /** @brief STAR detector 35 | */ 36 | class StarDetector: public FeatureDetector 37 | { 38 | public: 39 | 40 | /** @brief Default constructor 41 | */ 42 | StarDetector(); 43 | 44 | /** @brief Default destructor 45 | */ 46 | ~StarDetector(); 47 | 48 | /** @brief Implementation of the feature detector. 49 | * @param frame the input frame 50 | * @param input_img the image for feature detection, derived from the 51 | * RGB image of the frame after (optional) blurring 52 | */ 53 | void findFeatures(RGBDFrame& frame, const cv::Mat& input_img); 54 | 55 | /** @brief Set the minimum distance (in pixels) between the features 56 | * @param min_distance minimum distance (in pixels) between the features 57 | */ 58 | void setMinDistance(double min_distance); 59 | 60 | /** @brief Set the threshold for detection 61 | * @param threshold threshold for detection 62 | */ 63 | void setThreshold(double threshold); 64 | 65 | private: 66 | 67 | double min_distance_; ///< the minimum distance (in pixels) between the features 68 | double threshold_; ///< threshold for detection 69 | 70 | boost::shared_ptr star_detector_; ///< OpenCV detector class 71 | }; 72 | 73 | typedef boost::shared_ptr StarDetectorPtr; 74 | 75 | } // namespace rgbdtools 76 | 77 | #endif // RGBDTOOLS_STAR_DETECTOR_H 78 | -------------------------------------------------------------------------------- /include/rgbdtools/graph/keyframe_association.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_association.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_KEYFRAME_ASSOCIATION_H 25 | #define RGBDTOOLS_KEYFRAME_ASSOCIATION_H 26 | 27 | #include 28 | #include 29 | 30 | #include "rgbdtools/types.h" 31 | 32 | namespace rgbdtools { 33 | 34 | /** @brief Class representing an association between two keyframes, 35 | * used for graph-based pose alignement. 36 | * 37 | * Association types: 38 | * - VO: from visual odometry 39 | * - RANSAC: from RANSAC-based sparse feature matching 40 | * - ODOMETRY: from classical odometry sources 41 | */ 42 | 43 | class KeyframeAssociation 44 | { 45 | public: 46 | 47 | /** @brief Association types 48 | * 49 | * - VO: from visual odometry 50 | * - RANSAC: from RANSAC-based sparse feature matching 51 | * - ODOMETRY: from classical odometry sources 52 | */ 53 | enum Type {VO, RANSAC, ODOMETRY}; 54 | 55 | int kf_idx_a; ///< index of keyframe A 56 | int kf_idx_b; ///< index of keyframe B 57 | 58 | Type type; ///< source of the association 59 | 60 | std::vector matches; ///< for type=RANSAC, vector of RANSAC inliers mtaches 61 | 62 | /** @brief Transform between the two keyframe poses 63 | * 64 | * The transform is expressed in A's coordinate frame. 65 | * A and B's poses are expressed in the fixed frame. 66 | * 67 | * B.pose = A.pose * a2b 68 | */ 69 | AffineTransform a2b; 70 | }; 71 | 72 | typedef Eigen::aligned_allocator KeyframeAssociationAllocator; 73 | typedef std::vector KeyframeAssociationVector; 74 | 75 | } // namespace rgbdtools 76 | 77 | #endif // RGBDTOOLS_KEYFRAME_ASSOCIATION_H 78 | -------------------------------------------------------------------------------- /include/rgbdtools/graph/keyframe_graph_detector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_graph_detector.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H 25 | #define RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "rgbdtools/types.h" 33 | #include "rgbdtools/rgbd_keyframe.h" 34 | #include "rgbdtools/map_util.h" 35 | #include "rgbdtools/graph/keyframe_association.h" 36 | 37 | namespace rgbdtools { 38 | 39 | /** @brief Detects graph correspondences based on visual feature 40 | * matching between keyframes. 41 | */ 42 | class KeyframeGraphDetector 43 | { 44 | public: 45 | 46 | enum CandidateGenerationMethod 47 | { 48 | CANDIDATE_GENERATION_BRUTE_FORCE, 49 | CANDIDATE_GENERATION_SURF_TREE 50 | }; 51 | 52 | enum PairwiseMatchingMethod 53 | { 54 | PAIRWISE_MATCHING_BFSAC, 55 | PAIRWISE_MATCHING_RANSAC, 56 | }; 57 | 58 | enum PairwiseMatcherIndex 59 | { 60 | PAIRWISE_MATCHER_LINEAR, 61 | PAIRWISE_MATCHER_KDTREE, 62 | }; 63 | 64 | /** @brief Constructor from ROS nodehandles 65 | * @param nh the public nodehandle 66 | * @param nh_private the private nodehandle 67 | */ 68 | KeyframeGraphDetector(); 69 | 70 | /** @brief Default destructor 71 | */ 72 | virtual ~KeyframeGraphDetector(); 73 | 74 | void setVerbose(bool verbose); 75 | void setNCandidates(int n_candidates); 76 | void setKNearestNeighbors(int k_nearest_neighbors); 77 | void setNKeypoints(int n_keypoints); 78 | void setOutputPath(const std::string& output_path); 79 | void setSACSaveResults(bool sac_save_results); 80 | void setSACReestimateTf(bool sac_reestimate_tf); 81 | void setCandidateGenerationMethod(CandidateGenerationMethod candidate_method); 82 | void setPairwiseMatchingMethod(PairwiseMatchingMethod pairwsie_matching_method); 83 | void setPairwiseMatcherIndex(PairwiseMatcherIndex pairwsie_matcher_index); 84 | 85 | void setMatcherUseDescRatioTest(bool matcher_use_desc_ratio_test); 86 | void setMatcherMaxDescRatio(double matcher_max_desc_ratio); 87 | void setMatcherMaxDescDist(double matcher_max_desc_dist); 88 | 89 | const cv::Mat getAssociationMatrix() const { return association_matrix_; } 90 | const cv::Mat getCandidateMatrix() const { return candidate_matrix_; } 91 | const cv::Mat getCorrespondenceMatrix() const { return correspondence_matrix_; } 92 | const cv::Mat getMatchMatrix() const { return match_matrix_; } 93 | 94 | /** Main method for generating associatuions 95 | * @param keyframes the input vector of RGBD keyframes 96 | * @param associations reference to the output vector of associations 97 | */ 98 | void generateKeyframeAssociations( 99 | KeyframeVector& keyframes, 100 | KeyframeAssociationVector& associations); 101 | 102 | // -------------------------------------------- 103 | 104 | void prepareFeaturesForRANSAC(KeyframeVector& keyframes); 105 | 106 | void buildAssociationMatrix( 107 | const KeyframeVector& keyframes, 108 | KeyframeAssociationVector& associations); 109 | 110 | private: 111 | 112 | bool verbose_; 113 | 114 | /** @brief Maximim iterations for the RANSAC test 115 | */ 116 | int ransac_max_iterations_; 117 | 118 | /** @brief How many inliers are required to pass the RANSAC test. 119 | * 120 | * If a candidate keyframe has fewer correspondences or more, 121 | * it will not be eligible for a RANSAC test 122 | */ 123 | int sac_min_inliers_; 124 | 125 | bool matcher_use_desc_ratio_test_; 126 | 127 | double matcher_max_desc_ratio_; 128 | 129 | /** @brief Maximum distance (in descriptor space) between 130 | * two features to be considered a correspondence candidate 131 | */ 132 | double matcher_max_desc_dist_; 133 | 134 | /** @brief Maximum distance squared (in Euclidean space) between 135 | * two features to be considered a correspondence candidate 136 | */ 137 | double sac_max_eucl_dist_sq_; 138 | 139 | bool sac_reestimate_tf_; 140 | 141 | double ransac_sufficient_inlier_ratio_; 142 | 143 | double ransac_confidence_; 144 | double log_one_minus_ransac_confidence_; 145 | 146 | /** @brief If true, positive RANSAC results will be saved 147 | * to file as images with keypoint correspondences 148 | */ 149 | bool sac_save_results_; 150 | 151 | /** @brief The path where to save images if sac_save_results_ is true 152 | */ 153 | std::string output_path_; 154 | 155 | /** @brief For kd-tree based correspondences, how many candidate 156 | * keyframes will be tested agains the query keyframe using a RANSAC test 157 | */ 158 | double n_candidates_; 159 | 160 | /** @brief How many nearest neighbors are requested per keypoint 161 | */ 162 | int k_nearest_neighbors_; 163 | 164 | /** @brief Number of desired keypoints to detect in each image 165 | */ 166 | int n_keypoints_; 167 | 168 | double init_surf_threshold_; 169 | 170 | /** @brief TREE of BRUTE_FORCE */ 171 | CandidateGenerationMethod candidate_method_; 172 | 173 | PairwiseMatchingMethod pairwise_matching_method_; 174 | 175 | PairwiseMatcherIndex pairwise_matcher_index_; 176 | 177 | //------------------------------------------ 178 | 179 | /** @brief CV_8UC1, 1 if associated, 0 otherwise */ 180 | cv::Mat association_matrix_; 181 | 182 | /** @brief CV_8UC1, 1 if candidate, 0 otherwise */ 183 | cv::Mat candidate_matrix_; 184 | 185 | /** @brief CV_16UC1, for tree-based matching, contains number of inlier matches */ 186 | cv::Mat correspondence_matrix_; 187 | 188 | /** @brief CV_16UC1, for tree-based matching, contains number of total matches */ 189 | cv::Mat match_matrix_; 190 | 191 | std::vector matchers_; 192 | 193 | // ------------ 194 | 195 | void buildCandidateMatrix(const KeyframeVector& keyframes); 196 | 197 | void buildMatchMatrixSurfTree(const KeyframeVector& keyframes); 198 | 199 | void buildCandidateMatrixSurfTree(); 200 | 201 | void buildCorrespondenceMatrix( 202 | const KeyframeVector& keyframes, 203 | KeyframeAssociationVector& associations); 204 | 205 | int pairwiseMatching( 206 | int kf_idx_q, int kf_idx_t, 207 | const KeyframeVector& keyframes, 208 | DMatchVector& best_inlier_matches, 209 | Eigen::Matrix4f& best_transformation); 210 | 211 | int pairwiseMatchingBFSAC( 212 | int kf_idx_q, int kf_idx_t, 213 | const KeyframeVector& keyframes, 214 | DMatchVector& best_inlier_matches, 215 | Eigen::Matrix4f& best_transformation); 216 | 217 | int pairwiseMatchingRANSAC( 218 | int kf_idx_q, int kf_idx_t, 219 | const KeyframeVector& keyframes, 220 | DMatchVector& best_inlier_matches, 221 | Eigen::Matrix4f& best_transformation); 222 | 223 | void getCandidateMatches( 224 | const RGBDFrame& frame_q, const RGBDFrame& frame_t, 225 | cv::FlannBasedMatcher& matcher, 226 | DMatchVector& candidate_matches); 227 | 228 | void prepareMatchers( 229 | const KeyframeVector& keyframes); 230 | 231 | }; 232 | 233 | } // namespace rgbdtools 234 | 235 | #endif // RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H 236 | -------------------------------------------------------------------------------- /include/rgbdtools/graph/keyframe_graph_solver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_graph_solver.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_H 25 | #define RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_H 26 | 27 | #include "rgbdtools/rgbd_keyframe.h" 28 | #include "rgbdtools/graph/keyframe_association.h" 29 | 30 | namespace rgbdtools { 31 | 32 | /** @brief Base class for graph-based global alignment classes 33 | * 34 | * The class takes as input a vector of RGBD keyframes and a 35 | * vector of associations between them, and modifies the keyframe 36 | * poses based on the minimization of some error metric. 37 | */ 38 | class KeyframeGraphSolver 39 | { 40 | public: 41 | 42 | /** @brief Constructor 43 | */ 44 | KeyframeGraphSolver(); 45 | 46 | /** @brief Default destructor 47 | */ 48 | virtual ~KeyframeGraphSolver(); 49 | 50 | /** @brief Main method to call to perform graph solving. 51 | * 52 | * The keyframe poses will be modified according to some error 53 | * propagation model informed by the graph defined by the keyframe 54 | * associations. 55 | * 56 | * @param keyframes vector of keyframes 57 | * @param associations vector of input keyframe associations 58 | */ 59 | virtual void solve( 60 | KeyframeVector& keyframes, 61 | const KeyframeAssociationVector& associations) = 0; 62 | 63 | virtual void solve( 64 | KeyframeVector& keyframes, 65 | const KeyframeAssociationVector& associations, 66 | AffineTransformVector& path) = 0; 67 | }; 68 | 69 | } // namespace rgbdtools 70 | 71 | #endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_H 72 | -------------------------------------------------------------------------------- /include/rgbdtools/graph/keyframe_graph_solver_g2o.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_graph_solver_g2o.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H 25 | #define RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H 26 | 27 | #include "g2o/core/sparse_optimizer.h" 28 | #include "g2o/core/block_solver.h" 29 | #include "g2o/core/optimization_algorithm_levenberg.h" 30 | 31 | 32 | #include "rgbdtools/graph/keyframe_graph_solver.h" 33 | 34 | namespace rgbdtools { 35 | 36 | /** @brief Graph-based global alignement using g2o (generalized 37 | * graph optimizaiton) 38 | */ 39 | class KeyframeGraphSolverG2O: public KeyframeGraphSolver 40 | { 41 | public: 42 | 43 | /** @brief Constructor from ROS noehandles 44 | */ 45 | KeyframeGraphSolverG2O(); 46 | 47 | /** @brief Default destructor 48 | */ 49 | ~KeyframeGraphSolverG2O(); 50 | 51 | /** @brief Main method to call to perform graph solving using g2o. 52 | * 53 | * @param keyframes vector of keyframes 54 | * @param associations vector of input keyframe associations 55 | */ 56 | void solve(KeyframeVector& keyframes, 57 | const KeyframeAssociationVector& associations); 58 | 59 | void solve(KeyframeVector& keyframes, 60 | const KeyframeAssociationVector& associations, 61 | AffineTransformVector& path); 62 | 63 | private: 64 | 65 | g2o::BlockSolverX::LinearSolverType* linear_solver_; 66 | g2o::BlockSolverX* block_solver_; 67 | g2o::OptimizationAlgorithmLevenberg* optimization_algorithm_; 68 | g2o::SparseOptimizer optimizer_; 69 | 70 | int vertexIdx; 71 | 72 | /** @brief Adds a vertex to the g2o structure 73 | */ 74 | void addVertex(const AffineTransform& vertex_pose, 75 | int vertex_idx); 76 | 77 | /** @brief Adds an edge to the g2o structure 78 | */ 79 | void addEdge(int from_idx, int to_idx, 80 | const AffineTransform& relative_pose, 81 | const Eigen::Matrix& information_matrix); 82 | 83 | /** @brief runs the optimization 84 | */ 85 | void optimizeGraph(); 86 | 87 | /** @brief copies the (optimized) poses from the g2o structure into 88 | * the keyframe vector 89 | * @param keyframes the vector of keyframes to modify the poses 90 | */ 91 | //void updatePoses(KeyframeVector& keyframes); 92 | 93 | void getOptimizedPoses(AffineTransformVector& poses); 94 | }; 95 | 96 | } // namespace rgbdtools 97 | 98 | #endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H 99 | -------------------------------------------------------------------------------- /include/rgbdtools/header.h: -------------------------------------------------------------------------------- 1 | #ifndef RGBD_HEADER_H 2 | #define RGBD_HEADER_H 3 | 4 | namespace rgbdtools { 5 | 6 | struct Time { 7 | int32_t sec; 8 | int32_t nsec; 9 | }; 10 | 11 | struct Header { 12 | uint32_t seq; 13 | Time stamp; 14 | std::string frame_id; 15 | }; 16 | 17 | } // namespace rgbdtools 18 | 19 | #endif // RGBDTOOLS_HEADER_H 20 | -------------------------------------------------------------------------------- /include/rgbdtools/map_util.h: -------------------------------------------------------------------------------- 1 | #ifndef RGBDTOOLS_MAP_UTIL_H 2 | #define RGBDTOOLS_MAP_UTIL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "rgbdtools/types.h" 13 | #include "rgbdtools/rgbd_keyframe.h" 14 | 15 | namespace rgbdtools { 16 | 17 | void buildExpectedPhiHistorgtam( 18 | cv::Mat& histogram, 19 | double degrees_per_bin, 20 | double stdev); 21 | 22 | void buildPhiHistogram( 23 | const pcl::PointCloud& cloud, 24 | cv::Mat& histogram, 25 | double degrees_per_bin); 26 | 27 | void normalizeHistogram(cv::Mat& histogram); 28 | 29 | void shiftHistogram( 30 | const cv::Mat& hist_in, 31 | cv::Mat& hist_out, 32 | int bins); 33 | 34 | bool alignHistogram( 35 | const cv::Mat& hist, 36 | const cv::Mat& hist_exp, 37 | double hist_resolution, 38 | double& best_angle); 39 | 40 | void create8bitHistogram( 41 | const cv::Mat& histogram, 42 | cv::Mat& histogram_norm); 43 | 44 | void createImageFromHistogram( 45 | const cv::Mat& histogram, 46 | cv::Mat& image); 47 | 48 | void create2DProjectionImage( 49 | const PointCloudT& cloud, 50 | cv::Mat& img, 51 | double min_z = -std::numeric_limits::infinity(), 52 | double max_z = std::numeric_limits::infinity()); 53 | 54 | void filterCloudByHeight( 55 | const pcl::PointCloud& cloud_in, 56 | pcl::PointCloud& cloud_out, 57 | double min_z, 58 | double max_z); 59 | 60 | // *** matching 61 | 62 | /* 63 | void buildDenseAssociationMatrix( 64 | const KeyframeVector& keyframes, 65 | cv::Mat& association_matrix); 66 | 67 | void buildSURFAssociationMatrixBruteForce( 68 | const KeyframeVector& keyframes, 69 | cv::Mat& correspondence_matrix, 70 | int threshold); 71 | 72 | 73 | void buildSURFAssociationMatrixTree( 74 | const KeyframeVector& keyframes, 75 | cv::Mat& correspondence_matrix, 76 | int k_nearest_neighbors, 77 | int n_candidates, 78 | int threshold); 79 | 80 | void buildRANSACCorrespondenceMatrix( 81 | const KeyframeVector& keyframes, 82 | const cv::Mat& candidate_matrix, 83 | cv::Mat& correspondence_matrix); 84 | 85 | void buildSURFCandidateMatrixTree( 86 | const cv::Mat& match_matrix, 87 | cv::Mat& candidate_matrix, 88 | int n_candidates); 89 | 90 | void buildSURFMatchMatrixTree( 91 | const KeyframeVector& keyframes, 92 | cv::Mat& match_matrix, 93 | int k_nearest_neighbors); 94 | 95 | void floatMatrixToUintMatrix( 96 | const cv::Mat& mat_in, 97 | cv::Mat& mat_out, 98 | float scale = 0); 99 | 100 | void prepareFeaturesForRANSAC(KeyframeVector& keyframes); 101 | 102 | void pairwiseMatchingRANSAC( 103 | const RGBDFrame& frame_a, const RGBDFrame& frame_b, 104 | double max_eucl_dist_sq, 105 | double max_desc_dist, 106 | double sufficient_inlier_ratio, 107 | int max_ransac_iterations, 108 | std::vector& all_matches, 109 | std::vector& best_inlier_matches, 110 | Eigen::Matrix4f& best_transformation); 111 | */ 112 | 113 | void trainSURFMatcher( 114 | const KeyframeVector& keyframes, 115 | cv::FlannBasedMatcher& matcher); 116 | 117 | void getRandomIndices( 118 | int k, int n, IntVector& output); 119 | 120 | void get3RandomIndices( 121 | int n, std::set& mask, IntVector& output); 122 | 123 | double distEuclideanSq(const PointFeature& a, const PointFeature& b); 124 | 125 | void makeSymmetricOR(cv::Mat mat); 126 | 127 | void thresholdMatrix( 128 | const cv::Mat& mat_in, 129 | cv::Mat& mat_out, 130 | int threshold); 131 | 132 | void compareAssociationMatrix( 133 | const cv::Mat& a, 134 | const cv::Mat& b, 135 | int& false_pos, 136 | int& false_neg, 137 | int& total); 138 | 139 | 140 | } // namespace rgbdtools 141 | 142 | #endif // RGBDTOOLS_MAP_UTIL_H 143 | -------------------------------------------------------------------------------- /include/rgbdtools/registration/motion_estimation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motion_estimation.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_MOTION_ESTIMATION_H 25 | #define RGBDTOOLS_MOTION_ESTIMATION_H 26 | 27 | #include 28 | 29 | #include "rgbdtools/rgbd_frame.h" 30 | #include "rgbdtools/types.h" 31 | 32 | namespace rgbdtools { 33 | 34 | /** @brief Base class for visual odometry motion estimation methods 35 | * 36 | * The motion is estimated in increments of the change of pose 37 | * of the base frame. The increments are expressed wrt fixed frame. 38 | */ 39 | class MotionEstimation 40 | { 41 | public: 42 | 43 | enum MotionConstraint {NONE = 0, ROLL_PITCH = 1, ROLL_PITCH_Z = 2}; 44 | 45 | /** @brief Constructor from ROS nodehandles 46 | * @param nh the public nodehandle 47 | * @param nh_private the private nodehandle 48 | */ 49 | MotionEstimation(); 50 | 51 | /** @brief Default destructor 52 | */ 53 | virtual ~MotionEstimation(); 54 | 55 | /** @brief Main function for estimating motion 56 | * 57 | * The motion is equal to the change of pose of the base frame, and is 58 | * expressed wrt the fixed frame 59 | * 60 | * Pose_new = motion * Pose_old; 61 | * 62 | * @param frame The RGBD Frame for which the motion is estimated 63 | * @return incremental motion transform 64 | */ 65 | AffineTransform getMotionEstimation( 66 | RGBDFrame& frame, 67 | const AffineTransform& prediction = AffineTransform::Identity()); 68 | 69 | /** @brief Set the transformation between the base and camera frames. 70 | * @param b2c The transform from the base frame to the camera frame, 71 | * expressed wrt the base frame. 72 | */ 73 | void setBaseToCameraTf(const AffineTransform& b2c); 74 | 75 | void setMotionConstraint(int motion_constraint); 76 | 77 | /** @brief Return the size of the internal model. Overriden for classes 78 | * that use a model. 79 | * 80 | * @return the size of the model 81 | */ 82 | virtual int getModelSize() const { return 0; } 83 | 84 | protected: 85 | 86 | AffineTransform b2c_; ///< Base (moving) frame to Camera-optical frame 87 | 88 | int motion_constraint_; ///< The motion constraint type 89 | 90 | /** @brief Implementation of the motion estimation algorithm. 91 | * @param frame the current RGBD frame 92 | * @param prediction the motion prediction (currently ignored) 93 | * @param motion the output motion 94 | * @retval true the motion estimation was successful 95 | * @retval false the motion estimation failed 96 | */ 97 | virtual bool getMotionEstimationImpl( 98 | RGBDFrame& frame, 99 | const AffineTransform& prediction, 100 | AffineTransform& motion) = 0; 101 | 102 | /** @brief Constrains the motion in accordance to the class motion constraints 103 | * 104 | * This method can be called by the inheriting classes if desired. 105 | * 106 | * @param motion the incremental motion which is constrained. 107 | */ 108 | void constrainMotion(AffineTransform& motion); 109 | }; 110 | 111 | } // namespace rgbdtools 112 | 113 | #endif // RGBDTOOLS_MOTION_ESTIMATION_H 114 | -------------------------------------------------------------------------------- /include/rgbdtools/registration/motion_estimation_icp_prob_model.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motion_estimation_icp_prob_model.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H 25 | #define RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "rgbdtools/types.h" 32 | #include "rgbdtools/registration/motion_estimation.h" 33 | 34 | namespace rgbdtools { 35 | 36 | /** @brief Motion estimation based on aligning sparse features 37 | * against a persistent, dynamic model. 38 | * 39 | * The model is build from incoming features through a Kalman Filter 40 | * update step. 41 | */ 42 | class MotionEstimationICPProbModel: public MotionEstimation 43 | { 44 | public: 45 | 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | /** @brief Constructor from ROS noehandles 49 | * @param nh the public nodehandle 50 | * @param nh_private the private notehandle 51 | */ 52 | MotionEstimationICPProbModel(); 53 | 54 | /** @brief Default destructor 55 | */ 56 | virtual ~MotionEstimationICPProbModel(); 57 | 58 | /** @brief Main method for estimating the motion given an RGBD frame 59 | * @param frame the current RGBD frame 60 | * @param prediction the predicted motion (currently ignored) 61 | * @param motion the (output) incremental motion, wrt the fixed frame 62 | * @retval true the motion estimation was successful 63 | * @retval false the motion estimation failed 64 | */ 65 | bool getMotionEstimationImpl( 66 | RGBDFrame& frame, 67 | const AffineTransform& prediction, 68 | AffineTransform& motion); 69 | 70 | /** @brief Returns the number of points in the model built from the feature buffer 71 | * @returns number of points in model 72 | */ 73 | int getModelSize() const { return model_size_; } 74 | 75 | PointCloudFeature::Ptr getModel() { return model_ptr_; } 76 | Vector3fVector* getMeans() { return &means_; } 77 | Matrix3fVector* getCovariances() { return &covariances_; } 78 | 79 | void setMaxIterations(int max_iterations); 80 | void setMinCorrespondences(int min_correspondences); 81 | void setNNearestNeighbors(int n_nearest_neighbors); 82 | void setMaxModelSize(int max_model_size); 83 | void setTfEpsilonLinear(double tf_epsilon_linear); 84 | void setTfEpsilonAngular(double tf_epsilon_angular); 85 | void setMaxAssociationDistMahalanobis(double max_assoc_dist_mah); 86 | void setMaxCorrespondenceDistEuclidean(double max_corresp_dist_eucl); 87 | 88 | private: 89 | 90 | // **** params 91 | 92 | int max_iterations_; ///< Maximum number of ICP iterations 93 | int min_correspondences_; ///< Minimum number of correspondences for ICP to contuinue 94 | 95 | /** @brief How many euclidean neighbors to go through, in a brute force 96 | * search of the closest Mahalanobis neighbor. 97 | */ 98 | int n_nearest_neighbors_; 99 | 100 | /** @brief Upper bound for how many features to store in the model. 101 | * 102 | * New features added beyond thi spoint will overwrite old features 103 | */ 104 | int max_model_size_; 105 | 106 | double tf_epsilon_linear_; ///< Linear convergence criteria for ICP 107 | double tf_epsilon_angular_; ///< Angular convergence criteria for ICP 108 | 109 | /** @brief Maximum Mahalanobis distance for associating points 110 | * between the data and the model 111 | */ 112 | double max_assoc_dist_mah_; 113 | 114 | /** @brief Maximum Euclidean correspondce distance for ICP 115 | */ 116 | double max_corresp_dist_eucl_; 117 | 118 | 119 | /** @brief Maximum squared Mahalanobis distance for associating points 120 | * between the data and the model, derived. 121 | */ 122 | double max_assoc_dist_mah_sq_; 123 | 124 | /** @brief Maximum Euclidean correspondce distance for ICP, derived 125 | */ 126 | double max_corresp_dist_eucl_sq_; 127 | 128 | // **** variables 129 | 130 | PointCloudFeature::Ptr model_ptr_; ///< The model point cloud 131 | int model_idx_; ///< Current intex in the ring buffer 132 | int model_size_; ///< Current model size 133 | Vector3fVector means_; ///< Vector of model feature mean 134 | Matrix3fVector covariances_; ///< Vector of model feature covariances 135 | 136 | KdTree model_tree_; ///< Kdtree of model_ptr_ 137 | 138 | Matrix3f I_; ///< 3x3 Identity matrix 139 | 140 | AffineTransform f2b_; ///< Transform from fixed to moving frame 141 | 142 | // ***** funtions 143 | 144 | /** @brief Performs ICP alignment using the Euclidean distance for corresopndences 145 | * @param data_means a vector of 3x1 matrices, repesenting the 3D positions of the features 146 | * @param correction reference to the resulting transformation 147 | * @retval true the motion estimation was successful 148 | * @retval false the motion estimation failed 149 | */ 150 | bool alignICPEuclidean( 151 | const Vector3fVector& data_means, 152 | AffineTransform& correction); 153 | 154 | /** @brief Performs ICP alignment using the Euclidean distance for corresopndences 155 | * @param data_cloud a pointcloud of the 3D positions of the features 156 | * @param data_indices reference to a vector containting the resulting data indices 157 | * @param model_indices reference to a vector containting the resulting model indices 158 | */ 159 | void getCorrespEuclidean( 160 | const PointCloudFeature& data_cloud, 161 | IntVector& data_indices, 162 | IntVector& model_indices); 163 | 164 | /** @brief Finds the nearest Euclidean neighbor 165 | * @param data_point the query 3D data point 166 | * @param eucl_nn_idx reference to the resulting nearest neigbor index in the model 167 | * @param eucl_dist_sq reference to the resulting squared distance 168 | * @retval true a neighbor was found 169 | * @retval false a neighbor was not found 170 | */ 171 | bool getNNEuclidean( 172 | const PointFeature& data_point, 173 | int& eucl_nn_idx, double& eucl_dist_sq); 174 | 175 | /** @brief Finds the nearest Mahalanobis neighbor 176 | * 177 | * Requests the K nearest Euclidean neighbors (K = n_nearest_neighbors_) 178 | * using a kdtree, and performs a brute force search for the closest 179 | * Mahalanobis nighbor. Reasonable values for K are 4 or 8. 180 | * 181 | * @param data_mean 3x1 matrix of the query 3D data point mean 182 | * @param data_cov 3x3 matrix of the query 3D data point covariance 183 | * @param mah_nn_idx reference to the resulting nearest neigbor index in the model 184 | * @param mah_dist_sq reference to the resulting squared Mahalanobis distance 185 | * @param indices cache vector, pre-allocated with n_nearest_neighbors_ size 186 | * @param dists_sq cache vector, pre-allocated with n_nearest_neighbors_ size 187 | * @retval true a neighbor was found 188 | * @retval false a neighbor was not found 189 | */ 190 | bool getNNMahalanobis( 191 | const Vector3f& data_mean, const Matrix3f& data_cov, 192 | int& mah_nn_idx, double& mah_dist_sq, 193 | IntVector& indices, FloatVector& dists_sq); 194 | 195 | /** @brief Initializes the (empty) model from a set of data means and 196 | * covariances 197 | * @param data_means vector of 3x1 data point means 198 | * @param data_covariances vector of 3x3 data point covariances 199 | */ 200 | void initializeModelFromData( 201 | const Vector3fVector& data_means, 202 | const Matrix3fVector& data_covariances); 203 | 204 | /** @brief Updates the (non-empty) model from a set of data means and 205 | * covariances 206 | * 207 | * Any data points which have a Mahalanobis neighbor in the model 208 | * under a certain threshold distance get updated using a Kalman filter. 209 | * 210 | * Any data points without a Mahalanobis neighbor get insterted as new 211 | * model points. 212 | * 213 | * @param data_means vector of 3x1 data point means 214 | * @param data_covariances vector of 3x3 data point covariances 215 | */ 216 | void updateModelFromData(const Vector3fVector& data_means, 217 | const Matrix3fVector& data_covariances); 218 | 219 | /** @brief Adds a new point to the model 220 | * 221 | * The model is implemented using a rign buffer. If the number of 222 | * points in the model has reached the maximum, the new point will 223 | * overwrite the oldest model point 224 | * 225 | * @param data_mean 3x1 data point means 226 | * @param data_cov 3x3 data point covariances 227 | */ 228 | void addToModel( 229 | const Vector3f& data_mean, 230 | const Matrix3f& data_cov); 231 | 232 | bool saveModel(const std::string& filename); 233 | }; 234 | 235 | } // namespace rgbdtools 236 | 237 | #endif // RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H 238 | 239 | -------------------------------------------------------------------------------- /include/rgbdtools/registration/motion_estimation_pairwise_ransac.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motion_estimation_pairwise_ransac.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_MOTION_ESTIMATION_PAIRWISE_RANSAC_H 25 | #define RGBDTOOLS_MOTION_ESTIMATION_PAIRWISE_RANSAC_H 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "rgbdtools/types.h" 32 | #include "rgbdtools/registration/motion_estimation.h" 33 | #include "rgbdtools/features/orb_detector.h" 34 | 35 | namespace rgbdtools { 36 | 37 | /** @brief Motion estimation based on aligning sparse features 38 | * against a persistent, dynamic model. 39 | * 40 | * The model is build from incoming features through a Kalman Filter 41 | * update step. 42 | */ 43 | class MotionEstimationPairwiseRANSAC: public MotionEstimation 44 | { 45 | public: 46 | 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | /** @brief Constructor from ROS noehandles 50 | * @param nh the public nodehandle 51 | * @param nh_private the private notehandle 52 | */ 53 | MotionEstimationPairwiseRANSAC(); 54 | 55 | /** @brief Default destructor 56 | */ 57 | virtual ~MotionEstimationPairwiseRANSAC(); 58 | 59 | /** @brief Main method for estimating the motion given an RGBD frame 60 | * @param frame the current RGBD frame 61 | * @param prediction the predicted motion (currently ignored) 62 | * @param motion the (output) incremental motion, wrt the fixed frame 63 | * @retval true the motion estimation was successful 64 | * @retval false the motion estimation failed 65 | */ 66 | bool getMotionEstimationImpl( 67 | RGBDFrame& frame, 68 | const AffineTransform& prediction, 69 | AffineTransform& motion); 70 | 71 | void setFeatureDetector(FeatureDetectorPtr detector) {detector_ = detector;} 72 | 73 | private: 74 | 75 | bool initialized_; 76 | 77 | RGBDFrame prev_frame_; 78 | 79 | FeatureDetectorPtr detector_; 80 | 81 | AffineTransform f2b_; ///< Transform from fixed to moving frame 82 | 83 | // parameters 84 | 85 | int sac_min_inliers_; 86 | double sac_max_eucl_dist_sq_; 87 | bool sac_reestimate_tf_; 88 | int ransac_max_iterations_; 89 | double ransac_confidence_; 90 | double log_one_minus_ransac_confidence_; 91 | bool matcher_use_desc_ratio_test_; 92 | double matcher_max_desc_ratio_; 93 | double matcher_max_desc_dist_; 94 | 95 | int pairwiseMatchingRANSAC( 96 | const RGBDFrame& frame_q, const RGBDFrame& frame_t, 97 | DMatchVector& best_inlier_matches, 98 | Eigen::Matrix4f& best_transformation); 99 | 100 | void getCandidateMatches( 101 | const RGBDFrame& frame_q, const RGBDFrame& frame_t, 102 | cv::DescriptorMatcher& matcher, 103 | DMatchVector& candidate_matches); 104 | 105 | void getRandomIndices( 106 | int k, int n, IntVector& output) 107 | { 108 | while ((int)output.size() < k) 109 | { 110 | int random_number = rand() % n; 111 | bool duplicate = false; 112 | 113 | for (unsigned int i = 0; i < output.size(); ++i) 114 | { 115 | if (output[i] == random_number) 116 | { 117 | duplicate = true; 118 | break; 119 | } 120 | } 121 | 122 | if (!duplicate) 123 | output.push_back(random_number); 124 | } 125 | } 126 | 127 | void get3RandomIndices( 128 | int n, std::set& mask, IntVector& output) 129 | { 130 | int key; 131 | 132 | while(true) 133 | { 134 | output.clear(); 135 | getRandomIndices(3, n, output); 136 | 137 | // calculate a key based on the 3 random indices 138 | key = output[0] * n * n + output[1] * n + output[2]; 139 | 140 | //printf("%d %d %d\n", output[0], output[1], output[2]); 141 | 142 | // value not present in mask 143 | if (mask.find(key) == mask.end()) 144 | break; 145 | } 146 | 147 | mask.insert(key); 148 | } 149 | 150 | double distEuclideanSq(const PointFeature& a, const PointFeature& b) 151 | { 152 | float dx = a.x - b.x; 153 | float dy = a.y - b.y; 154 | float dz = a.z - b.z; 155 | return dx*dx + dy*dy + dz*dz; 156 | } 157 | }; 158 | 159 | } // namespace rgbdtools 160 | 161 | #endif // RGBDTOOLS_MOTION_ESTIMATION_PAIRWISE_RANSAC_H 162 | 163 | -------------------------------------------------------------------------------- /include/rgbdtools/rgbd_frame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_frame.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_RGBD_FRAME_H 25 | #define RGBDTOOLS_RGBD_FRAME_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "rgbdtools/types.h" 33 | #include "rgbdtools/header.h" 34 | #include "rgbdtools/rgbd_util.h" 35 | 36 | namespace rgbdtools { 37 | 38 | /** @brief Auxiliarry class that holds together rgb and depth images. 39 | * 40 | * The class also holds the detected keypoints and their 3D distributions. 41 | * Keypoints, descriptos, and kp_* are indexed the same way and may include 42 | * invalid points. An invalid keypoint occurs when: 43 | * - no z data 44 | * - z > threshold 45 | * - var_z > threshold 46 | */ 47 | class RGBDFrame 48 | { 49 | public: 50 | 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 52 | 53 | /** @brief Default (empty) constructor. 54 | */ 55 | RGBDFrame(); 56 | 57 | RGBDFrame(const RGBDFrame& other); 58 | 59 | /** @brief Constructor from ROS messages 60 | * @param rgb_img_in 8UC3 image message 61 | * @param depth_img_in 16UC1 ROS depth message (in mm, 0 = invalid data) 62 | * @param info_msg ROS camera info message, assumed no distortion, applies to both images 63 | */ 64 | 65 | RGBDFrame(const cv::Mat& rgb_img_in, 66 | const cv::Mat& depth_img_in, 67 | const cv::Mat& intr_in, 68 | const Header& header_in); 69 | 70 | int index; 71 | 72 | Header header; ///< Header taken from rgb_msg 73 | cv::Mat rgb_img; ///< RGB image (8UC3) 74 | cv::Mat depth_img; ///< Depth image in mm (16UC1). 0 = invalid data 75 | cv::Mat intr; 76 | 77 | /** @brief The intrinsic matrix which applies to both images. 78 | * 79 | * It's assumed that the images are already 80 | * rectified and in the same camera frame(RGB) 81 | */ 82 | //image_geometry::PinholeCameraModel model; 83 | 84 | KeypointVector keypoints; ///< 2D keypoint locations 85 | cv::Mat descriptors; ///< Feature descriptor vectors 86 | 87 | BoolVector kp_valid; ///< Is the z data valid? 88 | Vector3fVector kp_means; ///< 1x3 mat of 3D locations 89 | Matrix3fVector kp_covariances; ///< 3x3 mat of covariances 90 | 91 | int n_valid_keypoints; ///< How many keypoints have usable 3D data 92 | 93 | /** @brief Computes the 3D means and covariances for all the detected keypoints. 94 | * 95 | * Some features will be marked as invalid. 96 | * @todo do we want default values? 97 | * @param max_z [m] features with z bigger than this will be marked as invalid 98 | * @param max_stdev_z [m] features with std_dev(z) bigger than this 99 | * will be marked as invalid 100 | */ 101 | void computeDistributions( 102 | double max_z = 5.5, 103 | double max_stdev_z = 0.03); 104 | 105 | /** @brief Computes the 3D means and covariances for all the valid detected keypoints. 106 | * @param cloud Reference to the point cloud to be constructed 107 | */ 108 | void constructFeaturePointCloud(PointCloudFeature& cloud); 109 | 110 | /** @brief constructs a point cloud from the RGB and depth images 111 | * @param cloud the reference to the point cloud to be constructed 112 | * @param max_z [m] points with z bigger than this will be marked as NaN 113 | * @param max_stdev_z [m] points with std_dev(z) bigger than this 114 | * will be marked as NaN 115 | * 116 | * @todo do we want default values? or ROS parameters here) 117 | * 118 | */ 119 | void constructDensePointCloud(PointCloudT& cloud, 120 | double max_z = 5.5, 121 | double max_stdev_z = 0.03) const; 122 | 123 | /** @brief Saves the RGBD frame to disk. 124 | * 125 | * Saves the RGB and depth images as png, and the header and intrinsic matrix 126 | * as .yml files. Parent directory must exist prior to calling this function, or 127 | * function will fail. 128 | * 129 | * @param frame Reference to the frame being saved 130 | * @param path The path to the folder where everything will be stored 131 | * 132 | * @retval true Successfully saved the data 133 | * @retval false Saving failed - for example, cannot create directory 134 | */ 135 | static bool save(const RGBDFrame& frame, const std::string& path); 136 | 137 | /** @brief Loads the RGBD frame from disk. 138 | * 139 | * Loands the RGB and depth images from png, and the header and intrinsic matrix 140 | * from .yml files. 141 | * 142 | * @param frame Reference to the frame being loaded 143 | * @param path The path to the folder where everything was saved. 144 | * 145 | * @retval true Successfully loaded the data 146 | * @retval false Loading failed - for example, directory not found 147 | */ 148 | static bool load(RGBDFrame& frame, const std::string& path); 149 | 150 | protected: 151 | 152 | /** @brief Constant for calculating std_dev(z) 153 | * 154 | * Khoshelham, K.; Elberink, S.O. Accuracy and Resolution of Kinect 155 | * Depth Data for Indoor Mapping Applications. Sensors 2012, 12, 1437-1454. 156 | */ 157 | static const double Z_STDEV_CONSTANT = 0.001425; 158 | 159 | /** @brief Calculates the var(z) from z 160 | * @param z the z depth, in meters 161 | * @return the variance in z, in meters^2 162 | */ 163 | double getVarZ(double z) const; 164 | 165 | /** @brief Calculates the std_dev(z) from z 166 | * @param z the z depth, in meters 167 | * @return the standard deviation in z, in meters 168 | */ 169 | double getStdDevZ(double z) const; 170 | 171 | /** @brief Calculates the z distribution (mean and variance) for a given pixel 172 | * 173 | * Calculation is based on the standard quadratic model. See: 174 | * 175 | * Khoshelham, K.; Elberink, S.O. Accuracy and Resolution of Kinect 176 | * Depth Data for Indoor Mapping Applications. Sensors 2012, 12, 1437-1454. 177 | * 178 | * @param u the pixel u-coordinate 179 | * @param v the pixel v-coordinate 180 | * @param z_mean the mean of the distribution (will be the same az the z of the pixel), in meters 181 | * @param z_var var(z), will a quadratic function of the mean, in meters^2 182 | */ 183 | void getGaussianDistribution(int u, int v, double& z_mean, double& z_var) const; 184 | 185 | /** @brief Calculates the GMM z distribution (mean and variance) for a given pixel 186 | * 187 | * Calculation is based on a Gaussian Mixture Model on top of 188 | * the standard quadratic model. The neigboring pixels contribute different 189 | * wights to the mixture. See: 190 | * 191 | * Dryanovski et al ICRA2013 papaer 192 | * 193 | * @todo reference for the paper 194 | * 195 | * @param u the pixel u-coordinate 196 | * @param v the pixel v-coordinate 197 | * @param z_mean the mean of the distribution (will be the same az the z of the pixel), in meters 198 | * @param z_var var(z), will a quadratic function of the mean, in meters^2 199 | */ 200 | void getGaussianMixtureDistribution(int u, int v, double& z_mean, double& z_var) const; 201 | }; 202 | 203 | } // namespace rgbdtools 204 | 205 | #endif // RGBDTOOLS_RGBD_FRAME_H 206 | -------------------------------------------------------------------------------- /include/rgbdtools/rgbd_keyframe.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_keyframe.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_RGBD_KEYFRAME_H 25 | #define RGBDTOOLS_RGBD_KEYFRAME_H 26 | 27 | #include 28 | 29 | #include "rgbdtools/rgbd_frame.h" 30 | 31 | namespace rgbdtools { 32 | 33 | /** @brief Extension of an RGBDFrame, which has a pose, and a 3D point cloud 34 | * 35 | * The class is used for keyframe-based graph alignment, as well as dense 36 | * mapping. 37 | */ 38 | 39 | class RGBDKeyframe: public RGBDFrame 40 | { 41 | public: 42 | 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | 45 | /** @brief Default (empty) constructor. 46 | */ 47 | RGBDKeyframe(); 48 | 49 | /** @brief Copy constructor from a RGBDFrame 50 | * @param frame reference to the frame which is being used to create a keyframe 51 | */ 52 | RGBDKeyframe(const RGBDFrame& frame); 53 | 54 | Pose pose; ///< pose of the camera, in some fixed frame 55 | 56 | bool manually_added; ///< Whether the frame was added manually by the user 57 | 58 | /** @brief path length, in meters, of the camera trajectory at the moment 59 | * of adding the keyframe 60 | */ 61 | double path_length_linear; 62 | 63 | /** @brief path length, in radians, of the camera trajectory at the 64 | * moment of adding the keyframe 65 | */ 66 | double path_length_angular; 67 | 68 | /** @brief Saves the RGBD keyframe to disk. 69 | * 70 | * Saves the RGBDFrame, as well as the additional keyframe info as .yml 71 | * 72 | * @param keyframe Reference to the keyframe being saved 73 | * @param path The path to the folder where everything will be stored 74 | * 75 | * @retval true Successfully saved the data 76 | * @retval false Saving failed - for example, cannot create directory 77 | */ 78 | static bool save(const RGBDKeyframe& keyframe, 79 | const std::string& path); 80 | 81 | /** @brief Loads the RGBD keyframe to disk. 82 | * 83 | * Loads the RGBDFrame, as well as the additional keyframe info as .yml 84 | * 85 | * @param keyframe Reference to the keyframe being saved 86 | * @param path The path to the folder where everything will be stored 87 | * 88 | * @retval true Successfully loaded the data 89 | * @retval false Loading failed - for example, directory not found 90 | */ 91 | static bool load(RGBDKeyframe& keyframe, 92 | const std::string& path); 93 | }; 94 | 95 | typedef Eigen::aligned_allocator KeyframeAllocator; 96 | typedef std::vector KeyframeVector; 97 | 98 | /** @brief Saves a vector of RGBD keyframes to disk. 99 | * 100 | * @param keyframes Reference to the keyframe being saved 101 | * @param path The path to the folder where everything will be stored 102 | * 103 | * @retval true Successfully saved the data 104 | * @retval false Saving failed - for example, cannot create directory 105 | */ 106 | bool saveKeyframes(const KeyframeVector& keyframes, 107 | const std::string& path); 108 | 109 | /** @brief Loads a vector of RGBD keyframes to disk. 110 | * 111 | * @param keyframes Reference to the keyframe being saved 112 | * @param path The path to the folder where everything will be stored 113 | * 114 | * @retval true Successfully saved the data 115 | * @retval false Saving failed - for example, cannot create directory 116 | */ 117 | bool loadKeyframes(KeyframeVector& keyframes, 118 | const std::string& path); 119 | 120 | } // namespace rgbdtools 121 | 122 | #endif // RGBDTOOLS_RGBD_KEYFRAME_H 123 | -------------------------------------------------------------------------------- /include/rgbdtools/rgbd_util.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_util.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_RGBD_UTIL_H 25 | #define RGBDTOOLS_RGBD_UTIL_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "rgbdtools/types.h" 33 | 34 | namespace rgbdtools { 35 | 36 | /** @brief Polynomial fit modes for depth unwarping 37 | * 38 | * The modes include: 39 | * - DEPTH_FIT_LINEAR (c0 + c1*d) 40 | * - DEPTH_FIT_LINEAR_ZERO (c1*d) 41 | * - DEPTH_FIT_QUADRATIC (c0 + c1*d + c2*d^2) 42 | * - DEPTH_FIT_QUADRATIC_ZERO (c1*d + c2*d^2) 43 | * 44 | * The recommended mode is DEPTH_FIT_QUADRATIC 45 | */ 46 | enum DepthFitMode { 47 | DEPTH_FIT_LINEAR, 48 | DEPTH_FIT_LINEAR_ZERO, 49 | DEPTH_FIT_QUADRATIC, 50 | DEPTH_FIT_QUADRATIC_ZERO 51 | }; 52 | 53 | /** @brief Filters out a vector of means given a mask of valid 54 | * entries 55 | * 56 | * @param means input vector of 3x1 matrices 57 | * @param valid vector mask of valid flags 58 | * @param means_f output vector of 3x1 matrices 59 | */ 60 | void removeInvalidMeans( 61 | const Vector3fVector& means, 62 | const BoolVector& valid, 63 | Vector3fVector& means_f); 64 | 65 | /** @brief Filters out a vector of means and a vector of 66 | * covariances given a mask of valid entries 67 | * 68 | * @param means input vector of 3x1 matrices 69 | * @param covariances input vector of 3x3 matrices 70 | * @param valid vector mask of valid flags 71 | * @param means_f output vector of 3x1 matrices 72 | * @param covariances_f output vector of 3x1 matrices 73 | */ 74 | void removeInvalidDistributions( 75 | const Vector3fVector& means, 76 | const Matrix3fVector& covariances, 77 | const BoolVector& valid, 78 | Vector3fVector& means_f, 79 | Matrix3fVector& covariances_f); 80 | 81 | /** @brief Transforms a vector of means 82 | * 83 | * @param means vector of 3x1 matrices of positions (3D means) 84 | * @param transform the tranformation to be applied to all the means 85 | */ 86 | void transformMeans( 87 | Vector3fVector& means, 88 | const AffineTransform& transform); 89 | 90 | /** @brief Transforms a vector of means and covariances 91 | * 92 | * @param means vector of 3x1 matrices of positions (3D means) 93 | * @param covariances vector of 3x3 covariance matrices 94 | * @param transform the transformation to be applied to all the means and covariances 95 | */ 96 | void transformDistributions( 97 | Vector3fVector& means, 98 | Matrix3fVector& covariances, 99 | const AffineTransform& transform); 100 | 101 | /** @brief Creates a pcl point cloud form a vector 102 | * of eigen matrix means 103 | * 104 | * @param means vector of 3x1 matrices of positions (3D means) 105 | * @param cloud reference to the output cloud 106 | */ 107 | void pointCloudFromMeans( 108 | const Vector3fVector& means, 109 | PointCloudFeature& cloud); 110 | 111 | /** @brief reprojects a depth image to another depth image, 112 | * registered in the rgb camera's frame. 113 | * 114 | * Both images need to be rectified first. ir2rgb is a matrix 115 | * such that for any point P_IR in the depth camera frame 116 | * P_RGB = ir2rgb * P_IR 117 | * 118 | * @param intr_rect_ir intrinsic matrix of the rectified depth image 119 | * @param intr_rect_rgb intrinsic matrix of the rectified RGB image 120 | * @param ir2rgb extrinsic matrix between the IR(depth) and RGB cameras 121 | * @param depth_img_rect the input image: rectified depth image 122 | * @param depth_img_rect_reg the output image: rectified and registered into the 123 | * RGB frame 124 | */ 125 | void buildRegisteredDepthImage( 126 | const cv::Mat& intr_rect_ir, 127 | const cv::Mat& intr_rect_rgb, 128 | const cv::Mat& ir2rgb, 129 | const cv::Mat& depth_img_rect, 130 | cv::Mat& depth_img_rect_reg); 131 | 132 | /** @brief Constructs a point cloud, a depth image and intrinsic matrix 133 | * 134 | * @param depth_img_rect rectified depth image (16UC1, in mm) 135 | * @param intr_rect_ir intinsic matrix of the rectified depth image 136 | * @param cloud reference to teh output point cloud 137 | */ 138 | void buildPointCloud( 139 | const cv::Mat& depth_img_rect, 140 | const cv::Mat& intr_rect_ir, 141 | PointCloudT& cloud); 142 | 143 | /** @brief Constructs a point cloud with color 144 | * 145 | * Prior to calling this functions, both images need to be rectified, 146 | * and the depth image has to be registered into the frame of the RGB image. 147 | * 148 | * @param depth_img_rect_reg rectified and registered depth image (16UC1, in mm) 149 | * @param rgb_img_rect rectified rgb image (8UC3) 150 | * @param intr_rect_rgb intrinsic matrix 151 | * @param cloud reference to the output point cloud 152 | */ 153 | void buildPointCloud( 154 | const cv::Mat& depth_img_rect_reg, 155 | const cv::Mat& rgb_img_rect, 156 | const cv::Mat& intr_rect_rgb, 157 | PointCloudT& cloud); 158 | 159 | /** @brief converts a 32FC1 depth image (in meters) to a 160 | * 16UC1 depth image (in mm). 161 | * 162 | * @param depth_image_in the input 32FC1 image 163 | * @param depth_image_out the output 16UC1 image 164 | */ 165 | void depthImageFloatTo16bit( 166 | const cv::Mat& depth_image_in, 167 | cv::Mat& depth_image_out); 168 | 169 | void eigenAffineToOpenCVRt( 170 | const AffineTransform& transform, 171 | cv::Mat& R, 172 | cv::Mat& t); 173 | 174 | void openCVRtToEigenAffine( 175 | const cv::Mat& R, 176 | const cv::Mat& t, 177 | AffineTransform& eigen_affine); 178 | 179 | void eigenAffineToXYZRPY( 180 | const AffineTransform& transform, 181 | float& x, float& y, float& z, 182 | float& roll, float& pitch, float& yaw); 183 | 184 | void XYZRPYToEigenAffine( 185 | float x, float y, float z, 186 | float roll, float pitch, float yaw, 187 | AffineTransform& t); 188 | 189 | void getTfDifference( 190 | const AffineTransform& transform, 191 | double& dist, double& angle); 192 | 193 | /** @brief Given a depth image, uwarps it according to a polynomial model 194 | * 195 | * The size of the c matrices should be equal to the image size. 196 | * 197 | * @param depth_img_in depth image to be unwarped (16UC1, in mm) 198 | * @param coeff0 matrix of c0 coefficients 199 | * @param coeff1 matrix of c1 coefficients 200 | * @param coeff2 matrix of c2 coefficients 201 | * @param fit_mode the polynomial fitting mode, see \ref DepthFitMode. 202 | * d = c0 + c1*d + c2*d^2 203 | */ 204 | void unwarpDepthImage( 205 | cv::Mat& depth_img_in, 206 | const cv::Mat& coeff0, 207 | const cv::Mat& coeff1, 208 | const cv::Mat& coeff2, 209 | int fit_mode=DEPTH_FIT_QUADRATIC); 210 | 211 | void setRPY( 212 | float roll, float pitch, float yaw, 213 | Matrix3f& m); 214 | 215 | } // namespace rgbdtools 216 | 217 | #endif // RGBDTOOLS_RGBD_UTIL_H 218 | -------------------------------------------------------------------------------- /include/rgbdtools/rgbdtools.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbdtools.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_RGBDTOOLS_H 25 | #define RGBDTOOLS_RGBDTOOLS_H 26 | 27 | #include "rgbdtools/types.h" 28 | #include "rgbdtools/rgbd_util.h" 29 | #include "rgbdtools/map_util.h" 30 | 31 | #include "rgbdtools/header.h" 32 | #include "rgbdtools/rgbd_frame.h" 33 | #include "rgbdtools/rgbd_keyframe.h" 34 | 35 | #include "rgbdtools/features/feature_detector.h" 36 | #include "rgbdtools/features/gft_detector.h" 37 | #include "rgbdtools/features/orb_detector.h" 38 | #include "rgbdtools/features/star_detector.h" 39 | 40 | #include "rgbdtools/registration/motion_estimation.h" 41 | #include "rgbdtools/registration/motion_estimation_icp_prob_model.h" 42 | #include "rgbdtools/registration/motion_estimation_pairwise_ransac.h" 43 | 44 | #include "rgbdtools/graph/keyframe_association.h" 45 | #include "rgbdtools/graph/keyframe_graph_detector.h" 46 | #include "rgbdtools/graph/keyframe_graph_solver.h" 47 | #include "rgbdtools/graph/keyframe_graph_solver_g2o.h" 48 | 49 | #endif // RGBDTOOLS_RGBDTOOLS_H 50 | -------------------------------------------------------------------------------- /include/rgbdtools/types.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file types.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef RGBDTOOLS_TYPES_H 25 | #define RGBDTOOLS_TYPES_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace rgbdtools { 35 | 36 | // Eigen matrix types 37 | 38 | typedef Eigen::Matrix3f Matrix3f; 39 | typedef Eigen::Vector3f Vector3f; 40 | typedef Eigen::Affine3f Pose; 41 | typedef Eigen::Affine3f AffineTransform; 42 | typedef Eigen::Matrix InformationMatrix; 43 | 44 | // Vector types 45 | 46 | typedef std::vector IntVector; 47 | typedef std::vector FloatVector; 48 | typedef std::vector BoolVector; 49 | typedef std::vector Point2fVector; 50 | typedef std::vector Point3fVector; 51 | typedef std::vector Matrix3fVector; 52 | typedef std::vector Vector3fVector; 53 | typedef std::vector KeypointVector; 54 | typedef std::vector DMatchVector; 55 | 56 | typedef Eigen::aligned_allocator AffineTransformAllocator; 57 | typedef std::vector AffineTransformVector; 58 | 59 | // PCL types 60 | 61 | typedef pcl::PointXYZRGB PointT; 62 | typedef pcl::PointCloud PointCloudT; 63 | 64 | typedef pcl::PointXYZ PointFeature; 65 | typedef pcl::PointCloud PointCloudFeature; 66 | 67 | typedef pcl::KdTreeFLANN KdTree; 68 | typedef pcl::registration::TransformationEstimationSVD TransformationEstimationSVD; 69 | 70 | } // namespace rgbdtools 71 | 72 | #endif // RGBDTOOLS_TYPES_H 73 | -------------------------------------------------------------------------------- /src/features/feature_detector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file feature_detector.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/features/feature_detector.h" 25 | 26 | namespace rgbdtools { 27 | 28 | FeatureDetector::FeatureDetector(): 29 | compute_descriptors_(false), 30 | max_range_(7.0), 31 | max_stdev_(0.15), 32 | smooth_(0) 33 | { 34 | 35 | } 36 | 37 | FeatureDetector::~FeatureDetector() 38 | { 39 | 40 | } 41 | 42 | void FeatureDetector::findFeatures(RGBDFrame& frame) 43 | { 44 | boost::mutex::scoped_lock(mutex_); 45 | 46 | const cv::Mat& input_img = frame.rgb_img; 47 | 48 | cv::Mat gray_img(input_img.rows, input_img.cols, CV_8UC1); 49 | const cv::Mat* ptarget_img = NULL; 50 | 51 | if (input_img.type() != CV_8UC1) 52 | { 53 | // convert from RGB to grayscale only if necessary 54 | cvtColor(input_img, gray_img, CV_BGR2GRAY); 55 | ptarget_img = &gray_img; 56 | } 57 | else 58 | ptarget_img = &input_img; 59 | 60 | // blur if needed 61 | if(smooth_ > 0) 62 | { 63 | int blur_size = smooth_*2 + 1; 64 | cv::GaussianBlur(*ptarget_img, *ptarget_img, cv::Size(blur_size, blur_size), 0); 65 | } 66 | 67 | // clear previous data: 68 | /// todo: maybe check type of keypoints (ORB/GFT etc), and if matches, don't clear it 69 | frame.keypoints.clear(); 70 | frame.descriptors = cv::Mat(); 71 | 72 | // find the 2D coordinates of keypoints 73 | findFeatures(frame, *ptarget_img); 74 | 75 | // calculates the 3D position and covariance of features 76 | frame.computeDistributions(max_range_, max_stdev_); 77 | } 78 | 79 | void FeatureDetector::setComputeDescriptors(bool compute_descriptors) 80 | { 81 | compute_descriptors_ = compute_descriptors; 82 | } 83 | 84 | void FeatureDetector::setSmooth(int smooth) 85 | { 86 | smooth_ = smooth; 87 | } 88 | 89 | void FeatureDetector::setMaxRange(double max_range) 90 | { 91 | max_range_ = max_range; 92 | } 93 | 94 | void FeatureDetector::setMaxStDev(double max_stdev) 95 | { 96 | max_stdev_ = max_stdev; 97 | } 98 | 99 | int FeatureDetector::getSmooth() const 100 | { 101 | return smooth_; 102 | } 103 | 104 | double FeatureDetector::getMaxRange() const 105 | { 106 | return max_range_; 107 | } 108 | 109 | double FeatureDetector::getMaxStDev() const 110 | { 111 | return max_stdev_; 112 | } 113 | 114 | } // namespace rgbdtools 115 | -------------------------------------------------------------------------------- /src/features/gft_detector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gft_detector.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/features/gft_detector.h" 25 | 26 | namespace rgbdtools { 27 | 28 | GftDetector::GftDetector(): 29 | FeatureDetector() 30 | { 31 | gft_detector_.reset( 32 | new cv::GoodFeaturesToTrackDetector(n_features_, 0.01, min_distance_)); 33 | } 34 | 35 | GftDetector::~GftDetector() 36 | { 37 | 38 | } 39 | 40 | void GftDetector::findFeatures(RGBDFrame& frame, const cv::Mat& input_img) 41 | { 42 | cv::Mat mask(frame.depth_img.size(), CV_8UC1); 43 | frame.depth_img.convertTo(mask, CV_8U); 44 | 45 | gft_detector_->detect(input_img, frame.keypoints, mask); 46 | 47 | if(compute_descriptors_) 48 | orb_descriptor_.compute( 49 | input_img, frame.keypoints, frame.descriptors); 50 | } 51 | 52 | void GftDetector::setNFeatures(int n_features) 53 | { 54 | n_features_ = n_features; 55 | 56 | gft_detector_.reset( 57 | new cv::GoodFeaturesToTrackDetector(n_features_, 0.01, min_distance_)); 58 | } 59 | 60 | void GftDetector::setMinDistance(double min_distance) 61 | { 62 | min_distance_ = min_distance; 63 | 64 | gft_detector_.reset( 65 | new cv::GoodFeaturesToTrackDetector(n_features_, 0.01, min_distance_)); 66 | } 67 | 68 | } // namespace rgbdtools 69 | -------------------------------------------------------------------------------- /src/features/orb_detector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file orb_detector.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/features/orb_detector.h" 25 | 26 | namespace rgbdtools { 27 | 28 | OrbDetector::OrbDetector(): FeatureDetector(), 29 | n_features_(400), 30 | threshold_(31.0) 31 | { 32 | mutex_.lock(); 33 | 34 | orb_detector_.reset( 35 | new cv::OrbFeatureDetector(n_features_, 1.2f, 8, threshold_, 0, 2, 0, 31)); 36 | 37 | mutex_.unlock(); 38 | } 39 | 40 | OrbDetector::~OrbDetector() 41 | { 42 | 43 | } 44 | 45 | void OrbDetector::findFeatures(RGBDFrame& frame, const cv::Mat& input_img) 46 | { 47 | mutex_.lock(); 48 | 49 | cv::Mat mask(frame.depth_img.size(), CV_8UC1); 50 | frame.depth_img.convertTo(mask, CV_8U); 51 | 52 | orb_detector_->detect(input_img, frame.keypoints, mask); 53 | 54 | if(compute_descriptors_) 55 | orb_descriptor_.compute( 56 | input_img, frame.keypoints, frame.descriptors); 57 | 58 | mutex_.unlock(); 59 | } 60 | 61 | void OrbDetector::setThreshold(int threshold) 62 | { 63 | mutex_.lock(); 64 | 65 | threshold_ = threshold; 66 | 67 | orb_detector_.reset( 68 | new cv::OrbFeatureDetector(n_features_, 1.2f, 8, threshold_, 0, 2, 0, 31)); 69 | 70 | mutex_.unlock(); 71 | } 72 | 73 | void OrbDetector::setNFeatures(int n_features) 74 | { 75 | mutex_.lock(); 76 | n_features_ = n_features; 77 | 78 | orb_detector_.reset( 79 | new cv::OrbFeatureDetector(n_features_, 1.2f, 8, threshold_, 0, 2, 0, 31)); 80 | 81 | mutex_.unlock(); 82 | } 83 | 84 | } // namespace rgbdtools 85 | -------------------------------------------------------------------------------- /src/features/star_detector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file star_detector.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/features/star_detector.h" 25 | 26 | namespace rgbdtools { 27 | 28 | StarDetector::StarDetector(): FeatureDetector() 29 | { 30 | star_detector_.reset( 31 | new cv::StarFeatureDetector(16, threshold_, 10, 8, min_distance_)); 32 | } 33 | 34 | StarDetector::~StarDetector() 35 | { 36 | 37 | } 38 | 39 | void StarDetector::findFeatures(RGBDFrame& frame, const cv::Mat& input_img) 40 | { 41 | boost::mutex::scoped_lock(mutex_); 42 | 43 | cv::Mat mask(frame.depth_img.size(), CV_8UC1); 44 | frame.depth_img.convertTo(mask, CV_8U); 45 | 46 | star_detector_->detect(input_img, frame.keypoints, mask); 47 | } 48 | 49 | void StarDetector::setMinDistance(double min_distance) 50 | { 51 | min_distance_ = min_distance; 52 | 53 | star_detector_.reset( 54 | new cv::StarFeatureDetector(16, threshold_, 10, 8, min_distance_)); 55 | } 56 | 57 | void StarDetector::setThreshold(double threshold) 58 | { 59 | threshold_ = threshold; 60 | 61 | star_detector_.reset( 62 | new cv::StarFeatureDetector(16, threshold_, 10, 8, min_distance_)); 63 | } 64 | 65 | } // namespace rgbdtools 66 | -------------------------------------------------------------------------------- /src/graph/keyframe_graph_detector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_graph_detector.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/graph/keyframe_graph_detector.h" 25 | 26 | namespace rgbdtools { 27 | 28 | KeyframeGraphDetector::KeyframeGraphDetector() 29 | { 30 | srand(time(NULL)); 31 | 32 | // USRF params 33 | n_keypoints_ = 600; 34 | init_surf_threshold_ = 400; 35 | 36 | // Pairwise matching params 37 | pairwise_matching_method_ = PAIRWISE_MATCHING_RANSAC; 38 | 39 | // tree algorithm params 40 | candidate_method_ = CANDIDATE_GENERATION_SURF_TREE; 41 | n_candidates_ = 10; 42 | k_nearest_neighbors_ = 4; 43 | 44 | // matcher params 45 | pairwise_matcher_index_ = PAIRWISE_MATCHER_KDTREE; 46 | matcher_use_desc_ratio_test_ = true; 47 | matcher_max_desc_ratio_ = 0.75; // when ratio_test = true 48 | matcher_max_desc_dist_ = 0.5; // when ratio_test = false 49 | 50 | // common SAC params 51 | sac_max_eucl_dist_sq_ = 0.03 * 0.03; 52 | sac_min_inliers_ = 30; // this or more are needed 53 | sac_reestimate_tf_ = false; 54 | 55 | // RANSAC params 56 | ransac_confidence_ = 0.99; 57 | ransac_max_iterations_ = 1000; 58 | ransac_sufficient_inlier_ratio_ = 0.75; 59 | 60 | // output params 61 | verbose_ = false; // console output 62 | sac_save_results_ = false; 63 | 64 | // derived parameters 65 | log_one_minus_ransac_confidence_ = log(1.0 - ransac_confidence_); 66 | 67 | setOutputPath(std::getenv("HOME")); 68 | } 69 | 70 | KeyframeGraphDetector::~KeyframeGraphDetector() 71 | { 72 | 73 | } 74 | 75 | void KeyframeGraphDetector::setSACReestimateTf(bool sac_reestimate_tf) 76 | { 77 | sac_reestimate_tf_ = sac_reestimate_tf; 78 | } 79 | 80 | void KeyframeGraphDetector::setMatcherUseDescRatioTest(bool matcher_use_desc_ratio_test) 81 | { 82 | matcher_use_desc_ratio_test_ = matcher_use_desc_ratio_test; 83 | } 84 | 85 | void KeyframeGraphDetector::setMatcherMaxDescRatio(double matcher_max_desc_ratio) 86 | { 87 | matcher_max_desc_ratio_ = matcher_max_desc_ratio; 88 | } 89 | 90 | void KeyframeGraphDetector::setMatcherMaxDescDist(double matcher_max_desc_dist) 91 | { 92 | matcher_max_desc_dist_ = matcher_max_desc_dist; 93 | } 94 | 95 | void KeyframeGraphDetector::setNCandidates(int n_candidates) 96 | { 97 | n_candidates_ = n_candidates; 98 | } 99 | 100 | void KeyframeGraphDetector::setKNearestNeighbors(int k_nearest_neighbors) 101 | { 102 | k_nearest_neighbors_ = k_nearest_neighbors; 103 | } 104 | 105 | void KeyframeGraphDetector::setNKeypoints(int n_keypoints) 106 | { 107 | n_keypoints_ = n_keypoints; 108 | } 109 | 110 | void KeyframeGraphDetector::setOutputPath(const std::string& output_path) 111 | { 112 | output_path_ = output_path; 113 | boost::filesystem::create_directories(output_path_); 114 | } 115 | 116 | void KeyframeGraphDetector::setSACSaveResults(bool sac_save_results) 117 | { 118 | sac_save_results_ = sac_save_results; 119 | } 120 | 121 | void KeyframeGraphDetector::setVerbose(bool verbose) 122 | { 123 | verbose_ = verbose; 124 | } 125 | 126 | void KeyframeGraphDetector::setCandidateGenerationMethod( 127 | CandidateGenerationMethod candidate_method) 128 | { 129 | assert(candidate_method == CANDIDATE_GENERATION_BRUTE_FORCE || 130 | candidate_method == CANDIDATE_GENERATION_SURF_TREE); 131 | 132 | candidate_method_ = candidate_method; 133 | } 134 | 135 | void KeyframeGraphDetector::setPairwiseMatchingMethod( 136 | PairwiseMatchingMethod pairwise_matching_method) 137 | { 138 | assert(pairwise_matching_method == PAIRWISE_MATCHING_BFSAC || 139 | pairwise_matching_method == PAIRWISE_MATCHING_RANSAC); 140 | 141 | pairwise_matching_method_ = pairwise_matching_method; 142 | } 143 | 144 | void KeyframeGraphDetector::setPairwiseMatcherIndex( 145 | PairwiseMatcherIndex pairwise_matcher_index) 146 | { 147 | assert(pairwise_matcher_index == PAIRWISE_MATCHER_LINEAR || 148 | pairwise_matcher_index == PAIRWISE_MATCHER_KDTREE); 149 | 150 | pairwise_matcher_index_ = pairwise_matcher_index; 151 | } 152 | 153 | void KeyframeGraphDetector::generateKeyframeAssociations( 154 | KeyframeVector& keyframes, 155 | KeyframeAssociationVector& associations) 156 | { 157 | prepareFeaturesForRANSAC(keyframes); 158 | 159 | buildAssociationMatrix(keyframes, associations); 160 | } 161 | 162 | void KeyframeGraphDetector::prepareMatchers( 163 | const KeyframeVector& keyframes) 164 | { 165 | if(verbose_) printf("training individual keyframe matchers ...\n"); 166 | 167 | matchers_.clear(); 168 | matchers_.resize(keyframes.size()); 169 | 170 | for (unsigned int kf_idx = 0; kf_idx < keyframes.size(); kf_idx++) 171 | { 172 | const RGBDKeyframe& keyframe = keyframes[kf_idx]; 173 | cv::FlannBasedMatcher& matcher = matchers_[kf_idx]; 174 | 175 | // build matcher 176 | cv::Ptr indexParams; 177 | 178 | if (pairwise_matcher_index_ == PAIRWISE_MATCHER_LINEAR) 179 | indexParams = new cv::flann::LinearIndexParams(); 180 | else if (pairwise_matcher_index_ == PAIRWISE_MATCHER_KDTREE) 181 | indexParams = new cv::flann::KDTreeIndexParams(); 182 | 183 | cv::Ptr searchParams = new cv::flann::SearchParams(32); 184 | 185 | matcher = cv::FlannBasedMatcher(indexParams, searchParams); 186 | 187 | // train 188 | std::vector descriptors_vector; 189 | descriptors_vector.push_back(keyframe.descriptors); 190 | matcher.add(descriptors_vector); 191 | matcher.train(); 192 | } 193 | } 194 | 195 | void KeyframeGraphDetector::prepareFeaturesForRANSAC( 196 | KeyframeVector& keyframes) 197 | { 198 | bool upright = true; 199 | double orb_threshold = 31; 200 | 201 | if(verbose_) printf("preparing SURF features for matching...\n"); 202 | 203 | cv::OrbDescriptorExtractor extractor; 204 | 205 | for (unsigned int kf_idx = 0; kf_idx < keyframes.size(); kf_idx++) 206 | { 207 | RGBDKeyframe& keyframe = keyframes[kf_idx]; 208 | 209 | cv::OrbFeatureDetector detector(400, 1.2f, 8, orb_threshold, 0, 2, 0, 31); 210 | keyframe.keypoints.clear(); 211 | detector.detect(keyframe.rgb_img, keyframe.keypoints); 212 | 213 | if(verbose_) 214 | { 215 | printf("[KF %d of %d] %d ORB keypoints detected\n", 216 | (int)kf_idx, (int)keyframes.size(), (int)keyframe.keypoints.size()); 217 | } 218 | 219 | if (sac_save_results_) 220 | { 221 | cv::Mat kp_img; 222 | cv::drawKeypoints(keyframe.rgb_img, keyframe.keypoints, kp_img); 223 | std::stringstream ss1; 224 | ss1 << "kp_" << kf_idx; 225 | cv::imwrite(output_path_ + "/" + ss1.str() + ".png", kp_img); 226 | } 227 | 228 | extractor.compute(keyframe.rgb_img, keyframe.keypoints, keyframe.descriptors); 229 | keyframe.computeDistributions(); 230 | } 231 | } 232 | 233 | 234 | void KeyframeGraphDetector::buildAssociationMatrix( 235 | const KeyframeVector& keyframes, 236 | KeyframeAssociationVector& associations) 237 | { 238 | prepareMatchers(keyframes); 239 | 240 | // 1. Create the candidate matrix 241 | buildCandidateMatrix(keyframes); 242 | 243 | // 2. Perfrom pairwise matching for all candidates 244 | buildCorrespondenceMatrix(keyframes, associations); 245 | } 246 | 247 | void KeyframeGraphDetector::buildCandidateMatrix( 248 | const KeyframeVector& keyframes) 249 | { 250 | if (candidate_method_ == CANDIDATE_GENERATION_BRUTE_FORCE) // brute-force 251 | { 252 | // create a candidate matrix which considers all posiible combinations 253 | int size = keyframes.size(); 254 | candidate_matrix_ = cv::Mat::ones(size, size, CV_8UC1); 255 | } 256 | else if (candidate_method_ == CANDIDATE_GENERATION_SURF_TREE) // tree-based 257 | { 258 | // build a math knn matrix using a kdtree 259 | buildMatchMatrixSurfTree(keyframes); 260 | 261 | // keep only the top n candidates 262 | buildCandidateMatrixSurfTree(); 263 | } 264 | } 265 | 266 | /** @brief Builds a matrix of nearest neighbor matches between keyframes 267 | * using a kdtree 268 | * 269 | * match_matrix[query, train] = X correspondences 270 | */ 271 | void KeyframeGraphDetector::buildMatchMatrixSurfTree( 272 | const KeyframeVector& keyframes) 273 | { 274 | unsigned int kf_size = keyframes.size(); 275 | 276 | // train matcher from all the features 277 | cv::FlannBasedMatcher matcher; 278 | trainSURFMatcher(keyframes, matcher); 279 | 280 | // lookup per frame 281 | if(verbose_) printf("Keyframe lookups...\n"); 282 | 283 | match_matrix_ = cv::Mat::zeros(kf_size, kf_size, CV_16UC1); 284 | for (unsigned int kf_idx = 0; kf_idx < kf_size; ++kf_idx) 285 | { 286 | if(verbose_) 287 | printf("[KF %d of %d]: Computing SURF neighbors\n", (int)kf_idx, (int)kf_size); 288 | const RGBDFrame& keyframe = keyframes[kf_idx]; 289 | 290 | // find k nearest matches for each feature in the keyframe 291 | std::vector > matches_vector; 292 | matcher.knnMatch(keyframe.descriptors, matches_vector, k_nearest_neighbors_); 293 | 294 | // create empty bins vector of Pairs 295 | std::vector > bins; 296 | bins.resize(kf_size); 297 | for (unsigned int b = 0; b < bins.size(); ++b) 298 | bins[b] = std::pair(0, b); 299 | 300 | // fill out bins with match indices 301 | for (unsigned int j = 0; j < matches_vector.size(); ++j) 302 | { 303 | std::vector& matches = matches_vector[j]; 304 | for (unsigned int k = 0; k < matches.size(); ++k) 305 | { 306 | bins[matches[k].imgIdx].first++; 307 | } 308 | } 309 | 310 | for (unsigned int b = 0; b < kf_size; ++b) 311 | { 312 | unsigned int index_a = kf_idx; 313 | unsigned int index_b = bins[b].second; 314 | int corresp_count = bins[b].first; 315 | 316 | if (index_a != index_b) 317 | match_matrix_.at(index_a, index_b) = corresp_count; 318 | } 319 | } 320 | } 321 | 322 | /** @brief Takes in a matrix of matches from a SURF tree (match_matrix_) 323 | * and marks the top n_candidates in each row into (candidate_matrix) 324 | */ 325 | void KeyframeGraphDetector::buildCandidateMatrixSurfTree() 326 | { 327 | // check for square matrix 328 | assert(match_matrix_.rows == match_matrix_.cols); 329 | int size = match_matrix_.rows; 330 | assert(size > 0); 331 | 332 | // check for validity of n_candidates argument 333 | if(n_candidates_ < size) n_candidates_ = size; 334 | 335 | // initialize candidate matrix as all 0 336 | candidate_matrix_ = cv::Mat::eye(match_matrix_.size(), CV_8UC1); 337 | 338 | for (int v = 0; v < match_matrix_.rows; ++v) 339 | { 340 | // create a vector from the current row 341 | std::vector > values(match_matrix_.cols); 342 | for (int u = 0; u < match_matrix_.cols; ++u) 343 | { 344 | int value = match_matrix_.at(v,u); 345 | values[u] = std::pair(value, u); 346 | } 347 | 348 | // sort the vector based on values, highest first 349 | std::sort(values.begin(), values.end(), std::greater >()); 350 | 351 | // mark 1 for the top n_candidates, if > 0 352 | for (int u = 0; u < n_candidates_; ++u) 353 | { 354 | if (values[u].first == 0) continue; 355 | unsigned int uc = values[u].second; 356 | candidate_matrix_.at(v,uc) = 1; 357 | } 358 | } 359 | } 360 | 361 | void KeyframeGraphDetector::buildCorrespondenceMatrix( 362 | const KeyframeVector& keyframes, 363 | KeyframeAssociationVector& associations) 364 | { 365 | // check for square matrix 366 | assert(candidate_matrix_.rows == candidate_matrix_.cols); 367 | int size = candidate_matrix_.rows; 368 | 369 | // initialize correspondence matrix 370 | correspondence_matrix_ = cv::Mat::zeros(size, size, CV_16UC1); 371 | association_matrix_ = cv::Mat::zeros(size, size, CV_8UC1); 372 | 373 | for (int kf_idx_q = 0; kf_idx_q < size; ++kf_idx_q) 374 | for (int kf_idx_t = 0; kf_idx_t < size; ++kf_idx_t) 375 | { 376 | const RGBDKeyframe& keyframe_q = keyframes[kf_idx_q]; 377 | const RGBDKeyframe& keyframe_t = keyframes[kf_idx_t]; 378 | 379 | if (kf_idx_q == kf_idx_t) 380 | { 381 | // self-association 382 | 383 | //correspondence_matrix_.at(kf_idx_q, kf_idx_q) = keyframe_q.keypoints.size(); 384 | correspondence_matrix_.at(kf_idx_q, kf_idx_q) = keyframe_q.n_valid_keypoints; 385 | 386 | association_matrix_.at(kf_idx_q, kf_idx_q) = 1; 387 | } 388 | else 389 | { 390 | // skip non-candidates 391 | if (candidate_matrix_.at(kf_idx_q, kf_idx_t) != 0) 392 | { 393 | if(verbose_) printf("[RANSAC %d to %d]: ", kf_idx_q, kf_idx_t); 394 | 395 | std::vector inlier_matches; 396 | 397 | // perform ransac matching, b onto a 398 | Eigen::Matrix4f transformation; 399 | 400 | // query, train 401 | int iterations = pairwiseMatching( 402 | kf_idx_q, kf_idx_t, keyframes, inlier_matches, transformation); 403 | 404 | correspondence_matrix_.at(kf_idx_q, kf_idx_t) = inlier_matches.size(); 405 | 406 | if (inlier_matches.size() >= sac_min_inliers_) 407 | { 408 | // mark the association matrix 409 | association_matrix_.at(kf_idx_q, kf_idx_t) = 1; 410 | 411 | // add an association 412 | KeyframeAssociation association; 413 | association.type = KeyframeAssociation::RANSAC; 414 | association.kf_idx_a = kf_idx_t; 415 | association.kf_idx_b = kf_idx_q; 416 | association.matches = inlier_matches; 417 | association.a2b = transformation; 418 | associations.push_back(association); 419 | 420 | // output the results to screen 421 | if(verbose_) 422 | printf("pass [%d][%d]\n", iterations, (int)inlier_matches.size()); 423 | 424 | // save the results to file 425 | if (sac_save_results_) 426 | { 427 | cv::Mat img_matches; 428 | cv::drawMatches(keyframe_q.rgb_img, keyframe_q.keypoints, 429 | keyframe_t.rgb_img, keyframe_t.keypoints, 430 | inlier_matches, img_matches); 431 | 432 | std::stringstream ss1; 433 | ss1 << kf_idx_q << "_to_" << kf_idx_t; 434 | cv::imwrite(output_path_ + "/" + ss1.str() + ".png", img_matches); 435 | } 436 | } 437 | else 438 | { 439 | association_matrix_.at(kf_idx_q, kf_idx_t) = 0; 440 | 441 | if(verbose_) 442 | printf("fail [%d][%d]\n", iterations, (int)inlier_matches.size()); 443 | } 444 | } 445 | } 446 | } 447 | } 448 | 449 | // frame_a = train, frame_b = query 450 | void KeyframeGraphDetector::getCandidateMatches( 451 | const RGBDFrame& frame_q, const RGBDFrame& frame_t, 452 | cv::FlannBasedMatcher& matcher, 453 | DMatchVector& candidate_matches) 454 | { 455 | // **** build candidate matches *********************************** 456 | // assumes detectors and distributions are computed 457 | // establish all matches from b to a 458 | 459 | if (matcher_use_desc_ratio_test_) 460 | { 461 | std::vector all_matches2; 462 | 463 | matcher.knnMatch( 464 | frame_q.descriptors, all_matches2, 2); 465 | 466 | for (unsigned int m_idx = 0; m_idx < all_matches2.size(); ++m_idx) 467 | { 468 | const cv::DMatch& match1 = all_matches2[m_idx][0]; 469 | const cv::DMatch& match2 = all_matches2[m_idx][1]; 470 | 471 | double ratio = match1.distance / match2.distance; 472 | 473 | // remove bad matches - ratio test, valid keypoints 474 | if (ratio < matcher_max_desc_ratio_) 475 | { 476 | int idx_q = match1.queryIdx; 477 | int idx_t = match1.trainIdx; 478 | 479 | if (frame_t.kp_valid[idx_t] && frame_q.kp_valid[idx_q]) 480 | candidate_matches.push_back(match1); 481 | } 482 | } 483 | } 484 | else 485 | { 486 | DMatchVector all_matches; 487 | 488 | matcher.match( 489 | frame_q.descriptors, all_matches); 490 | 491 | for (unsigned int m_idx = 0; m_idx < all_matches.size(); ++m_idx) 492 | { 493 | const cv::DMatch& match = all_matches[m_idx]; 494 | 495 | // remove bad matches - descriptor distance, valid keypoints 496 | if (match.distance < matcher_max_desc_dist_) 497 | { 498 | int idx_q = match.queryIdx; 499 | int idx_t = match.trainIdx; 500 | 501 | if (frame_t.kp_valid[idx_t] && frame_q.kp_valid[idx_q]) 502 | candidate_matches.push_back(match); 503 | } 504 | } 505 | } 506 | } 507 | 508 | // frame_a = train, frame_b = query 509 | int KeyframeGraphDetector::pairwiseMatching( 510 | int kf_idx_q, int kf_idx_t, 511 | const KeyframeVector& keyframes, 512 | DMatchVector& best_inlier_matches, 513 | Eigen::Matrix4f& best_transformation) 514 | { 515 | int iterations; 516 | 517 | if (pairwise_matching_method_ == PAIRWISE_MATCHING_RANSAC) 518 | { 519 | iterations = pairwiseMatchingRANSAC( 520 | kf_idx_q, kf_idx_t, keyframes, best_inlier_matches, best_transformation); 521 | } 522 | else if (pairwise_matching_method_ == PAIRWISE_MATCHING_BFSAC) 523 | { 524 | iterations = pairwiseMatchingBFSAC( 525 | kf_idx_q, kf_idx_t, keyframes, best_inlier_matches, best_transformation); 526 | } 527 | 528 | return iterations; 529 | } 530 | 531 | // frame_a = train, frame_b = query 532 | int KeyframeGraphDetector::pairwiseMatchingBFSAC( 533 | int kf_idx_q, int kf_idx_t, 534 | const KeyframeVector& keyframes, 535 | DMatchVector& best_inlier_matches, 536 | Eigen::Matrix4f& best_transformation) 537 | { 538 | // constants 539 | int min_sample_size = 3; 540 | 541 | const RGBDFrame& frame_t = keyframes[kf_idx_t]; 542 | const RGBDFrame& frame_q = keyframes[kf_idx_q]; 543 | cv::FlannBasedMatcher& matcher = matchers_[kf_idx_t]; 544 | 545 | // find candidate matches 546 | DMatchVector candidate_matches; 547 | getCandidateMatches(frame_q, frame_t, matcher, candidate_matches); 548 | if (candidate_matches.size() < min_sample_size) return 0; 549 | 550 | // **** build 3D features for SVD ******************************** 551 | 552 | PointCloudFeature features_t, features_q; 553 | 554 | features_t.resize(candidate_matches.size()); 555 | features_q.resize(candidate_matches.size()); 556 | 557 | for (int m_idx = 0; m_idx < candidate_matches.size(); ++m_idx) 558 | { 559 | const cv::DMatch& match = candidate_matches[m_idx]; 560 | int idx_q = match.queryIdx; 561 | int idx_t = match.trainIdx; 562 | 563 | PointFeature& p_t = features_t[m_idx]; 564 | p_t.x = frame_t.kp_means[idx_t](0,0); 565 | p_t.y = frame_t.kp_means[idx_t](1,0); 566 | p_t.z = frame_t.kp_means[idx_t](2,0); 567 | 568 | PointFeature& p_q = features_q[m_idx]; 569 | p_q.x = frame_q.kp_means[idx_q](0,0); 570 | p_q.y = frame_q.kp_means[idx_q](1,0); 571 | p_q.z = frame_q.kp_means[idx_q](2,0); 572 | } 573 | 574 | // **** main BFSAC loop **************************************** 575 | 576 | TransformationEstimationSVD svd; 577 | Eigen::Matrix4f transformation; // transformation used inside loop 578 | best_inlier_matches.clear(); 579 | int iterations = 0; 580 | 581 | for (int i = 0; i < candidate_matches.size(); ++i) 582 | for (int j = i+1; j < candidate_matches.size(); ++j) 583 | for (int k = j+1; k < candidate_matches.size(); ++k) 584 | { 585 | // build the Minimum Sample Set of 3 matches (index vector) 586 | IntVector inlier_idx; 587 | inlier_idx.push_back(i); 588 | inlier_idx.push_back(j); 589 | inlier_idx.push_back(k); 590 | 591 | // build the Minimum Sample Set of 3 matches (actual matches) 592 | std::vector inlier_matches; 593 | inlier_matches.push_back(candidate_matches[i]); 594 | inlier_matches.push_back(candidate_matches[j]); 595 | inlier_matches.push_back(candidate_matches[k]); 596 | 597 | // estimate transformation from minimum set of random samples 598 | svd.estimateRigidTransformation( 599 | features_q, inlier_idx, 600 | features_t, inlier_idx, 601 | transformation); 602 | 603 | // evaluate transformation fitness by checking distance to all points 604 | PointCloudFeature features_q_tf; 605 | pcl::transformPointCloud(features_q, features_q_tf, transformation); 606 | 607 | for (int m_idx = 0; m_idx < candidate_matches.size(); ++m_idx) 608 | { 609 | // euclidedan distance test 610 | const PointFeature& p_t = features_t[m_idx]; 611 | const PointFeature& p_q = features_q_tf[m_idx]; 612 | float eucl_dist_sq = distEuclideanSq(p_t, p_q); 613 | 614 | if (eucl_dist_sq < sac_max_eucl_dist_sq_) 615 | { 616 | inlier_idx.push_back(m_idx); 617 | inlier_matches.push_back(candidate_matches[m_idx]); 618 | 619 | // reestimate transformation from all inliers 620 | if (sac_reestimate_tf_) 621 | { 622 | svd.estimateRigidTransformation( 623 | features_q, inlier_idx, 624 | features_t, inlier_idx, 625 | transformation); 626 | pcl::transformPointCloud(features_q, features_q_tf, transformation); 627 | } 628 | } 629 | } 630 | 631 | // check if inliers are better than the best model so far 632 | if (inlier_matches.size() > best_inlier_matches.size()) 633 | { 634 | svd.estimateRigidTransformation( 635 | features_q, inlier_idx, 636 | features_t, inlier_idx, 637 | transformation); 638 | 639 | best_transformation = transformation; 640 | best_inlier_matches = inlier_matches; 641 | } 642 | 643 | iterations++; 644 | } 645 | 646 | return iterations; 647 | } 648 | 649 | // frame_a = train, frame_b = query 650 | int KeyframeGraphDetector::pairwiseMatchingRANSAC( 651 | int kf_idx_q, int kf_idx_t, 652 | const KeyframeVector& keyframes, 653 | DMatchVector& best_inlier_matches, 654 | Eigen::Matrix4f& best_transformation) 655 | { 656 | // constants 657 | int min_sample_size = 3; 658 | 659 | // find candidate matches 660 | const RGBDFrame& frame_t = keyframes[kf_idx_t]; 661 | const RGBDFrame& frame_q = keyframes[kf_idx_q]; 662 | cv::FlannBasedMatcher& matcher = matchers_[kf_idx_t]; 663 | 664 | DMatchVector candidate_matches; 665 | getCandidateMatches(frame_q, frame_t, matcher, candidate_matches); 666 | 667 | // check if enough matches are present 668 | if (candidate_matches.size() < min_sample_size) return 0; 669 | if (candidate_matches.size() < sac_min_inliers_) return 0; 670 | 671 | // **** build 3D features for SVD ******************************** 672 | 673 | PointCloudFeature features_t, features_q; 674 | 675 | features_t.resize(candidate_matches.size()); 676 | features_q.resize(candidate_matches.size()); 677 | 678 | for (int m_idx = 0; m_idx < candidate_matches.size(); ++m_idx) 679 | { 680 | const cv::DMatch& match = candidate_matches[m_idx]; 681 | int idx_q = match.queryIdx; 682 | int idx_t = match.trainIdx; 683 | 684 | PointFeature& p_t = features_t[m_idx]; 685 | p_t.x = frame_t.kp_means[idx_t](0,0); 686 | p_t.y = frame_t.kp_means[idx_t](1,0); 687 | p_t.z = frame_t.kp_means[idx_t](2,0); 688 | 689 | PointFeature& p_q = features_q[m_idx]; 690 | p_q.x = frame_q.kp_means[idx_q](0,0); 691 | p_q.y = frame_q.kp_means[idx_q](1,0); 692 | p_q.z = frame_q.kp_means[idx_q](2,0); 693 | } 694 | 695 | // **** main RANSAC loop **************************************** 696 | 697 | TransformationEstimationSVD svd; 698 | Eigen::Matrix4f transformation; // transformation used inside loop 699 | best_inlier_matches.clear(); 700 | int iteration = 0; 701 | 702 | std::set mask; 703 | 704 | while(true) 705 | //for (iteration = 0; iteration < ransac_max_iterations_; ++iteration) 706 | { 707 | // generate random indices 708 | IntVector sample_idx; 709 | get3RandomIndices(candidate_matches.size(), mask, sample_idx); 710 | 711 | // build initial inliers from random indices 712 | IntVector inlier_idx; 713 | std::vector inlier_matches; 714 | 715 | for (unsigned int s_idx = 0; s_idx < sample_idx.size(); ++s_idx) 716 | { 717 | int m_idx = sample_idx[s_idx]; 718 | inlier_idx.push_back(m_idx); 719 | inlier_matches.push_back(candidate_matches[m_idx]); 720 | } 721 | 722 | // estimate transformation from minimum set of random samples 723 | svd.estimateRigidTransformation( 724 | features_q, inlier_idx, 725 | features_t, inlier_idx, 726 | transformation); 727 | 728 | // evaluate transformation fitness by checking distance to all points 729 | PointCloudFeature features_q_tf; 730 | pcl::transformPointCloud(features_q, features_q_tf, transformation); 731 | 732 | for (int m_idx = 0; m_idx < candidate_matches.size(); ++m_idx) 733 | { 734 | // euclidedan distance test 735 | const PointFeature& p_t = features_t[m_idx]; 736 | const PointFeature& p_q = features_q_tf[m_idx]; 737 | float eucl_dist_sq = distEuclideanSq(p_t, p_q); 738 | 739 | if (eucl_dist_sq < sac_max_eucl_dist_sq_) 740 | { 741 | inlier_idx.push_back(m_idx); 742 | inlier_matches.push_back(candidate_matches[m_idx]); 743 | 744 | // reestimate transformation from all inliers 745 | if (sac_reestimate_tf_) 746 | { 747 | svd.estimateRigidTransformation( 748 | features_q, inlier_idx, 749 | features_t, inlier_idx, 750 | transformation); 751 | pcl::transformPointCloud(features_q, features_q_tf, transformation); 752 | } 753 | } 754 | } 755 | 756 | // check if inliers are better than the best model so far 757 | if (inlier_matches.size() > best_inlier_matches.size()) 758 | { 759 | svd.estimateRigidTransformation( 760 | features_q, inlier_idx, 761 | features_t, inlier_idx, 762 | transformation); 763 | 764 | best_transformation = transformation; 765 | best_inlier_matches = inlier_matches; 766 | } 767 | 768 | double best_inlier_ratio = (double) best_inlier_matches.size() / 769 | (double) candidate_matches.size(); 770 | 771 | // **** termination: iterations + inlier ratio 772 | if(best_inlier_matches.size() < sac_min_inliers_) 773 | { 774 | if (iteration >= ransac_max_iterations_) break; 775 | } 776 | // **** termination: confidence ratio test 777 | else 778 | { 779 | double h = log_one_minus_ransac_confidence_ / 780 | log(1.0 - pow(best_inlier_ratio, min_sample_size)); 781 | 782 | if (iteration > (int)(h+1)) break; 783 | } 784 | 785 | iteration++; 786 | } 787 | 788 | return iteration; 789 | } 790 | 791 | } // namespace rgbdtools 792 | -------------------------------------------------------------------------------- /src/graph/keyframe_graph_solver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_graph_solver.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/graph/keyframe_graph_solver.h" 25 | 26 | namespace rgbdtools { 27 | 28 | KeyframeGraphSolver::KeyframeGraphSolver() 29 | { 30 | 31 | } 32 | 33 | KeyframeGraphSolver::~KeyframeGraphSolver() 34 | { 35 | 36 | } 37 | 38 | } // namespace rgbdtools 39 | -------------------------------------------------------------------------------- /src/graph/keyframe_graph_solver_g2o.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_graph_solver_g2o.cpp 3 | * @author Ivan Dryanovski 4 | * @note based on GraphOptimizer_G2O.cpp by Miguel Algaba Borrego 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/graph/keyframe_graph_solver_g2o.h" 25 | 26 | #include "g2o/core/factory.h" 27 | #include "g2o/solvers/csparse/linear_solver_csparse.h" 28 | #include "g2o/types/slam3d/types_slam3d.h" 29 | 30 | G2O_USE_TYPE_GROUP(slam3d); 31 | 32 | namespace rgbdtools { 33 | 34 | KeyframeGraphSolverG2O::KeyframeGraphSolverG2O(): 35 | KeyframeGraphSolver(), 36 | vertexIdx(0) 37 | { 38 | // Create the linear solver. 39 | linear_solver_ = 40 | new g2o::LinearSolverCSparse(); 41 | 42 | // Create the block solver on top of the linear solver. 43 | block_solver_ = new g2o::BlockSolverX(linear_solver_); 44 | 45 | // Create the algorithm to carry out the optimization. 46 | optimization_algorithm_ = new 47 | g2o::OptimizationAlgorithmLevenberg(block_solver_); 48 | 49 | // create the optimizer to load the data and carry out the optimization 50 | optimizer_.setVerbose(true); 51 | optimizer_.setAlgorithm(optimization_algorithm_); 52 | } 53 | 54 | KeyframeGraphSolverG2O::~KeyframeGraphSolverG2O() 55 | { 56 | delete linear_solver_; 57 | delete block_solver_; 58 | delete optimization_algorithm_; 59 | } 60 | 61 | void KeyframeGraphSolverG2O::solve( 62 | KeyframeVector& keyframes, 63 | const KeyframeAssociationVector& associations, 64 | AffineTransformVector& path) 65 | { 66 | // add vertices 67 | printf("Adding vertices...\n"); 68 | for (unsigned int p_idx = 0; p_idx < path.size(); ++p_idx) 69 | { 70 | addVertex(path[p_idx], p_idx); 71 | } 72 | 73 | // add edges odometry edges 74 | printf("Adding VO edges...\n"); 75 | 76 | if(path.size() > 1) 77 | { 78 | InformationMatrix path_inf = InformationMatrix::Identity(); 79 | 80 | for (unsigned int p_idx = 0; p_idx < path.size()-1; ++p_idx) 81 | { 82 | int from_idx = p_idx; 83 | int to_idx = p_idx+1; 84 | 85 | const AffineTransform& from_pose = path[from_idx]; 86 | const AffineTransform& to_pose = path[to_idx]; 87 | 88 | AffineTransform tf = from_pose.inverse() * to_pose; 89 | 90 | InformationMatrix inf = path_inf * 100.0; 91 | addEdge(from_idx, to_idx, tf, inf); 92 | } 93 | } 94 | 95 | printf("Adding RANSAC edges...\n"); 96 | InformationMatrix ransac_inf = InformationMatrix::Identity(); 97 | 98 | for (unsigned int as_idx = 0; as_idx < associations.size(); ++as_idx) 99 | { 100 | const KeyframeAssociation& association = associations[as_idx]; 101 | 102 | // skip non-ransac associations 103 | if (association.type != KeyframeAssociation::RANSAC) continue; 104 | 105 | // get the keyframe indices 106 | int kf_from_idx = association.kf_idx_a; 107 | int kf_to_idx = association.kf_idx_b; 108 | 109 | // get the corresponding frame indices (also matches path index) 110 | int from_idx = keyframes[kf_from_idx].index; 111 | int to_idx = keyframes[kf_to_idx].index; 112 | 113 | // calculate the information matrix 114 | int n_matches = association.matches.size(); 115 | InformationMatrix inf = ransac_inf; 116 | 117 | // add the edge 118 | addEdge(from_idx, to_idx, association.a2b, inf); 119 | } 120 | 121 | // run the optimization 122 | printf("Optimizing...\n"); 123 | optimizeGraph(); 124 | 125 | // update the path poses 126 | printf("Updating path poses...\n"); 127 | getOptimizedPoses(path); 128 | 129 | // update the keyframe poses 130 | printf("Updating keyframe poses...\n"); 131 | for (unsigned int kf_idx = 0; kf_idx < keyframes.size(); ++kf_idx) 132 | { 133 | RGBDKeyframe& keyframe = keyframes[kf_idx]; 134 | printf("keyframe.index: %d\n", keyframe.index); 135 | keyframe.pose = path[keyframe.index]; 136 | } 137 | } 138 | 139 | void KeyframeGraphSolverG2O::solve( 140 | KeyframeVector& keyframes, 141 | const KeyframeAssociationVector& associations) 142 | { 143 | // add vertices 144 | printf("Adding vertices...\n"); 145 | for (unsigned int kf_idx = 0; kf_idx < keyframes.size(); ++kf_idx) 146 | { 147 | const RGBDKeyframe& keyframe = keyframes[kf_idx]; 148 | addVertex(keyframe.pose, kf_idx); 149 | } 150 | 151 | // add edges odometry edges 152 | printf("Adding VO edges...\n"); 153 | 154 | InformationMatrix inf_identity = InformationMatrix::Identity(); 155 | 156 | for (unsigned int kf_idx = 0; kf_idx < keyframes.size()-1; ++kf_idx) 157 | { 158 | int from_idx = kf_idx; 159 | int to_idx = kf_idx+1; 160 | 161 | const AffineTransform& from_pose = keyframes[from_idx].pose; 162 | const AffineTransform& to_pose = keyframes[to_idx].pose; 163 | 164 | AffineTransform tf = from_pose.inverse() * to_pose; 165 | 166 | InformationMatrix inf = inf_identity * 100.0; 167 | addEdge(from_idx, to_idx, tf, inf); 168 | } 169 | 170 | // add edges 171 | printf("Adding RANSAC edges...\n"); 172 | for (unsigned int as_idx = 0; as_idx < associations.size(); ++as_idx) 173 | { 174 | const KeyframeAssociation& association = associations[as_idx]; 175 | int from_idx = association.kf_idx_a; 176 | int to_idx = association.kf_idx_b; 177 | 178 | int matches = association.matches.size(); 179 | InformationMatrix inf = inf_identity * matches; 180 | 181 | addEdge(from_idx, to_idx, association.a2b, inf); 182 | } 183 | 184 | // run the optimization 185 | printf("Optimizing...\n"); 186 | optimizeGraph(); 187 | 188 | // update the keyframe poses 189 | printf("Updating keyframe poses...\n"); 190 | 191 | AffineTransformVector optimized_poses; 192 | optimized_poses.resize(keyframes.size()); 193 | getOptimizedPoses(optimized_poses); 194 | 195 | for (unsigned int kf_idx = 0; kf_idx < keyframes.size(); ++kf_idx) 196 | { 197 | RGBDKeyframe& keyframe = keyframes[kf_idx]; 198 | keyframe.pose = optimized_poses[kf_idx]; 199 | } 200 | } 201 | 202 | void KeyframeGraphSolverG2O::addVertex( 203 | const AffineTransform& vertex_pose, 204 | int vertex_idx) 205 | { 206 | // TODO: use eigen quaternion, not manual conversion 207 | //Transform Eigen::Matrix4f into 3D traslation and rotation for g2o 208 | double yaw,pitch,roll; 209 | yaw = atan2f(vertex_pose(1,0),vertex_pose(0,0)); 210 | pitch = asinf(-vertex_pose(2,0)); 211 | roll = atan2f(vertex_pose(2,1),vertex_pose(2,2)); 212 | 213 | g2o::Vector3d t(vertex_pose(0,3),vertex_pose(1,3),vertex_pose(2,3)); 214 | g2o::Quaterniond q; 215 | q.x()=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2); 216 | q.y()=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2); 217 | q.z()=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2); 218 | q.w()=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2); 219 | 220 | g2o::SE3Quat pose(q,t); // vertex pose 221 | 222 | // TODO: smart pointers 223 | 224 | // set up node 225 | g2o::VertexSE3 *vc = new g2o::VertexSE3(); 226 | vc->setEstimate(pose); 227 | vc->setId(vertex_idx); 228 | 229 | // set first pose fixed 230 | if (vertex_idx == 0) 231 | vc->setFixed(true); 232 | 233 | // add to optimizer 234 | optimizer_.addVertex(vc); 235 | } 236 | 237 | void KeyframeGraphSolverG2O::addEdge( 238 | int from_idx, 239 | int to_idx, 240 | const AffineTransform& relative_pose, 241 | const Eigen::Matrix& information_matrix) 242 | { 243 | // TODO: use eigen quaternion, not manual conversion 244 | //Transform Eigen::Matrix4f into 3D traslation and rotation for g2o 245 | double yaw,pitch,roll; 246 | yaw = atan2f(relative_pose(1,0),relative_pose(0,0)); 247 | pitch = asinf(-relative_pose(2,0)); 248 | roll = atan2f(relative_pose(2,1),relative_pose(2,2)); 249 | 250 | g2o::Vector3d t(relative_pose(0,3),relative_pose(1,3),relative_pose(2,3)); 251 | g2o::Quaterniond q; 252 | q.x()=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2); 253 | q.y()=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2); 254 | q.z()=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2); 255 | q.w()=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2); 256 | 257 | // relative transformation 258 | g2o::SE3Quat transf(q,t); 259 | 260 | // TODO: smart pointers 261 | 262 | g2o::EdgeSE3* edge = new g2o::EdgeSE3; 263 | edge->vertices()[0] = optimizer_.vertex(from_idx); 264 | edge->vertices()[1] = optimizer_.vertex(to_idx); 265 | edge->setMeasurement(transf); 266 | 267 | //Set the information matrix 268 | edge->setInformation(information_matrix); 269 | 270 | optimizer_.addEdge(edge); 271 | } 272 | 273 | void KeyframeGraphSolverG2O::optimizeGraph() 274 | { 275 | //Prepare and run the optimization 276 | optimizer_.initializeOptimization(); 277 | 278 | //Set the initial Levenberg-Marquardt lambda 279 | // FIXME(idryanov): This does not compile 280 | //optimizer_.setUserLambdaInit(0.01); 281 | 282 | //Run optimization 283 | optimizer_.optimize(20); 284 | } 285 | 286 | void KeyframeGraphSolverG2O::getOptimizedPoses(AffineTransformVector& poses) 287 | { 288 | for (unsigned int idx = 0; idx < poses.size(); ++idx) 289 | { 290 | //Transform the vertex pose from G2O quaternion to Eigen::Matrix4f 291 | g2o::VertexSE3* vertex = dynamic_cast( 292 | optimizer_.vertex(idx)); 293 | double optimized_pose_quat[7]; 294 | vertex->getEstimateData(optimized_pose_quat); 295 | 296 | AffineTransform optimized_pose; 297 | double qx,qy,qz,qr,qx2,qy2,qz2,qr2; 298 | 299 | qx=optimized_pose_quat[3]; 300 | qy=optimized_pose_quat[4]; 301 | qz=optimized_pose_quat[5]; 302 | qr=optimized_pose_quat[6]; 303 | qx2=qx*qx; 304 | qy2=qy*qy; 305 | qz2=qz*qz; 306 | qr2=qr*qr; 307 | 308 | optimized_pose(0,0)=qr2+qx2-qy2-qz2; 309 | optimized_pose(0,1)=2*(qx*qy-qr*qz); 310 | optimized_pose(0,2)=2*(qz*qx+qr*qy); 311 | optimized_pose(0,3)=optimized_pose_quat[0]; 312 | optimized_pose(1,0)=2*(qx*qy+qr*qz); 313 | optimized_pose(1,1)=qr2-qx2+qy2-qz2; 314 | optimized_pose(1,2)=2*(qy*qz-qr*qx); 315 | optimized_pose(1,3)=optimized_pose_quat[1]; 316 | optimized_pose(2,0)=2*(qz*qx-qr*qy); 317 | optimized_pose(2,1)=2*(qy*qz+qr*qx); 318 | optimized_pose(2,2)=qr2-qx2-qy2+qz2; 319 | optimized_pose(2,3)=optimized_pose_quat[2]; 320 | 321 | //Set the optimized pose to the vector of poses 322 | poses[idx] = optimized_pose; 323 | } 324 | } 325 | 326 | } // namespace ccny_rgbd 327 | -------------------------------------------------------------------------------- /src/registration/motion_estimation.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motion_estimation.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/registration/motion_estimation.h" 25 | 26 | namespace rgbdtools { 27 | 28 | MotionEstimation::MotionEstimation() 29 | { 30 | 31 | } 32 | 33 | MotionEstimation::~MotionEstimation() 34 | { 35 | 36 | } 37 | 38 | AffineTransform MotionEstimation::getMotionEstimation( 39 | RGBDFrame& frame, 40 | const AffineTransform& prediction) 41 | { 42 | ///@todo this should return a covariance 43 | 44 | AffineTransform motion; 45 | bool result; 46 | 47 | if (frame.n_valid_keypoints == 0) 48 | { 49 | std::cerr << "No features detected." << std::endl; 50 | result = false; 51 | } 52 | else 53 | { 54 | result = getMotionEstimationImpl(frame, prediction, motion); 55 | } 56 | 57 | if (!result) 58 | { 59 | motion.setIdentity(); 60 | } 61 | 62 | return motion; 63 | } 64 | 65 | void MotionEstimation::constrainMotion(AffineTransform& motion) 66 | { 67 | // **** degree-of-freedom constraints 68 | 69 | if (motion_constraint_ == ROLL_PITCH) 70 | { 71 | float x, y, z, roll, pitch, yaw; 72 | eigenAffineToXYZRPY(motion, x, y, z, roll, pitch, yaw); 73 | XYZRPYToEigenAffine(x, y, z, 0, 0, yaw, motion); 74 | } 75 | else if (motion_constraint_ == ROLL_PITCH_Z) 76 | { 77 | float x, y, z, roll, pitch, yaw; 78 | eigenAffineToXYZRPY(motion, x, y, z, roll, pitch, yaw); 79 | XYZRPYToEigenAffine(x, y, 0, 0, 0, yaw, motion); 80 | } 81 | } 82 | 83 | void MotionEstimation::setBaseToCameraTf(const AffineTransform& b2c) 84 | { 85 | b2c_ = b2c; 86 | } 87 | 88 | void MotionEstimation::setMotionConstraint(int motion_constraint) 89 | { 90 | motion_constraint_ = motion_constraint; 91 | } 92 | 93 | } // namespace rgbdtools 94 | -------------------------------------------------------------------------------- /src/registration/motion_estimation_icp_prob_model.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motion_estimation_icp_prob_model.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/registration/motion_estimation_icp_prob_model.h" 25 | 26 | namespace rgbdtools { 27 | 28 | MotionEstimationICPProbModel::MotionEstimationICPProbModel(): 29 | MotionEstimation(), 30 | model_idx_(0), 31 | model_size_(0) 32 | { 33 | // parameters 34 | tf_epsilon_linear_ = 1e-4; // 1 mm 35 | tf_epsilon_angular_ = 1.7e-3; // 1 deg 36 | max_iterations_ = 10; 37 | min_correspondences_ = 15; 38 | max_model_size_ = 3000; 39 | max_corresp_dist_eucl_ = 0.15; 40 | max_assoc_dist_mah_ = 10.0; 41 | n_nearest_neighbors_ = 4; 42 | 43 | // derived params 44 | max_corresp_dist_eucl_sq_ = max_corresp_dist_eucl_ * max_corresp_dist_eucl_; 45 | max_assoc_dist_mah_sq_ = max_assoc_dist_mah_ * max_assoc_dist_mah_; 46 | 47 | // state variables 48 | model_ptr_.reset(new PointCloudFeature()); 49 | 50 | f2b_.setIdentity(); 51 | I_.setIdentity(); 52 | } 53 | 54 | MotionEstimationICPProbModel::~MotionEstimationICPProbModel() 55 | { 56 | 57 | } 58 | 59 | void MotionEstimationICPProbModel::addToModel( 60 | const Vector3f& data_mean, 61 | const Matrix3f& data_cov) 62 | { 63 | // **** create a PCL point 64 | 65 | PointFeature data_point; 66 | data_point.x = data_mean(0,0); 67 | data_point.y = data_mean(1,0); 68 | data_point.z = data_mean(2,0); 69 | 70 | if (model_size_ < max_model_size_) 71 | { 72 | covariances_.push_back(data_cov); 73 | means_.push_back(data_mean); 74 | model_ptr_->push_back(data_point); 75 | 76 | model_size_++; 77 | } 78 | else // model_size_ == max_model_size_ 79 | { 80 | if (model_idx_ == max_model_size_) 81 | model_idx_ = 0; 82 | 83 | covariances_.at(model_idx_) = data_cov; 84 | means_.at(model_idx_) = data_mean; 85 | model_ptr_->at(model_idx_) = data_point; 86 | } 87 | 88 | model_idx_++; 89 | } 90 | 91 | bool MotionEstimationICPProbModel::getMotionEstimationImpl( 92 | RGBDFrame& frame, 93 | const AffineTransform& prediction, 94 | AffineTransform& motion) 95 | { 96 | /// @todo: currently ignores prediction 97 | 98 | bool result; 99 | Vector3fVector data_means; 100 | Matrix3fVector data_covariances; 101 | 102 | // remove nans from distributinos 103 | removeInvalidDistributions( 104 | frame.kp_means, frame.kp_covariances, frame.kp_valid, 105 | data_means, data_covariances); 106 | 107 | // transform distributions to world frame 108 | transformDistributions(data_means, data_covariances, prediction * f2b_* b2c_); 109 | 110 | // **** perform registration 111 | 112 | if (model_size_ == 0) 113 | { 114 | std::cout << "No points in model: initializing from features." << std::endl; 115 | motion.setIdentity(); 116 | initializeModelFromData(data_means, data_covariances); 117 | result = true; 118 | } 119 | else 120 | { 121 | AffineTransform motion_icp; 122 | 123 | // align using icp 124 | result = alignICPEuclidean(data_means, motion_icp); 125 | 126 | if (!result) 127 | { 128 | motion_icp.setIdentity(); 129 | std::cerr << "ICP alignment failed. Using identity transform." << std::endl; 130 | } 131 | 132 | motion = motion_icp * prediction; 133 | 134 | constrainMotion(motion); 135 | f2b_ = motion * f2b_; 136 | 137 | // transform distributions to world frame 138 | transformDistributions(data_means, data_covariances, motion_icp); 139 | 140 | // update model: inserts new features and updates old ones with KF 141 | updateModelFromData(data_means, data_covariances); 142 | } 143 | 144 | // update the model tree 145 | model_tree_.setInputCloud(model_ptr_); 146 | 147 | // update model metadata 148 | model_ptr_->width = model_ptr_->points.size(); 149 | 150 | // Stamp of the point cloud, in usec. 151 | model_ptr_->header.stamp = frame.header.stamp.sec * 1e6 + 152 | frame.header.stamp.nsec * 1e-3; 153 | 154 | return result; 155 | } 156 | 157 | bool MotionEstimationICPProbModel::alignICPEuclidean( 158 | const Vector3fVector& data_means, 159 | AffineTransform& correction) 160 | { 161 | TransformationEstimationSVD svd; 162 | 163 | // create a point cloud from the means 164 | PointCloudFeature data_cloud; 165 | pointCloudFromMeans(data_means, data_cloud); 166 | 167 | // initialize the result transform 168 | AffineTransform final_transformation; 169 | final_transformation.setIdentity(); 170 | 171 | for (int iteration = 0; iteration < max_iterations_; ++iteration) 172 | { 173 | // get corespondences 174 | IntVector data_indices, model_indices; 175 | getCorrespEuclidean(data_cloud, data_indices, model_indices); 176 | 177 | if ((int)data_indices.size() < min_correspondences_) 178 | { 179 | std::cerr << "[ICP] Not enough correspondences (" 180 | << (int)data_indices.size() << " of " 181 | << min_correspondences_ << " minimum). Leaving ICP loop" 182 | << std::endl; 183 | return false; 184 | } 185 | 186 | // estimae transformation 187 | Eigen::Matrix4f transform_eigen; 188 | svd.estimateRigidTransformation (data_cloud, data_indices, 189 | *model_ptr_, model_indices, 190 | transform_eigen); 191 | 192 | 193 | AffineTransform transformation(transform_eigen); 194 | 195 | // rotate 196 | pcl::transformPointCloud(data_cloud, data_cloud, transformation); 197 | 198 | // accumulate incremental tf 199 | final_transformation = transformation * final_transformation; 200 | 201 | // check for convergence 202 | double linear, angular; 203 | getTfDifference(transformation, linear, angular); 204 | if (linear < tf_epsilon_linear_ && 205 | angular < tf_epsilon_angular_) 206 | { 207 | //printf("(%f %f) conv. at [%d] leaving loop\n", 208 | // linear*1000.0, angular*10.0*180.0/3.14, iteration); 209 | break; 210 | } 211 | } 212 | 213 | correction = final_transformation; 214 | return true; 215 | } 216 | 217 | void MotionEstimationICPProbModel::getCorrespEuclidean( 218 | const PointCloudFeature& data_cloud, 219 | IntVector& data_indices, 220 | IntVector& model_indices) 221 | { 222 | for (unsigned int data_idx = 0; data_idx < data_cloud.size(); ++data_idx) 223 | { 224 | const PointFeature& data_point = data_cloud.points[data_idx]; 225 | 226 | int eucl_nn_idx; 227 | double eucl_dist_sq; 228 | 229 | bool nn_result = getNNEuclidean(data_point, eucl_nn_idx, eucl_dist_sq); 230 | 231 | if (nn_result && eucl_dist_sq < max_corresp_dist_eucl_sq_) 232 | { 233 | data_indices.push_back(data_idx); 234 | model_indices.push_back(eucl_nn_idx); 235 | } 236 | } 237 | } 238 | 239 | bool MotionEstimationICPProbModel::getNNEuclidean( 240 | const PointFeature& data_point, 241 | int& eucl_nn_idx, double& eucl_dist_sq) 242 | { 243 | // find n Euclidean nearest neighbors 244 | IntVector indices; 245 | FloatVector dist_sq; 246 | 247 | indices.resize(1); 248 | dist_sq.resize(1); 249 | 250 | int n_retrieved = model_tree_.nearestKSearch(data_point, 1, indices, dist_sq); 251 | 252 | if (n_retrieved != 0) 253 | { 254 | eucl_nn_idx = indices[0]; 255 | eucl_dist_sq = dist_sq[0]; 256 | return true; 257 | } 258 | else return false; 259 | } 260 | 261 | bool MotionEstimationICPProbModel::getNNMahalanobis( 262 | const Vector3f& data_mean, const Matrix3f& data_cov, 263 | int& mah_nn_idx, double& mah_dist_sq, 264 | IntVector& indices, FloatVector& dists_sq) 265 | { 266 | PointFeature p_data; 267 | p_data.x = data_mean(0,0); 268 | p_data.y = data_mean(1,0); 269 | p_data.z = data_mean(2,0); 270 | 271 | int n_retrieved = model_tree_.nearestKSearch(p_data, n_nearest_neighbors_, indices, dists_sq); 272 | 273 | // iterate over Euclidean NNs to find Mah. NN 274 | double best_mah_dist_sq = 0; 275 | int best_mah_nn_idx = -1; 276 | //int best_i = 0; // optionally print this to check how far in we found the best one 277 | for (int i = 0; i < n_retrieved; i++) 278 | { 279 | int nn_idx = indices[i]; 280 | 281 | const Vector3f& model_mean = means_[nn_idx]; 282 | const Matrix3f& model_cov = covariances_[nn_idx]; 283 | 284 | Vector3f diff_mat = model_mean - data_mean; 285 | Matrix3f sum_cov = model_cov + data_cov; 286 | Matrix3f sum_cov_inv = sum_cov.inverse(); 287 | 288 | Eigen::Matrix mah_mat = diff_mat.transpose() * sum_cov_inv * diff_mat; 289 | 290 | double mah_dist_sq = mah_mat(0,0); 291 | 292 | if (best_mah_nn_idx == -1 || mah_dist_sq < best_mah_dist_sq) 293 | { 294 | best_mah_dist_sq = mah_dist_sq; 295 | best_mah_nn_idx = nn_idx; 296 | //best_i = i; 297 | } 298 | } 299 | 300 | if (best_mah_nn_idx != -1) 301 | { 302 | //if (best_i != 0) printf("BEST NEIGHBOR WAS #%d\n", best_i); 303 | mah_dist_sq = best_mah_dist_sq; 304 | mah_nn_idx = best_mah_nn_idx; 305 | return true; 306 | } 307 | else return false; 308 | } 309 | 310 | void MotionEstimationICPProbModel::initializeModelFromData( 311 | const Vector3fVector& data_means, 312 | const Matrix3fVector& data_covariances) 313 | { 314 | for (unsigned int idx = 0; idx < data_means.size(); ++idx) 315 | { 316 | const Vector3f& mean = data_means[idx]; 317 | const Matrix3f& cov = data_covariances[idx]; 318 | addToModel(mean, cov); 319 | } 320 | } 321 | 322 | void MotionEstimationICPProbModel::updateModelFromData( 323 | const Vector3fVector& data_means, 324 | const Matrix3fVector& data_covariances) 325 | { 326 | // pre-allocate search vectors 327 | IntVector indices; 328 | FloatVector dists_sq; 329 | indices.resize(n_nearest_neighbors_); 330 | dists_sq.resize(n_nearest_neighbors_); 331 | 332 | for (unsigned int idx = 0; idx < data_means.size(); ++idx) 333 | { 334 | const Vector3f& data_mean = data_means[idx]; 335 | const Matrix3f& data_cov = data_covariances[idx]; 336 | 337 | // find nearest neighbor in model 338 | double mah_dist_sq; 339 | int mah_nn_idx; 340 | bool nn_result = getNNMahalanobis( 341 | data_mean, data_cov, mah_nn_idx, mah_dist_sq, indices, dists_sq); 342 | 343 | if (nn_result && mah_dist_sq < max_assoc_dist_mah_sq_) 344 | { 345 | // **** KF update ********************************* 346 | 347 | // predicted state 348 | const Vector3f& model_mean_pred = means_[mah_nn_idx]; 349 | const Matrix3f& model_cov_pred = covariances_[mah_nn_idx]; 350 | 351 | // calculate measurement and cov residual 352 | Vector3f y = data_mean - model_mean_pred; 353 | Matrix3f S = data_cov + model_cov_pred; 354 | 355 | // calculate Kalman gain 356 | Matrix3f K = model_cov_pred * S.inverse(); 357 | 358 | // updated state estimate (mean and cov) 359 | Vector3f model_mean_upd = model_mean_pred + K * y; 360 | Matrix3f model_cov_upd = (I_ - K) * model_cov_pred; 361 | 362 | // update in model 363 | means_[mah_nn_idx] = model_mean_upd; 364 | covariances_[mah_nn_idx] = model_cov_upd; 365 | 366 | PointFeature updated_point; 367 | updated_point.x = model_mean_upd(0,0); 368 | updated_point.y = model_mean_upd(1,0); 369 | updated_point.z = model_mean_upd(2,0); 370 | 371 | model_ptr_->points[mah_nn_idx] = updated_point; 372 | } 373 | else 374 | { 375 | addToModel(data_mean, data_cov); 376 | } 377 | } 378 | } 379 | 380 | bool MotionEstimationICPProbModel::saveModel(const std::string& filename) 381 | { 382 | /// @todo also save Eigen means and covariances 383 | /* 384 | // save as OpenCV yml matrix 385 | std::string filename_yml = filename + ".yml"; 386 | 387 | cv::FileStorage fs(filename_yml, cv::FileStorage::WRITE); 388 | fs << "means" << means_; 389 | fs << "covariances" << covariances_; 390 | fs << "model_idx" << model_idx_; 391 | fs << "model_size" << model_size_; 392 | */ 393 | 394 | // save as pcd 395 | std::string filename_pcd = filename + ".pcd"; 396 | pcl::PCDWriter writer; 397 | int result_pcd = writer.writeBinary(filename_pcd, *model_ptr_); 398 | 399 | return (result_pcd == 0); 400 | } 401 | 402 | void MotionEstimationICPProbModel::setMaxIterations(int max_iterations) 403 | { 404 | max_iterations_ = max_iterations; 405 | } 406 | 407 | void MotionEstimationICPProbModel::setMinCorrespondences(int min_correspondences) 408 | { 409 | min_correspondences_ = min_correspondences; 410 | } 411 | 412 | void MotionEstimationICPProbModel::setNNearestNeighbors(int n_nearest_neighbors) 413 | { 414 | n_nearest_neighbors_ = n_nearest_neighbors; 415 | } 416 | 417 | void MotionEstimationICPProbModel::setMaxModelSize(int max_model_size) 418 | { 419 | max_model_size_ = max_model_size; 420 | } 421 | 422 | void MotionEstimationICPProbModel::setTfEpsilonLinear(double tf_epsilon_linear) 423 | { 424 | tf_epsilon_linear_ = tf_epsilon_linear; 425 | } 426 | 427 | void MotionEstimationICPProbModel::setTfEpsilonAngular(double tf_epsilon_angular) 428 | { 429 | tf_epsilon_angular_ = tf_epsilon_angular; 430 | } 431 | 432 | void MotionEstimationICPProbModel::setMaxAssociationDistMahalanobis(double max_assoc_dist_mah) 433 | { 434 | max_assoc_dist_mah_ = max_assoc_dist_mah; 435 | 436 | max_assoc_dist_mah_sq_ = max_assoc_dist_mah_ * max_assoc_dist_mah_; 437 | } 438 | 439 | void MotionEstimationICPProbModel::setMaxCorrespondenceDistEuclidean(double max_corresp_dist_eucl) 440 | { 441 | max_corresp_dist_eucl_ = max_corresp_dist_eucl; 442 | 443 | max_corresp_dist_eucl_sq_ = max_corresp_dist_eucl_ * max_corresp_dist_eucl_; 444 | } 445 | 446 | } // namespace rgbdtools 447 | -------------------------------------------------------------------------------- /src/registration/motion_estimation_pairwise_ransac.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motion_estimation_pairwise_ransac.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/registration/motion_estimation_pairwise_ransac.h" 25 | 26 | namespace rgbdtools { 27 | 28 | MotionEstimationPairwiseRANSAC::MotionEstimationPairwiseRANSAC(): 29 | MotionEstimation(), 30 | initialized_(false) 31 | { 32 | sac_min_inliers_ = 30; 33 | sac_max_eucl_dist_sq_ = pow(0.03, 2.0); 34 | sac_reestimate_tf_ = false; 35 | ransac_max_iterations_ = 200; 36 | ransac_confidence_ = 0.999; 37 | matcher_use_desc_ratio_test_ = true; 38 | matcher_max_desc_ratio_ = 0.8; 39 | matcher_max_desc_dist_ = 50.0; 40 | 41 | // derived parameters 42 | log_one_minus_ransac_confidence_ = log(1.0 - ransac_confidence_); 43 | 44 | f2b_.setIdentity(); 45 | 46 | //cv::namedWindow("sacmatches", 0); 47 | } 48 | 49 | MotionEstimationPairwiseRANSAC::~MotionEstimationPairwiseRANSAC() 50 | { 51 | 52 | } 53 | 54 | bool MotionEstimationPairwiseRANSAC::getMotionEstimationImpl( 55 | RGBDFrame& frame, 56 | const AffineTransform& prediction, 57 | AffineTransform& motion) 58 | { 59 | bool result; 60 | 61 | if (!initialized_) 62 | { 63 | initialized_ = true; 64 | result = false; 65 | } 66 | else 67 | { 68 | DMatchVector sac_matches; 69 | Eigen::Matrix4f sac_transformation; 70 | int ransac_iterations = pairwiseMatchingRANSAC( 71 | frame, prev_frame_, sac_matches, sac_transformation); 72 | 73 | //printf("ransac_iterations: %d\n", ransac_iterations); 74 | if (sac_matches.size() > sac_min_inliers_) 75 | { 76 | // express the motion in the fixed frame 77 | AffineTransform motion_cam(sac_transformation); 78 | AffineTransform f2c = f2b_ * b2c_; 79 | AffineTransform f2c_new = f2c * motion_cam; 80 | motion = f2c_new * f2c.inverse(); 81 | 82 | // update the pose of the base frame 83 | f2b_ = f2c_new * b2c_.inverse(); 84 | 85 | //delete prev_frame_; 86 | 87 | result = true; 88 | 89 | /* 90 | // visualize 91 | cv::Mat img_matches; 92 | cv::drawMatches(frame.rgb_img, frame.keypoints, 93 | prev_frame_.rgb_img, prev_frame_.keypoints, 94 | sac_matches, img_matches); 95 | cv::imshow("sacmatches", img_matches); 96 | cv::waitKey(1);*/ 97 | } 98 | else 99 | { 100 | /* 101 | // visualize 102 | cv::Mat img_matches; 103 | cv::drawMatches(frame.rgb_img, frame.keypoints, 104 | prev_frame_.rgb_img, prev_frame_.keypoints, 105 | DMatchVector(), img_matches); 106 | cv::imshow("sacmatches", img_matches); 107 | cv::waitKey(1);*/ 108 | 109 | std::cerr << "RANSAC alignment failed. Using identity transform." << std::endl; 110 | result = false; 111 | } 112 | } 113 | 114 | prev_frame_ = RGBDFrame(frame); 115 | 116 | return result; 117 | } 118 | 119 | int MotionEstimationPairwiseRANSAC::pairwiseMatchingRANSAC( 120 | const RGBDFrame& frame_q, const RGBDFrame& frame_t, 121 | DMatchVector& best_inlier_matches, 122 | Eigen::Matrix4f& best_transformation) 123 | { 124 | // constants 125 | int min_sample_size = 3; 126 | 127 | // **** establish matches *************************************** 128 | 129 | // build matcher 130 | cv::BFMatcher matcher(cv::NORM_HAMMING, false); 131 | 132 | // train 133 | //printf("training\n"); 134 | std::vector descriptors_vector; 135 | descriptors_vector.push_back(frame_t.descriptors); 136 | matcher.add(descriptors_vector); 137 | matcher.train(); 138 | 139 | // get matchers 140 | DMatchVector candidate_matches; 141 | getCandidateMatches(frame_q, frame_t, matcher, candidate_matches); 142 | 143 | //printf("candidate_matches.size(): %d\n", (int)candidate_matches.size()); 144 | 145 | // check if enough matches are present 146 | if (candidate_matches.size() < min_sample_size) return 0; 147 | if (candidate_matches.size() < sac_min_inliers_) return 0; 148 | 149 | // **** build 3D features for SVD ******************************** 150 | 151 | PointCloudFeature features_t, features_q; 152 | 153 | features_t.resize(candidate_matches.size()); 154 | features_q.resize(candidate_matches.size()); 155 | 156 | for (int m_idx = 0; m_idx < candidate_matches.size(); ++m_idx) 157 | { 158 | const cv::DMatch& match = candidate_matches[m_idx]; 159 | int idx_q = match.queryIdx; 160 | int idx_t = match.trainIdx; 161 | 162 | PointFeature& p_t = features_t[m_idx]; 163 | p_t.x = frame_t.kp_means[idx_t](0,0); 164 | p_t.y = frame_t.kp_means[idx_t](1,0); 165 | p_t.z = frame_t.kp_means[idx_t](2,0); 166 | 167 | PointFeature& p_q = features_q[m_idx]; 168 | p_q.x = frame_q.kp_means[idx_q](0,0); 169 | p_q.y = frame_q.kp_means[idx_q](1,0); 170 | p_q.z = frame_q.kp_means[idx_q](2,0); 171 | } 172 | 173 | // **** main RANSAC loop **************************************** 174 | 175 | TransformationEstimationSVD svd; 176 | Eigen::Matrix4f transformation; // transformation used inside loop 177 | best_inlier_matches.clear(); 178 | std::set mask; 179 | 180 | int iteration = 0; 181 | for (iteration = 0; iteration < ransac_max_iterations_; ++iteration) 182 | { 183 | // generate random indices 184 | IntVector sample_idx; 185 | get3RandomIndices(candidate_matches.size(), mask, sample_idx); 186 | 187 | // build initial inliers from random indices 188 | IntVector init_inlier_idx; 189 | std::vector init_inlier_matches; 190 | 191 | for (unsigned int s_idx = 0; s_idx < sample_idx.size(); ++s_idx) 192 | { 193 | int m_idx = sample_idx[s_idx]; 194 | init_inlier_idx.push_back(m_idx); 195 | init_inlier_matches.push_back(candidate_matches[m_idx]); 196 | } 197 | 198 | // estimate transformation from minimum set of random samples 199 | svd.estimateRigidTransformation( 200 | features_q, init_inlier_idx, 201 | features_t, init_inlier_idx, 202 | transformation); 203 | 204 | // transformation rejection 205 | 206 | /* 207 | double angular, linear; 208 | AffineTransform temp(transformation); 209 | getTfDifference(temp, linear, angular); 210 | if (linear > 0.10 || angular > 5.0 * M_PI / 180.0) 211 | continue; 212 | */ 213 | 214 | // evaluate transformation fitness by checking distance to all points 215 | PointCloudFeature features_q_tf; 216 | pcl::transformPointCloud(features_q, features_q_tf, transformation); 217 | 218 | IntVector inlier_idx; 219 | std::vector inlier_matches; 220 | 221 | for (int m_idx = 0; m_idx < candidate_matches.size(); ++m_idx) 222 | { 223 | // euclidedan distance test 224 | const PointFeature& p_t = features_t[m_idx]; 225 | const PointFeature& p_q = features_q_tf[m_idx]; 226 | float eucl_dist_sq = distEuclideanSq(p_t, p_q); 227 | 228 | if (eucl_dist_sq < sac_max_eucl_dist_sq_) 229 | { 230 | inlier_idx.push_back(m_idx); 231 | inlier_matches.push_back(candidate_matches[m_idx]); 232 | 233 | // reestimate transformation from all inliers 234 | if (sac_reestimate_tf_) 235 | { 236 | svd.estimateRigidTransformation( 237 | features_q, inlier_idx, 238 | features_t, inlier_idx, 239 | transformation); 240 | pcl::transformPointCloud(features_q, features_q_tf, transformation); 241 | } 242 | } 243 | } 244 | 245 | // check if inliers are better than the best model so far 246 | if (inlier_matches.size() > best_inlier_matches.size()) 247 | { 248 | svd.estimateRigidTransformation( 249 | features_q, inlier_idx, 250 | features_t, inlier_idx, 251 | transformation); 252 | 253 | best_transformation = transformation; 254 | best_inlier_matches = inlier_matches; 255 | } 256 | 257 | double best_inlier_ratio = (double) best_inlier_matches.size() / 258 | (double) candidate_matches.size(); 259 | 260 | // **** early termination: iterations + inlier ratio 261 | if(best_inlier_matches.size() >= sac_min_inliers_) 262 | { 263 | double h = log_one_minus_ransac_confidence_ / 264 | log(1.0 - pow(best_inlier_ratio, min_sample_size)); 265 | 266 | if (iteration > (int)(h+1)) break; 267 | } 268 | } 269 | 270 | //printf("best_inlier_matches.size(): %d\n", (int)best_inlier_matches.size()); 271 | 272 | return iteration; 273 | } 274 | 275 | // frame_a = train, frame_b = query 276 | void MotionEstimationPairwiseRANSAC::getCandidateMatches( 277 | const RGBDFrame& frame_q, const RGBDFrame& frame_t, 278 | cv::DescriptorMatcher& matcher, 279 | DMatchVector& candidate_matches) 280 | { 281 | // **** build candidate matches *********************************** 282 | // assumes detectors and distributions are computed 283 | // establish all matches from b to a 284 | 285 | if (matcher_use_desc_ratio_test_) 286 | { 287 | std::vector all_matches2; 288 | 289 | matcher.knnMatch( 290 | frame_q.descriptors, all_matches2, 2); 291 | 292 | for (unsigned int m_idx = 0; m_idx < all_matches2.size(); ++m_idx) 293 | { 294 | const cv::DMatch& match1 = all_matches2[m_idx][0]; 295 | const cv::DMatch& match2 = all_matches2[m_idx][1]; 296 | 297 | double ratio = match1.distance / match2.distance; 298 | 299 | // remove bad matches - ratio test, valid keypoints 300 | if (ratio < matcher_max_desc_ratio_) 301 | { 302 | int idx_q = match1.queryIdx; 303 | int idx_t = match1.trainIdx; 304 | 305 | if (frame_t.kp_valid[idx_t] && frame_q.kp_valid[idx_q]) 306 | candidate_matches.push_back(match1); 307 | } 308 | } 309 | } 310 | else 311 | { 312 | DMatchVector all_matches; 313 | 314 | matcher.match( 315 | frame_q.descriptors, all_matches); 316 | 317 | for (unsigned int m_idx = 0; m_idx < all_matches.size(); ++m_idx) 318 | { 319 | const cv::DMatch& match = all_matches[m_idx]; 320 | 321 | // remove bad matches - descriptor distance, valid keypoints 322 | if (match.distance < matcher_max_desc_dist_) 323 | { 324 | int idx_q = match.queryIdx; 325 | int idx_t = match.trainIdx; 326 | 327 | if (frame_t.kp_valid[idx_t] && frame_q.kp_valid[idx_q]) 328 | candidate_matches.push_back(match); 329 | } 330 | } 331 | } 332 | } 333 | 334 | } // namespace rgbdtools 335 | -------------------------------------------------------------------------------- /src/rgbd_frame.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_frame.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/rgbd_frame.h" 25 | 26 | namespace rgbdtools { 27 | 28 | RGBDFrame::RGBDFrame() 29 | { 30 | 31 | } 32 | 33 | RGBDFrame::RGBDFrame(const RGBDFrame& other) 34 | { 35 | index = other.index; 36 | 37 | rgb_img = other.rgb_img.clone(); 38 | depth_img = other.depth_img.clone(); 39 | 40 | header = other.header; 41 | intr = other.intr; 42 | 43 | keypoints = other.keypoints; 44 | descriptors = other.descriptors.clone(); 45 | 46 | kp_valid = other.kp_valid; 47 | kp_means = other.kp_means; 48 | kp_covariances = other.kp_covariances; 49 | n_valid_keypoints = other.n_valid_keypoints; 50 | } 51 | 52 | RGBDFrame::RGBDFrame( 53 | const cv::Mat& rgb_img_in, 54 | const cv::Mat& depth_img_in, 55 | const cv::Mat& intr_in, 56 | const Header& header_in) 57 | { 58 | // zero-copy assignment 59 | rgb_img = rgb_img_in; 60 | depth_img = depth_img_in; 61 | 62 | header = header_in; 63 | intr = intr_in; 64 | 65 | // @FIXME: handle encodings 66 | /* 67 | const std::string& enc = depth_msg->encoding; 68 | if (enc.compare("16UC1") == 0) 69 | depth_img = cv_bridge::toCvShare(depth_msg)->image; 70 | else if (enc.compare("32FC1") == 0) 71 | depthImageFloatTo16bit(cv_bridge::toCvShare(depth_msg)->image, depth_img); 72 | */ 73 | 74 | // @FIXME double vs float - change all to float 75 | } 76 | 77 | /* 78 | RGBDFrame::RGBDFrame( 79 | const ImageMsg::ConstPtr& rgb_msg, 80 | const ImageMsg::ConstPtr& depth_msg, 81 | const CameraInfoMsg::ConstPtr& info_msg) 82 | { 83 | rgb_img = cv_bridge::toCvShare(rgb_msg)->image; 84 | 85 | // handles 16UC1 natively 86 | // 32FC1 need to be converted into 16UC1 87 | const std::string& enc = depth_msg->encoding; 88 | if (enc.compare("16UC1") == 0) 89 | depth_img = cv_bridge::toCvShare(depth_msg)->image; 90 | else if (enc.compare("32FC1") == 0) 91 | depthImageFloatTo16bit(cv_bridge::toCvShare(depth_msg)->image, depth_img); 92 | 93 | header = rgb_msg->header; 94 | 95 | model.fromCameraInfo(info_msg); 96 | }*/ 97 | 98 | double RGBDFrame::getStdDevZ(double z) const 99 | { 100 | return Z_STDEV_CONSTANT * z * z; 101 | } 102 | 103 | double RGBDFrame::getVarZ(double z) const 104 | { 105 | double std_dev_z = getStdDevZ(z); 106 | return std_dev_z * std_dev_z; 107 | } 108 | 109 | void RGBDFrame::getGaussianDistribution( 110 | int u, int v, double& z_mean, double& z_var) const 111 | { 112 | // get raw z value (in mm) 113 | uint16_t z_raw = depth_img.at(v, u); 114 | 115 | // z [meters] 116 | z_mean = z_raw * 0.001; 117 | 118 | // var_z [meters] 119 | z_var = getVarZ(z_mean); 120 | } 121 | 122 | void RGBDFrame::getGaussianMixtureDistribution( 123 | int u, int v, double& z_mean, double& z_var) const 124 | { 125 | /// @todo Different window sizes? based on sigma_u, sigma_v? 126 | int w = 1; 127 | 128 | int u_start = std::max(u - w, 0); 129 | int v_start = std::max(v - w, 0); 130 | int u_end = std::min(u + w, depth_img.cols - 1); 131 | int v_end = std::min(v + w, depth_img.rows - 1); 132 | 133 | // iterate accross window - find mean 134 | double weight_sum = 0.0; 135 | double mean_sum = 0.0; 136 | double alpha_sum = 0.0; 137 | 138 | for (int uu = u_start; uu <= u_end; ++uu) 139 | for (int vv = v_start; vv <= v_end; ++vv) 140 | { 141 | uint16_t z_neighbor_raw = depth_img.at(vv, uu); 142 | 143 | if (z_neighbor_raw != 0) 144 | { 145 | double z_neighbor = z_neighbor_raw * 0.001; 146 | 147 | // determine and aggregate weight 148 | double weight; 149 | if (u==uu && v==vv) weight = 4.0; 150 | else if (u==uu || v==vv) weight = 2.0; 151 | else weight = 1.0; 152 | weight_sum += weight; 153 | 154 | // aggregate mean 155 | mean_sum += weight * z_neighbor; 156 | 157 | // aggregate var 158 | double var_z_neighbor = getVarZ(z_neighbor); 159 | alpha_sum += weight * (var_z_neighbor + z_neighbor * z_neighbor); 160 | } 161 | } 162 | 163 | z_mean = mean_sum / weight_sum; 164 | z_var = alpha_sum / weight_sum - z_mean * z_mean; 165 | } 166 | 167 | void RGBDFrame::computeDistributions( 168 | double max_z, 169 | double max_stdev_z) 170 | { 171 | double max_var_z = max_stdev_z * max_stdev_z; // maximum allowed z variance 172 | 173 | /// @todo These should be arguments or const static members 174 | double s_u = 1.0; // uncertainty in pixels 175 | double s_v = 1.0; // uncertainty in pixels 176 | 177 | n_valid_keypoints = 0; 178 | 179 | // center point 180 | double cx = intr.at(0, 2); 181 | double cy = intr.at(1, 2); 182 | 183 | // focus length 184 | double fx = intr.at(0, 0); 185 | double fy = intr.at(1, 1); 186 | 187 | // precompute for convenience 188 | double var_u = s_u * s_u; 189 | double var_v = s_v * s_v; 190 | double fx2 = fx*fx; 191 | double fy2 = fy*fy; 192 | 193 | // allocate space 194 | kp_valid.clear(); 195 | kp_means.clear(); 196 | kp_covariances.clear(); 197 | 198 | kp_valid.resize(keypoints.size()); 199 | kp_means.resize(keypoints.size()); 200 | kp_covariances.resize(keypoints.size()); 201 | 202 | for (unsigned int kp_idx = 0; kp_idx < keypoints.size(); ++kp_idx) 203 | { 204 | // calculate pixel coordinates 205 | double u = keypoints[kp_idx].pt.x; 206 | double v = keypoints[kp_idx].pt.y; 207 | 208 | // get raw z value 209 | uint16_t z_raw = depth_img.at((int)v, (int)u); 210 | 211 | // skip bad values 212 | if (z_raw == 0) 213 | { 214 | kp_valid[kp_idx] = false; 215 | continue; 216 | } 217 | 218 | // get z: mean and variance 219 | double z, var_z; 220 | //getGaussianDistribution(u, v, z, var_z); 221 | getGaussianMixtureDistribution(u, v, z, var_z); 222 | 223 | // skip bad values - too far away, or z-variance too big 224 | if (z > max_z || var_z > max_var_z) 225 | { 226 | kp_valid[kp_idx] = false; 227 | continue; 228 | } 229 | kp_valid[kp_idx] = true; 230 | n_valid_keypoints++; 231 | 232 | // precompute for convenience 233 | double z_2 = z * z; 234 | double umcx = u - cx; 235 | double vmcy = v - cy; 236 | 237 | // calculate x and y 238 | double x = z * umcx / fx; 239 | double y = z * vmcy / fy; 240 | 241 | // calculate covariances 242 | double s_xz = var_z * umcx / fx; 243 | double s_yz = var_z * vmcy / fy; 244 | 245 | double s_xx = (var_z * umcx * umcx + var_u * (z_2 + var_z))/fx2; 246 | double s_yy = (var_z * vmcy * vmcy + var_v * (z_2 + var_z))/fy2; 247 | 248 | double s_xy = umcx * vmcy * var_z / (fx * fy); 249 | double s_yx = s_xy; 250 | 251 | double s_zz = var_z; 252 | 253 | double s_zy = s_yz; 254 | double s_zx = s_xz; 255 | 256 | // fill out mean matrix 257 | Vector3f& kp_mean = kp_means[kp_idx]; 258 | 259 | kp_mean(0,0) = x; 260 | kp_mean(1,0) = y; 261 | kp_mean(2,0) = z; 262 | 263 | // fill out covariance matrix 264 | Matrix3f& kp_covariance = kp_covariances[kp_idx]; 265 | 266 | kp_covariance(0,0) = s_xx; // xx 267 | kp_covariance(0,1) = s_xy; // xy 268 | kp_covariance(0,2) = s_xz; // xz 269 | 270 | kp_covariance(1,0) = s_yx; // xy 271 | kp_covariance(1,1) = s_yy; // yy 272 | kp_covariance(1,2) = s_yz; // yz 273 | 274 | kp_covariance(2,0) = s_zx; // xz- 275 | kp_covariance(2,1) = s_zy; // yz 276 | kp_covariance(2,2) = s_zz; // zz 277 | } 278 | } 279 | 280 | void RGBDFrame::constructFeaturePointCloud( 281 | PointCloudFeature& cloud) 282 | { 283 | // filter invalid 284 | Vector3fVector means_f; 285 | removeInvalidMeans(kp_means, kp_valid, means_f); 286 | 287 | // create point cloud 288 | pointCloudFromMeans(means_f, cloud); 289 | 290 | //set the header 291 | cloud.header.frame_id = header.frame_id; 292 | cloud.header.seq = header.seq; 293 | 294 | // The point cloud stamp, in usec. 295 | cloud.header.stamp = header.stamp.sec * 1e6 + header.stamp.nsec * 1e-3; 296 | } 297 | 298 | void RGBDFrame::constructDensePointCloud( 299 | PointCloudT& cloud, 300 | double max_z, 301 | double max_stdev_z) const 302 | { 303 | double max_var_z = max_stdev_z * max_stdev_z; // maximum allowed z variance 304 | 305 | // center point 306 | double cx = intr.at(0, 2); 307 | double cy = intr.at(1, 2); 308 | 309 | // focus length 310 | double fx = intr.at(0, 0); 311 | double fy = intr.at(1, 1); 312 | 313 | // Scale by focal length for computing (X,Y) 314 | float constant_x = 1.0 / fx; 315 | float constant_y = 1.0 / fy; 316 | 317 | float bad_point = std::numeric_limits::quiet_NaN(); 318 | 319 | cloud.points.clear(); 320 | cloud.points.resize(rgb_img.rows * rgb_img.cols); 321 | for (int v = 0; v < rgb_img.rows; ++v) 322 | for (int u = 0; u < rgb_img.cols; ++u) 323 | { 324 | unsigned int index = v * rgb_img.cols + u; 325 | 326 | uint16_t z_raw = depth_img.at(v, u); 327 | 328 | float z = z_raw * 0.001; //convert to meters 329 | 330 | PointT& p = cloud.points[index]; 331 | 332 | double z_mean, z_var; 333 | 334 | // check for out of range or bad measurements 335 | if (z_raw != 0) 336 | { 337 | getGaussianMixtureDistribution(u, v, z_mean, z_var); 338 | 339 | // check for variance and z limits 340 | if (z_var < max_var_z && z_mean < max_z) 341 | { 342 | // fill in XYZ 343 | p.x = z * (u - cx) * constant_x; 344 | p.y = z * (v - cy) * constant_y; 345 | p.z = z; 346 | } 347 | else 348 | { 349 | p.x = p.y = p.z = bad_point; 350 | } 351 | } 352 | else 353 | { 354 | p.x = p.y = p.z = bad_point; 355 | } 356 | 357 | // fill out color 358 | const cv::Vec3b& color = rgb_img.at(v,u); 359 | p.r = color[2]; 360 | p.g = color[1]; 361 | p.b = color[0]; 362 | } 363 | 364 | // set cloud header 365 | cloud.header.frame_id = header.frame_id; 366 | // The point cloud stamp, in usec. 367 | cloud.header.stamp = header.stamp.sec * 1e6 + header.stamp.nsec * 1e-3; 368 | cloud.header.seq = header.seq; 369 | 370 | cloud.height = rgb_img.rows; 371 | cloud.width = rgb_img.cols; 372 | cloud.is_dense = false; 373 | } 374 | 375 | bool RGBDFrame::save( 376 | const RGBDFrame& frame, 377 | const std::string& path) 378 | { 379 | // set the filenames 380 | std::string rgb_filename = path + "/rgb.png"; 381 | std::string depth_filename = path + "/depth.png"; 382 | std::string header_filename = path + "/header.yml"; 383 | std::string intr_filename = path + "/intr.yml"; 384 | std::string cloud_filename = path + "/cloud.pcd"; 385 | 386 | // create the directory 387 | bool directory_result = boost::filesystem::create_directory(path); 388 | 389 | if (!directory_result) 390 | { 391 | std::cerr << "Could not create directory:" << path << std::endl; 392 | return false; 393 | } 394 | 395 | // save header 396 | cv::FileStorage fs_h(header_filename, cv::FileStorage::WRITE); 397 | fs_h << "frame_id" << frame.header.frame_id; 398 | fs_h << "seq" << (int)frame.header.seq; 399 | fs_h << "stamp_sec" << (int)frame.header.stamp.sec; 400 | fs_h << "stamp_nsec" << (int)frame.header.stamp.nsec; 401 | fs_h << "index" << frame.index; 402 | 403 | // save images 404 | cv::imwrite(rgb_filename, frame.rgb_img); 405 | cv::imwrite(depth_filename, frame.depth_img); 406 | 407 | // save intrinsic matrix 408 | cv::FileStorage fs_mat(intr_filename, cv::FileStorage::WRITE); 409 | fs_mat << "intr" << frame.intr; 410 | 411 | return true; 412 | } 413 | 414 | bool RGBDFrame::load(RGBDFrame& frame, const std::string& path) 415 | { 416 | // set the filenames 417 | std::string rgb_filename = path + "/rgb.png"; 418 | std::string depth_filename = path + "/depth.png"; 419 | std::string header_filename = path + "/header.yml"; 420 | std::string intr_filename = path + "/intr.yml"; 421 | 422 | // check if files exist 423 | if (!boost::filesystem::exists(rgb_filename) || 424 | !boost::filesystem::exists(depth_filename) || 425 | !boost::filesystem::exists(header_filename) || 426 | !boost::filesystem::exists(intr_filename) ) 427 | { 428 | std::cerr << "files for loading frame not found" << std::endl; 429 | return false; 430 | } 431 | 432 | // load header 433 | cv::FileStorage fs_h(header_filename, cv::FileStorage::READ); 434 | int seq, sec, nsec; 435 | 436 | fs_h["frame_id"] >> frame.header.frame_id; 437 | fs_h["seq"] >> seq; 438 | fs_h["stamp_sec"] >> sec; 439 | fs_h["stamp_nsec"] >> nsec; 440 | 441 | frame.header.seq = seq; 442 | frame.header.stamp.sec = sec; 443 | frame.header.stamp.nsec = nsec; 444 | 445 | fs_h["index"] >> frame.index; 446 | 447 | // load images 448 | frame.rgb_img = cv::imread(rgb_filename); 449 | frame.depth_img = cv::imread(depth_filename, -1); 450 | 451 | // load intrinsic matrix 452 | cv::FileStorage fs_mat(intr_filename, cv::FileStorage::READ); 453 | fs_mat["intr"] >> frame.intr; 454 | 455 | return true; 456 | } 457 | 458 | } // namespace rgbdtools 459 | -------------------------------------------------------------------------------- /src/rgbd_keyframe.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_keyframe.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/rgbd_keyframe.h" 25 | 26 | namespace rgbdtools { 27 | 28 | RGBDKeyframe::RGBDKeyframe(): 29 | manually_added(false) 30 | { 31 | 32 | } 33 | 34 | RGBDKeyframe::RGBDKeyframe(const RGBDFrame& frame): 35 | RGBDFrame(), 36 | manually_added(false) 37 | { 38 | // copies the data 39 | rgb_img = frame.rgb_img.clone(); 40 | depth_img = frame.depth_img.clone(); 41 | intr = frame.intr.clone(); 42 | header = frame.header; 43 | 44 | // this preserves the index number of the original frame 45 | index = frame.index; 46 | } 47 | 48 | bool RGBDKeyframe::save( 49 | const RGBDKeyframe& keyframe, 50 | const std::string& path) 51 | { 52 | std::string pose_filename = path + "/pose.yaml"; 53 | std::string prop_filename = path + "/properties.yaml"; 54 | 55 | // save frame 56 | bool save_frame_result = RGBDFrame::save(keyframe, path); 57 | if (!save_frame_result) return false; 58 | 59 | // save pose as OpenCV rmat and tvec 60 | cv::Mat rmat, tvec; 61 | eigenAffineToOpenCVRt(keyframe.pose, rmat, tvec); 62 | 63 | cv::FileStorage fs_m(pose_filename, cv::FileStorage::WRITE); 64 | fs_m << "rmat" << rmat; 65 | fs_m << "tvec" << tvec; 66 | 67 | // save other class members 68 | cv::FileStorage fs_p(prop_filename, cv::FileStorage::WRITE); 69 | fs_p << "manually_added" << keyframe.manually_added; 70 | fs_p << "path_length_linear" << keyframe.path_length_linear; 71 | fs_p << "path_length_angular" << keyframe.path_length_angular; 72 | 73 | return true; 74 | } 75 | 76 | bool RGBDKeyframe::load(RGBDKeyframe& keyframe, const std::string& path) 77 | { 78 | // load frame 79 | bool load_frame_result = RGBDFrame::load(keyframe, path); 80 | if (!load_frame_result) return false; 81 | 82 | // set up filenames 83 | std::string pose_filename = path + "/pose.yaml"; 84 | std::string prop_filename = path + "/properties.yaml"; 85 | 86 | // check if files exist 87 | if (!boost::filesystem::exists(pose_filename) || 88 | !boost::filesystem::exists(prop_filename) ) 89 | { 90 | std::cerr << "files for loading keyframe not found" <> rmat; 99 | fs_m["tvec"] >> tvec; 100 | 101 | openCVRtToEigenAffine(rmat, tvec, keyframe.pose); 102 | 103 | // load other class members 104 | cv::FileStorage fs_p(prop_filename, cv::FileStorage::READ); 105 | fs_p["manually_added"] >> keyframe.manually_added; 106 | fs_p["path_length_linear"] >> keyframe.path_length_linear; 107 | fs_p["path_length_angular"] >> keyframe.path_length_angular; 108 | 109 | return true; 110 | } 111 | 112 | bool saveKeyframes( 113 | const KeyframeVector& keyframes, 114 | const std::string& path) 115 | { 116 | for (unsigned int kf_idx = 0; kf_idx < keyframes.size(); ++kf_idx) 117 | { 118 | std::stringstream ss_idx; 119 | ss_idx << std::setw(4) << std::setfill('0') << kf_idx; 120 | 121 | std::string kf_path = path + "/" + ss_idx.str(); 122 | 123 | bool save_result = RGBDKeyframe::save(keyframes[kf_idx], kf_path); 124 | if (!save_result) return false; 125 | } 126 | 127 | return true; 128 | } 129 | 130 | bool loadKeyframes( 131 | KeyframeVector& keyframes, 132 | const std::string& path) 133 | { 134 | keyframes.clear(); 135 | 136 | int kf_idx = 0; 137 | 138 | while(true) 139 | { 140 | std::stringstream ss_idx; 141 | ss_idx << std::setw(4) << std::setfill('0') << kf_idx; 142 | 143 | std::string path_kf = path + "/" + ss_idx.str(); 144 | 145 | if (boost::filesystem::exists(path_kf)) 146 | { 147 | std::cout << "Loading " << path_kf << std::endl; 148 | RGBDKeyframe keyframe; 149 | bool result_load = RGBDKeyframe::load(keyframe, path_kf); 150 | if (result_load) keyframes.push_back(keyframe); 151 | else 152 | { 153 | std::cerr << "Error loading" << std::endl; 154 | return false; 155 | } 156 | } 157 | else return true; 158 | 159 | kf_idx++; 160 | } 161 | } 162 | 163 | } // namespace rgbdtools 164 | -------------------------------------------------------------------------------- /src/rgbd_util.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_util.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "rgbdtools/rgbd_util.h" 25 | 26 | namespace rgbdtools { 27 | 28 | void removeInvalidMeans( 29 | const Vector3fVector& means, 30 | const BoolVector& valid, 31 | Vector3fVector& means_f) 32 | { 33 | unsigned int size = valid.size(); 34 | for(unsigned int i = 0; i < size; ++i) 35 | { 36 | if (valid[i]) 37 | { 38 | const Vector3f& mean = means[i]; 39 | means_f.push_back(mean); 40 | } 41 | } 42 | } 43 | 44 | void removeInvalidDistributions( 45 | const Vector3fVector& means, 46 | const Matrix3fVector& covariances, 47 | const BoolVector& valid, 48 | Vector3fVector& means_f, 49 | Matrix3fVector& covariances_f) 50 | { 51 | unsigned int size = valid.size(); 52 | for(unsigned int i = 0; i < size; ++i) 53 | { 54 | if (valid[i]) 55 | { 56 | const Vector3f& mean = means[i]; 57 | const Matrix3f& cov = covariances[i]; 58 | 59 | means_f.push_back(mean); 60 | covariances_f.push_back(cov); 61 | } 62 | } 63 | } 64 | 65 | void pointCloudFromMeans( 66 | const Vector3fVector& means, 67 | PointCloudFeature& cloud) 68 | { 69 | unsigned int size = means.size(); 70 | cloud.points.resize(size); 71 | for(unsigned int i = 0; i < size; ++i) 72 | { 73 | const Vector3f& m = means[i]; 74 | PointFeature& p = cloud.points[i]; 75 | 76 | p.x = m(0,0); 77 | p.y = m(1,0); 78 | p.z = m(2,0); 79 | } 80 | 81 | cloud.height = 1; 82 | cloud.width = size; 83 | cloud.is_dense = true; 84 | } 85 | 86 | 87 | void buildPointCloud( 88 | const cv::Mat& depth_img_rect, 89 | const cv::Mat& intr_rect_ir, 90 | PointCloudT& cloud) 91 | { 92 | int w = depth_img_rect.cols; 93 | int h = depth_img_rect.rows; 94 | 95 | double cx = intr_rect_ir.at(0,2); 96 | double cy = intr_rect_ir.at(1,2); 97 | double fx_inv = 1.0 / intr_rect_ir.at(0,0); 98 | double fy_inv = 1.0 / intr_rect_ir.at(1,1); 99 | 100 | cloud.resize(w*h); 101 | 102 | for (int u = 0; u < w; ++u) 103 | for (int v = 0; v < h; ++v) 104 | { 105 | uint16_t z = depth_img_rect.at(v, u); 106 | PointT& pt = cloud.points[v*w + u]; 107 | 108 | if (z != 0) 109 | { 110 | double z_metric = z * 0.001; 111 | 112 | pt.x = z_metric * ((u - cx) * fx_inv); 113 | pt.y = z_metric * ((v - cy) * fy_inv); 114 | pt.z = z_metric; 115 | } 116 | else 117 | { 118 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); 119 | } 120 | } 121 | 122 | cloud.width = w; 123 | cloud.height = h; 124 | cloud.is_dense = true; 125 | } 126 | 127 | void buildPointCloud( 128 | const cv::Mat& depth_img_rect_reg, 129 | const cv::Mat& rgb_img_rect, 130 | const cv::Mat& intr_rect_rgb, 131 | PointCloudT& cloud) 132 | { 133 | int w = rgb_img_rect.cols; 134 | int h = rgb_img_rect.rows; 135 | 136 | double cx = intr_rect_rgb.at(0,2); 137 | double cy = intr_rect_rgb.at(1,2); 138 | double fx_inv = 1.0 / intr_rect_rgb.at(0,0); 139 | double fy_inv = 1.0 / intr_rect_rgb.at(1,1); 140 | 141 | cloud.resize(w*h); 142 | 143 | for (int u = 0; u < w; ++u) 144 | for (int v = 0; v < h; ++v) 145 | { 146 | uint16_t z = depth_img_rect_reg.at(v, u); 147 | const cv::Vec3b& c = rgb_img_rect.at(v, u); 148 | 149 | PointT& pt = cloud.points[v*w + u]; 150 | 151 | if (z != 0) 152 | { 153 | double z_metric = z * 0.001; 154 | 155 | pt.x = z_metric * ((u - cx) * fx_inv); 156 | pt.y = z_metric * ((v - cy) * fy_inv); 157 | pt.z = z_metric; 158 | 159 | pt.r = c[2]; 160 | pt.g = c[1]; 161 | pt.b = c[0]; 162 | } 163 | else 164 | { 165 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); 166 | } 167 | } 168 | 169 | cloud.width = w; 170 | cloud.height = h; 171 | cloud.is_dense = true; 172 | } 173 | 174 | void buildRegisteredDepthImage( 175 | const cv::Mat& intr_rect_ir, 176 | const cv::Mat& intr_rect_rgb, 177 | const cv::Mat& ir2rgb, 178 | const cv::Mat& depth_img_rect, 179 | cv::Mat& depth_img_rect_reg) 180 | { 181 | int w = depth_img_rect.cols; 182 | int h = depth_img_rect.rows; 183 | 184 | depth_img_rect_reg = cv::Mat::zeros(h, w, CV_16UC1); 185 | 186 | cv::Mat intr_rect_ir_inv = intr_rect_ir.inv(); 187 | 188 | // Eigen intr_rect_rgb (3x3) 189 | Eigen::Matrix intr_rect_rgb_eigen; 190 | for (int u = 0; u < 3; ++u) 191 | for (int v = 0; v < 3; ++v) 192 | intr_rect_rgb_eigen(v,u) = intr_rect_rgb.at(v, u); 193 | 194 | // Eigen rgb2ir_eigen (3x4) 195 | Eigen::Matrix ir2rgb_eigen; 196 | for (int u = 0; u < 4; ++u) 197 | for (int v = 0; v < 3; ++v) 198 | ir2rgb_eigen(v,u) = ir2rgb.at(v, u); 199 | 200 | // Eigen intr_rect_ir_inv (4x4) 201 | Eigen::Matrix4d intr_rect_ir_inv_eigen; 202 | for (int v = 0; v < 3; ++v) 203 | for (int u = 0; u < 3; ++u) 204 | intr_rect_ir_inv_eigen(v,u) = intr_rect_ir_inv.at(v,u); 205 | 206 | intr_rect_ir_inv_eigen(0, 3) = 0; 207 | intr_rect_ir_inv_eigen(1, 3) = 0; 208 | intr_rect_ir_inv_eigen(2, 3) = 0; 209 | intr_rect_ir_inv_eigen(3, 0) = 0; 210 | intr_rect_ir_inv_eigen(3, 1) = 0; 211 | intr_rect_ir_inv_eigen(3, 2) = 0; 212 | intr_rect_ir_inv_eigen(3, 3) = 1; 213 | 214 | // multiply into single (3x4) matrix 215 | Eigen::Matrix H_eigen = 216 | intr_rect_rgb_eigen * (ir2rgb_eigen * intr_rect_ir_inv_eigen); 217 | 218 | // *** reproject 219 | 220 | Eigen::Vector3d p_rgb; 221 | Eigen::Vector4d p_depth; 222 | 223 | for (int v = 0; v < h; ++v) 224 | for (int u = 0; u < w; ++u) 225 | { 226 | uint16_t z = depth_img_rect.at(v,u); 227 | 228 | if (z != 0) 229 | { 230 | p_depth(0,0) = u * z; 231 | p_depth(1,0) = v * z; 232 | p_depth(2,0) = z; 233 | p_depth(3,0) = 1.0; 234 | p_rgb = H_eigen * p_depth; 235 | 236 | double px = p_rgb(0,0); 237 | double py = p_rgb(1,0); 238 | double pz = p_rgb(2,0); 239 | 240 | int qu = (int)(px / pz); 241 | int qv = (int)(py / pz); 242 | 243 | // skip outside of image 244 | if (qu < 0 || qu >= w || qv < 0 || qv >= h) continue; 245 | 246 | uint16_t& val = depth_img_rect_reg.at(qv, qu); 247 | 248 | // z buffering 249 | if (val == 0 || val > pz) val = pz; 250 | } 251 | } 252 | } 253 | 254 | void depthImageFloatTo16bit( 255 | const cv::Mat& depth_image_in, 256 | cv::Mat& depth_image_out) 257 | { 258 | depth_image_in.convertTo(depth_image_out, CV_16UC1, 1000.0); 259 | } 260 | 261 | void eigenAffineToOpenCVRt( 262 | const AffineTransform& transform, 263 | cv::Mat& R, 264 | cv::Mat& t) 265 | { 266 | // extract translation 267 | t = cv::Mat(3, 1, CV_64F); 268 | t.at(0,0) = transform(0,3); 269 | t.at(1,0) = transform(1,3); 270 | t.at(2,0) = transform(2,3); 271 | 272 | // extract rotation 273 | R = cv::Mat(3, 3, CV_64F); 274 | for(int i = 0; i < 3; ++i) 275 | for(int j = 0; j < 3; ++j) 276 | R.at(j,i) = transform(j,i); 277 | } 278 | 279 | void openCVRtToEigenAffine( 280 | const cv::Mat& R, 281 | const cv::Mat& t, 282 | AffineTransform& transform) 283 | { 284 | // extract translation 285 | transform(0,3) = t.at(0,0); 286 | transform(1,3) = t.at(1,0); 287 | transform(2,3) = t.at(2,0); 288 | 289 | // extract rotation 290 | for(int i = 0; i < 3; ++i) 291 | for(int j = 0; j < 3; ++j) 292 | transform(j,i) = R.at(j,i); 293 | 294 | // last row 295 | transform(3,0) = 0.0; 296 | transform(3,1) = 0.0; 297 | transform(3,2) = 0.0; 298 | transform(3,3) = 1.0; 299 | } 300 | 301 | void eigenAffineToXYZRPY( 302 | const AffineTransform& transform, 303 | float& x, float& y, float& z, 304 | float& roll, float& pitch, float& yaw) 305 | { 306 | x = transform(0,3); 307 | y = transform(1,3); 308 | z = transform(2,3); 309 | 310 | roll = atan2f(transform(2,1), transform(2,2)); 311 | pitch = asinf(-transform(2,0)); 312 | yaw = atan2f(transform(1,0), transform(0,0)); 313 | } 314 | 315 | void XYZRPYToEigenAffine( 316 | float x, float y, float z, 317 | float roll, float pitch, float yaw, 318 | AffineTransform& t) 319 | { 320 | float A=cosf(yaw), B=sinf(yaw), C=cosf(pitch), D=sinf(pitch), 321 | E=cosf(roll), F=sinf(roll), DE=D*E, DF=D*F; 322 | t(0,0) = A*C; t(0,1) = A*DF - B*E; t(0,2) = B*F + A*DE; t(0,3) = x; 323 | t(1,0) = B*C; t(1,1) = A*E + B*DF; t(1,2) = B*DE - A*F; t(1,3) = y; 324 | t(2,0) = -D; t(2,1) = C*F; t(2,2) = C*E; t(2,3) = z; 325 | t(3,0) = 0; t(3,1) = 0; t(3,2) = 0; t(3,3) = 1; 326 | } 327 | 328 | void transformMeans( 329 | Vector3fVector& means, 330 | const AffineTransform& transform) 331 | { 332 | Matrix3f R = transform.rotation(); 333 | Vector3f t = transform.translation(); 334 | 335 | unsigned int size = means.size(); 336 | for(unsigned int i = 0; i < size; ++i) 337 | { 338 | Vector3f& m = means[i]; 339 | m = R * m + t; 340 | } 341 | } 342 | 343 | void transformDistributions( 344 | Vector3fVector& means, 345 | Matrix3fVector& covariances, 346 | const AffineTransform& transform) 347 | { 348 | Matrix3f R = transform.rotation(); 349 | Vector3f t = transform.translation(); 350 | Matrix3f R_T = R.transpose(); 351 | 352 | unsigned int size = means.size(); 353 | for(unsigned int i = 0; i < size; ++i) 354 | { 355 | Vector3f& m = means[i]; 356 | Matrix3f& c = covariances[i]; 357 | m = R * m + t; 358 | c = R * c * R_T; 359 | } 360 | } 361 | 362 | void getTfDifference( 363 | const AffineTransform& transform, 364 | double& dist, double& angle) 365 | { 366 | Matrix3f R = transform.rotation(); 367 | Vector3f t = transform.translation(); 368 | 369 | dist = sqrt(t(0,0)*t(0,0) + t(1,0)*t(1,0) + t(2,0)*t(2,0)); 370 | 371 | double trace = R(0,0) + R(1,1) + R(2,2); 372 | angle = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0))); 373 | } 374 | 375 | void unwarpDepthImage( 376 | cv::Mat& depth_img, 377 | const cv::Mat& coeff0, 378 | const cv::Mat& coeff1, 379 | const cv::Mat& coeff2, 380 | int fit_mode) 381 | { 382 | /* 383 | // NOTE: This is slightly faster (5ms vs 4ms) 384 | // BUT images need to be passed as CV_64FC1 or CV_32FC1 385 | // currently the conversion to float messes up 0 <-> nan values 386 | // Alternatively, coeff0 needs to be 0 387 | 388 | cv::Mat temp; 389 | depth_img.convertTo(temp, CV_64FC1); 390 | temp = coeff0 + temp.mul(coeff1 + temp.mul(coeff2)); 391 | temp.convertTo(depth_img, CV_16UC1); 392 | */ 393 | 394 | if (fit_mode == DEPTH_FIT_QUADRATIC) 395 | { 396 | for (int u = 0; u < depth_img.cols; ++u) 397 | for (int v = 0; v < depth_img.rows; ++v) 398 | { 399 | uint16_t d = depth_img.at(v, u); 400 | if (d != 0) 401 | { 402 | double c0 = coeff0.at(v, u); 403 | double c1 = coeff1.at(v, u); 404 | double c2 = coeff2.at(v, u); 405 | 406 | uint16_t res = (int)(c0 + d*(c1 + d*c2)); 407 | depth_img.at(v, u) = res; 408 | } 409 | } 410 | } 411 | else if(fit_mode == DEPTH_FIT_LINEAR) 412 | { 413 | for (int u = 0; u < depth_img.cols; ++u) 414 | for (int v = 0; v < depth_img.rows; ++v) 415 | { 416 | uint16_t d = depth_img.at(v, u); 417 | if (d != 0) 418 | { 419 | double c0 = coeff0.at(v, u); 420 | double c1 = coeff1.at(v, u); 421 | 422 | uint16_t res = (int)(c0 + d*c1); 423 | depth_img.at(v, u) = res; 424 | } 425 | } 426 | } 427 | else if (fit_mode == DEPTH_FIT_LINEAR_ZERO) 428 | { 429 | cv::Mat temp; 430 | depth_img.convertTo(temp, CV_64FC1); 431 | temp = temp.mul(coeff1); 432 | temp.convertTo(depth_img, CV_16UC1); 433 | } 434 | else if (fit_mode == DEPTH_FIT_QUADRATIC_ZERO) 435 | { 436 | cv::Mat temp; 437 | depth_img.convertTo(temp, CV_64FC1); 438 | temp = temp.mul(coeff1 + temp.mul(coeff2)); 439 | temp.convertTo(depth_img, CV_16UC1); 440 | } 441 | } 442 | 443 | void setRPY( 444 | float roll, float pitch, float yaw, 445 | Eigen::Matrix3f& m) 446 | { 447 | m = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) * 448 | Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) * 449 | Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 450 | } 451 | 452 | } // namespace rgbdtools 453 | --------------------------------------------------------------------------------