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

initial commit

Browse Source
This commit is contained in:
g-spacewhale 2019-03-15 07:47:29 +00:00
parent 3c8e446f76
commit ea93237912
5 changed files with 38 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -29,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(OpenCV 3.1 EXACT REQUIRED)
find_package(SuiteSparse REQUIRED)
##################

9
include/msckf_vio/image_processor.h
View File

@ -14,6 +14,10 @@
#include <opencv2/opencv.hpp>
#include <opencv2/video.hpp>
#include <opencv2/cudaoptflow.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudaarithm.hpp>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
@ -363,6 +367,11 @@ private:
boost::shared_ptr<GridFeatures> prev_features_ptr;
boost::shared_ptr<GridFeatures> curr_features_ptr;
cv::cuda::GpuMat curr_cam0_pyramid_gpu;
cv::cuda::GpuMat curr_cam1_pyramid_gpu;
cv::cuda::GpuMat cam0_points_gpu;
cv::cuda::GpuMat cam1_points_gpu;
// Number of features after each outlier removal step.
int before_tracking;
int after_tracking;

7
include/msckf_vio/utils.h
View File

@ -11,6 +11,9 @@
#include <ros/ros.h>
#include <string>
#include <opencv2/core/core.hpp>
#include <opencv2/cudaoptflow.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudaarithm.hpp>
#include <Eigen/Geometry>
namespace msckf_vio {
@ -18,6 +21,10 @@ namespace msckf_vio {
* @brief utilities for msckf_vio
*/
namespace utils {
static 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);
Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
const std::string &field);

7
src/image_processor.cpp
View File

@ -170,6 +170,10 @@ 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;
}
@ -296,6 +300,7 @@ 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),
@ -303,6 +308,7 @@ 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),
@ -456,6 +462,7 @@ 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,

14
src/utils.cpp
View File

@ -11,6 +11,20 @@
namespace msckf_vio {
namespace utils {
static void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& 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<uchar>& vec)
{
vec.resize(d_mat.cols);
cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);
d_mat.download(mat);
}
Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
const std::string &field) {
Eigen::Isometry3d T;