public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add laplacian to ignore blur frame

Browse Source
This commit is contained in:
gfuse
2021-08-20 11:39:53 +02:00
parent dcb321175f
commit b73bd992db
2 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/src/frame_handler_mono.cpp
+1 -1
View File
@@ -227,7 +227,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// add keyframe to map // add keyframe to map
map_.addKeyframe(new_frame_); map_.addKeyframe(new_frame_);
std::cout<<"Svo pipline was finished\n"; //std::cout<<"Svo pipline was finished\n";
return RESULT_IS_KEYFRAME; return RESULT_IS_KEYFRAME;
} }
GPU_version/vio/src/vo_node.cpp
+6 -6
View File
@@ -88,7 +88,6 @@ public:
}; };
private: private:
double* imu_; double* imu_;
double* cmd_;
ros::Time imu_time_; ros::Time imu_time_;
int width; int width;
int height; int height;
@@ -105,7 +104,6 @@ VioNode::VioNode() :
width=vk::getParam<int>("vio/cam_width", 640); width=vk::getParam<int>("vio/cam_width", 640);
height=vk::getParam<int>("vio/cam_height", 480); height=vk::getParam<int>("vio/cam_height", 480);
imu_=(double *)calloc(static_cast<std::size_t>(3), sizeof(double)); imu_=(double *)calloc(static_cast<std::size_t>(3), sizeof(double));
cmd_=(double *)calloc(static_cast<std::size_t>(3), sizeof(double));
if(!vk::camera_loader::loadFromRosNs("vio", cam_)) if(!vk::camera_loader::loadFromRosNs("vio", cam_))
throw std::runtime_error("Camera model not correctly specified."); throw std::runtime_error("Camera model not correctly specified.");
Eigen::Matrix<double,3,1> init; Eigen::Matrix<double,3,1> init;
@@ -130,9 +128,12 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
try { try {
auto start=std::chrono::steady_clock::now(); auto start=std::chrono::steady_clock::now();
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
if(img.empty()) if(img.empty())return;
return; cv::Mat imgbul;
if(cv::countNonZero(img) < 0.85*height*width)return; cv::Laplacian(img,imgbul,CV_64F);
cv::Scalar mean, stddev; // 0:1st channel, 1:2nd channel and 2:3rd channel
meanStdDev(imgbul, mean, stddev, cv::Mat());
if(stddev.val[0] * stddev.val[0]< 50.0)return;
vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp); vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp);
} catch (cv_bridge::Exception& e) { } catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what()); ROS_ERROR("cv_bridge exception: %s", e.what());
@@ -164,7 +165,6 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) { void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
if(!start_)return; if(!start_)return;
double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z}; double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};
memcpy(cmd_, _cmd, static_cast<std::size_t>(3*sizeof(double)));
vo_->UpdateCmd(_cmd,imu_time_); vo_->UpdateCmd(_cmd,imu_time_);
} }
void VioNode::imu_th(){ void VioNode::imu_th(){