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

added moving window structure, not yet done. added timestame sync for images and features detected

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-10 18:36:11 +02:00
parent e6620a4ed4
commit 79cce26dad
8 changed files with 174 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
src/image_processor.cpp
View File

@ -170,10 +170,6 @@ bool ImageProcessor::loadParameters() {
processor_config.ransac_threshold);
ROS_INFO("stereo_threshold: %f",
processor_config.stereo_threshold);
ROS_INFO("OpenCV Major Version: %d",
CV_MAJOR_VERSION);
ROS_INFO("OpenCV Minor Version: %d",
CV_MINOR_VERSION);
ROS_INFO("===========================================");
return true;
}
@ -223,9 +219,7 @@ void ImageProcessor::stereoCallback(
sensor_msgs::image_encodings::MONO8);
// Build the image pyramids once since they're used at multiple places
// removed due to alternate cuda construct
//createImagePyramids();
createImagePyramids();
// Detect features in the first frame.
if (is_first_img) {
@ -302,7 +296,6 @@ void ImageProcessor::imuCallback(
void ImageProcessor::createImagePyramids() {
const Mat& curr_cam0_img = cam0_curr_img_ptr->image;
// TODO: build cuda optical flow
buildOpticalFlowPyramid(
curr_cam0_img, curr_cam0_pyramid_,
Size(processor_config.patch_size, processor_config.patch_size),
@ -310,7 +303,6 @@ void ImageProcessor::createImagePyramids() {
BORDER_CONSTANT, false);
const Mat& curr_cam1_img = cam1_curr_img_ptr->image;
// TODO: build cuda optical flow
buildOpticalFlowPyramid(
curr_cam1_img, curr_cam1_pyramid_,
Size(processor_config.patch_size, processor_config.patch_size),
@ -464,7 +456,6 @@ void ImageProcessor::trackFeatures() {
predictFeatureTracking(prev_cam0_points,
cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
//TODO: change to GPU
calcOpticalFlowPyrLK(
prev_cam0_pyramid_, curr_cam0_pyramid_,
prev_cam0_points, curr_cam0_points,
@ -634,30 +625,7 @@ 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(),
@ -667,7 +635,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) {
@ -1027,7 +995,7 @@ void ImageProcessor::twoPointRansac(
// Check the size of input point size.
if (pts1.size() != pts2.size())
ROS_ERROR("Sets of different size (%i and %i) are used...",
ROS_ERROR("Sets of different size (%lu and %lu) are used...",
pts1.size(), pts2.size());
double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]);