raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added branch

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-03 06:51:18 +00:00
parent f4eb906896
commit c6b8a2c2fc
2 changed files with 30 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/msckf_vio/image_processor.h
View File

@ -367,6 +367,8 @@ private:
boost::shared_ptr<GridFeatures> prev_features_ptr;
boost::shared_ptr<GridFeatures> curr_features_ptr;
cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse;
cv::cuda::GpuMat cam0_curr_img;
cv::cuda::GpuMat cam1_curr_img;
cv::cuda::GpuMat cam0_points_gpu;

35
src/image_processor.cpp
View File

@ -204,6 +204,13 @@ bool ImageProcessor::initialize() {
detector_ptr = FastFeatureDetector::create(
processor_config.fast_threshold);
//create gpu optical flow lk
d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create(
Size(processor_config.patch_size, processor_config.patch_size),
processor_config.pyramid_levels,
processor_config.max_iteration,
true);
if (!createRosIO()) return false;
ROS_INFO("Finish creating ROS IO...");
@ -464,7 +471,8 @@ void ImageProcessor::trackFeatures() {
predictFeatureTracking(prev_cam0_points,
cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
//TODO: change to GPU
//TODO: test change to sparse
/*
calcOpticalFlowPyrLK(
prev_cam0_pyramid_, curr_cam0_pyramid_,
prev_cam0_points, curr_cam0_points,
@ -475,6 +483,25 @@ void ImageProcessor::trackFeatures() {
processor_config.max_iteration,
processor_config.track_precision),
cv::OPTFLOW_USE_INITIAL_FLOW);
*/
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(prev_cam0_points);
cam1_points_gpu = cv::cuda::GpuMat(curr_cam0_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, curr_cam0_points);
utils::download(inlier_markers_gpu, track_inliers);
// Mark those tracked points out of the image region
// as untracked.
@ -634,12 +661,6 @@ 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);