Skip to content

Commit

Permalink
Add gray version
Browse files Browse the repository at this point in the history
  • Loading branch information
@xuhao1
xuhao1 committed Sep 20, 2020
1 parent 0c99c2a commit 30725cf
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 56 deletions.
3 changes: 2 additions & 1 deletion vins_estimator/src/estimator/parameters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@ int FLOW_BACK;

int WARN_IMU_DURATION;
int PUB_FLATTEN;
int FLATTEN_COLOR;

std::string configPath;

Expand Down Expand Up @@ -156,7 +157,7 @@ void readParameters(std::string config_file)
IMAGE_FREQ = fsSettings["image_freq"];
WARN_IMU_DURATION = fsSettings["warn_imu_duration"];
PUB_FLATTEN = fsSettings["pub_flatten"];

FLATTEN_COLOR = fsSettings["flatten_color"];
USE_IMU = fsSettings["imu"];
printf("USE_IMU: %d\n", USE_IMU);
if(USE_IMU)
Expand Down
1 change: 1 addition & 0 deletions vins_estimator/src/estimator/parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,7 @@ extern double IMAGE_FREQ;

extern int WARN_IMU_DURATION;
extern int PUB_FLATTEN;
extern int FLATTEN_COLOR;

void readParameters(std::string config_file);

Expand Down
112 changes: 59 additions & 53 deletions vins_estimator/src/fisheyeNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,31 +68,34 @@ void FisheyeFlattenHandler::img_callback(double t, const cv::Mat & img1, const c

TicToc t_f;


if (USE_GPU) {
// is_color = true;
fisheye_up_imgs_cuda = fisheys_undists[0].undist_all_cuda(img1, is_color, mask_up);
fisheye_up_imgs_cuda_gray.clear();
fisheye_down_imgs_cuda_gray.clear();


fisheye_down_imgs_cuda = fisheys_undists[1].undist_all_cuda(img2, is_color, mask_down);

for (auto & img: fisheye_up_imgs_cuda) {
cv::cuda::GpuMat gray;
if(!img.empty()) {
cv::cuda::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
if (is_color) {
fisheye_up_imgs_cuda = fisheys_undists[0].undist_all_cuda(img1, true, mask_up);
fisheye_down_imgs_cuda = fisheys_undists[1].undist_all_cuda(img2, true, mask_down);
fisheye_up_imgs_cuda_gray.clear();
fisheye_down_imgs_cuda_gray.clear();


for (auto & img: fisheye_up_imgs_cuda) {
cv::cuda::GpuMat gray;
if(!img.empty()) {
cv::cuda::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
}
fisheye_up_imgs_cuda_gray.push_back(gray);
}
fisheye_up_imgs_cuda_gray.push_back(gray);
}

for (auto & img: fisheye_down_imgs_cuda) {
cv::cuda::GpuMat gray;
if(!img.empty()) {
cv::cuda::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
}
for (auto & img: fisheye_down_imgs_cuda) {
cv::cuda::GpuMat gray;
if(!img.empty()) {
cv::cuda::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
}

fisheye_down_imgs_cuda_gray.push_back(gray);
fisheye_down_imgs_cuda_gray.push_back(gray);
}
} else {
fisheye_up_imgs_cuda_gray = fisheys_undists[0].undist_all_cuda(img1, false, mask_up);
fisheye_down_imgs_cuda_gray = fisheys_undists[1].undist_all_cuda(img2, false, mask_down);
}

if (!is_blank_init) {
Expand All @@ -105,34 +108,35 @@ void FisheyeFlattenHandler::img_callback(double t, const cv::Mat & img1, const c
fisheye_cuda_buf_down_color.push(fisheye_down_imgs_cuda);

buf_lock.unlock();
} else {
return;
}


} else {
fisheys_undists[0].stereo_flatten(img1, img2, &fisheys_undists[1],
fisheye_up_imgs, fisheye_down_imgs, false,
enable_up_top, enable_rear_side, enable_down_top, enable_rear_side);

for (auto & img: fisheye_up_imgs_cuda) {
cv::Mat gray;
if(!img.empty()) {
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
if (is_color) {
fisheys_undists[0].stereo_flatten(img1, img2, &fisheys_undists[1],
fisheye_up_imgs, fisheye_down_imgs, true,
enable_up_top, enable_rear_side, enable_down_top, enable_rear_side);
fisheye_up_imgs_gray.clear();
fisheye_down_imgs_gray.clear();
for (auto & img: fisheye_up_imgs_cuda) {
cv::Mat gray;
if(!img.empty()) {
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
}
fisheye_up_imgs_gray.push_back(gray);
}
fisheye_up_imgs_gray.push_back(gray);
}

for (auto & img: fisheye_down_imgs_cuda) {
cv::Mat gray;
if(!img.empty()) {
cv::cuda::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
for (auto & img: fisheye_down_imgs_cuda) {
cv::Mat gray;
if(!img.empty()) {
cv::cuda::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
}
fisheye_down_imgs_gray.push_back(gray);
}

fisheye_down_imgs_gray.push_back(gray);
} else {
fisheys_undists[0].stereo_flatten(img1, img2, &fisheys_undists[1],
fisheye_up_imgs_gray, fisheye_down_imgs_gray, false,
enable_up_top, enable_rear_side, enable_down_top, enable_rear_side);
}


buf_lock.lock();
fisheye_buf_t.push(t);
fisheye_buf_up.push(fisheye_up_imgs_gray);
Expand All @@ -142,7 +146,6 @@ void FisheyeFlattenHandler::img_callback(double t, const cv::Mat & img1, const c
fisheye_buf_down_color.push(fisheye_down_imgs);

buf_lock.unlock();

}

double tf = t_f.toc();
Expand Down Expand Up @@ -189,22 +192,25 @@ double FisheyeFlattenHandler::pop_from_buffer(
buf_lock.lock();
if (fisheye_buf_t.size() > 0) {
auto t = fisheye_buf_t.front();
up_gray.getGpuMatVecRef() = fisheye_cuda_buf_up.front();
down_gray.getGpuMatVecRef() = fisheye_cuda_buf_down.front();

if(is_color) {
up_color.getGpuMatVecRef() = fisheye_cuda_buf_up_color.front();
down_color.getGpuMatVecRef() = fisheye_cuda_buf_down_color.front();
for (size_t i = 0; i < 5; i++) {
up_gray.assign(fisheye_buf_up.front());
down_gray.assign(fisheye_buf_down.front());

if(is_color) {
up_color.assign(fisheye_buf_up_color.front());
down_color.assign(fisheye_buf_down_color.front());
}
}


fisheye_buf_t.pop();
fisheye_cuda_buf_up.pop();
fisheye_cuda_buf_down.pop();
fisheye_buf_up.pop();
fisheye_buf_down.pop();

if(is_color) {
fisheye_cuda_buf_up_color.pop();
fisheye_cuda_buf_down_color.pop();
fisheye_buf_up_color.pop();
fisheye_buf_down_color.pop();
}

buf_lock.unlock();
Expand Down Expand Up @@ -513,9 +519,9 @@ void VinsNodeBaseClass::Init(ros::NodeHandle & n)
registerPub(n);

if (FISHEYE) {
fisheye_handler = new FisheyeFlattenHandler(n);
fisheye_handler = new FisheyeFlattenHandler(n, FLATTEN_COLOR);
}

//We use blank images to initialize cuda before every thing
if (USE_GPU) {
TicToc blank;
Expand Down
4 changes: 2 additions & 2 deletions vins_estimator/src/fisheyeNode.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,8 +106,8 @@ class VinsNodeBaseClass {
CvCudaImages cur_up_color_cuda, cur_down_color_cuda;
CvCudaImages cur_up_gray_cuda, cur_down_gray_cuda;

CvCudaImages cur_up_color, cur_down_color;
CvCudaImages cur_up_gray, cur_down_gray;
CvImages cur_up_color, cur_down_color;
CvImages cur_up_gray, cur_down_gray;

double cur_frame_t;

Expand Down

0 comments on commit 30725cf

Please sign in to comment.