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
Download Patch File Download Diff File Expand all files Collapse all files

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;