diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h index 236be1a..1004086 100644 --- a/include/msckf_vio/image_processor.h +++ b/include/msckf_vio/image_processor.h @@ -312,7 +312,7 @@ private: const std::vector& markers, std::vector& refined_vec) { if (raw_vec.size() != markers.size()) { - ROS_WARN("The input size of raw_vec(%lu) and markers(%lu) does not match...", + ROS_WARN("The input size of raw_vec(%i) and markers(%i) does not match...", raw_vec.size(), markers.size()); } for (int i = 0; i < markers.size(); ++i) { @@ -367,8 +367,8 @@ private: boost::shared_ptr prev_features_ptr; boost::shared_ptr curr_features_ptr; - cv::cuda::GpuMat curr_cam0_pyramid_gpu; - cv::cuda::GpuMat curr_cam1_pyramid_gpu; + cv::cuda::GpuMat cam0_curr_img; + cv::cuda::GpuMat cam1_curr_img; cv::cuda::GpuMat cam0_points_gpu; cv::cuda::GpuMat cam1_points_gpu; diff --git a/include/msckf_vio/utils.h b/include/msckf_vio/utils.h index 0f2a0c6..cf0d39a 100644 --- a/include/msckf_vio/utils.h +++ b/include/msckf_vio/utils.h @@ -22,8 +22,8 @@ namespace msckf_vio { */ namespace utils { -static void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); -static void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); +void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); +void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh, const std::string &field); diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 28363c5..ba220b1 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -223,7 +223,9 @@ void ImageProcessor::stereoCallback( sensor_msgs::image_encodings::MONO8); // Build the image pyramids once since they're used at multiple places - createImagePyramids(); + + // removed due to alternate cuda construct + //createImagePyramids(); // Detect features in the first frame. if (is_first_img) { @@ -632,7 +634,30 @@ void ImageProcessor::stereoMatch( cam1_distortion_model, cam1_distortion_coeffs); } + auto d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create( + Size(processor_config.patch_size, processor_config.patch_size), + processor_config.pyramid_levels, + processor_config.max_iteration, + true); + + cam0_curr_img = cv::cuda::GpuMat(cam0_curr_img_ptr->image); + cam1_curr_img = cv::cuda::GpuMat(cam1_curr_img_ptr->image); + cam0_points_gpu = cv::cuda::GpuMat(cam0_points); + cam1_points_gpu = cv::cuda::GpuMat(cam1_points); + + cv::cuda::GpuMat inlier_markers_gpu; + d_pyrLK_sparse->calc(cam0_curr_img, + cam1_curr_img, + cam0_points_gpu, + cam1_points_gpu, + inlier_markers_gpu, + noArray()); + + utils::download(cam1_points_gpu, cam1_points); + utils::download(inlier_markers_gpu, inlier_markers); + // Track features using LK optical flow method. + /* calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_, cam0_points, cam1_points, inlier_markers, noArray(), @@ -642,7 +667,7 @@ void ImageProcessor::stereoMatch( processor_config.max_iteration, processor_config.track_precision), cv::OPTFLOW_USE_INITIAL_FLOW); - + */ // Mark those tracked points out of the image region // as untracked. for (int i = 0; i < cam1_points.size(); ++i) { @@ -1002,7 +1027,7 @@ void ImageProcessor::twoPointRansac( // Check the size of input point size. if (pts1.size() != pts2.size()) - ROS_ERROR("Sets of different size (%lu and %lu) are used...", + ROS_ERROR("Sets of different size (%i and %i) are used...", pts1.size(), pts2.size()); double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]); diff --git a/src/utils.cpp b/src/utils.cpp index 1545566..830d84f 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -11,14 +11,14 @@ namespace msckf_vio { namespace utils { -static void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) +void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) { vec.resize(d_mat.cols); cv::Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]); d_mat.download(mat); } -static void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) +void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) { vec.resize(d_mat.cols); cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);