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

cuda updated

Browse Source
This commit is contained in:
g-spacewhale 2019-03-15 09:42:59 +00:00
parent ea93237912
commit f4eb906896
4 changed files with 35 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/msckf_vio/image_processor.h
View File

@ -312,7 +312,7 @@ private:
const std::vector<unsigned char>& markers, const std::vector<unsigned char>& markers,
std::vector<T>& refined_vec) { std::vector<T>& refined_vec) {
if (raw_vec.size() != markers.size()) { 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()); raw_vec.size(), markers.size());
} }
for (int i = 0; i < markers.size(); ++i) { for (int i = 0; i < markers.size(); ++i) {
@ -367,8 +367,8 @@ private:
boost::shared_ptr<GridFeatures> prev_features_ptr; boost::shared_ptr<GridFeatures> prev_features_ptr;
boost::shared_ptr<GridFeatures> curr_features_ptr; boost::shared_ptr<GridFeatures> curr_features_ptr;
cv::cuda::GpuMat curr_cam0_pyramid_gpu; cv::cuda::GpuMat cam0_curr_img;
cv::cuda::GpuMat curr_cam1_pyramid_gpu; cv::cuda::GpuMat cam1_curr_img;
cv::cuda::GpuMat cam0_points_gpu; cv::cuda::GpuMat cam0_points_gpu;
cv::cuda::GpuMat cam1_points_gpu; cv::cuda::GpuMat cam1_points_gpu;

4
include/msckf_vio/utils.h
View File

@ -22,8 +22,8 @@ namespace msckf_vio {
*/ */
namespace utils { namespace utils {
static void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec); void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec);
static void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec); void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec);
Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh, Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
const std::string &field); const std::string &field);

31
src/image_processor.cpp
View File

@ -223,7 +223,9 @@ void ImageProcessor::stereoCallback(
sensor_msgs::image_encodings::MONO8); sensor_msgs::image_encodings::MONO8);
// Build the image pyramids once since they're used at multiple places // 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. // Detect features in the first frame.
if (is_first_img) { if (is_first_img) {
@ -632,7 +634,30 @@ void ImageProcessor::stereoMatch(
cam1_distortion_model, cam1_distortion_coeffs); 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. // Track features using LK optical flow method.
/*
calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_, calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_,
cam0_points, cam1_points, cam0_points, cam1_points,
inlier_markers, noArray(), inlier_markers, noArray(),
@ -642,7 +667,7 @@ void ImageProcessor::stereoMatch(
processor_config.max_iteration, processor_config.max_iteration,
processor_config.track_precision), processor_config.track_precision),
cv::OPTFLOW_USE_INITIAL_FLOW); cv::OPTFLOW_USE_INITIAL_FLOW);
*/
// Mark those tracked points out of the image region // Mark those tracked points out of the image region
// as untracked. // as untracked.
for (int i = 0; i < cam1_points.size(); ++i) { for (int i = 0; i < cam1_points.size(); ++i) {
@ -1002,7 +1027,7 @@ void ImageProcessor::twoPointRansac(
// Check the size of input point size. // Check the size of input point size.
if (pts1.size() != pts2.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()); pts1.size(), pts2.size());
double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]); double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]);

4
src/utils.cpp
View File

@ -11,14 +11,14 @@
namespace msckf_vio { namespace msckf_vio {
namespace utils { namespace utils {
static void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec) void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec)
{ {
vec.resize(d_mat.cols); vec.resize(d_mat.cols);
cv::Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]); cv::Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]);
d_mat.download(mat); d_mat.download(mat);
} }
static void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec) void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec)
{ {
vec.resize(d_mat.cols); vec.resize(d_mat.cols);
cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]); cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);