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

ignore craupted image

Browse Source
This commit is contained in:
gfuse
2021-08-19 17:40:47 +02:00
parent 49ad990444
commit 5827af8bcb
3 changed files with 12 additions and 4 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
@@ -164,7 +164,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//assert (false); //assert (false);
reprojector_.reprojectMap(new_frame_, overlap_kfs_); reprojector_.reprojectMap(new_frame_, overlap_kfs_);
/* /*
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_ <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<< <<"\t distance between two frames: "<<
GPU_version/vio/src/initialization.cpp
+4 -3
View File
@@ -43,7 +43,7 @@ InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
{ {
trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_); trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
if(disparities_.size() < Config::initMinTracked()) if(disparities_.size() < 1)
return NO_KEYFRAME; return NO_KEYFRAME;
double disparity = vk::getMedian(disparities_); double disparity = vk::getMedian(disparities_);
if(disparity < Config::initMinDisparity()){ if(disparity < Config::initMinDisparity()){
@@ -134,6 +134,7 @@ void trackKlt(
vector<Vector3d>& f_cur, vector<Vector3d>& f_cur,
vector<double>& disparities) vector<double>& disparities)
{ {
disparities.clear();
const double klt_win_size = 30.0;//30.0 const double klt_win_size = 30.0;//30.0
const int klt_max_iter = 30;//30 const int klt_max_iter = 30;//30
const double klt_eps = 0.001; const double klt_eps = 0.001;
@@ -141,17 +142,17 @@ void trackKlt(
vector<float> error; vector<float> error;
vector<float> min_eig_vec; vector<float> min_eig_vec;
cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps); cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);
if(frame_ref->img_pyr_[0].empty() || frame_cur->img_pyr_[0].empty())return;
cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0], cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
px_ref, px_cur, px_ref, px_cur,
status, error, status, error,
cv::Size2i(klt_win_size, klt_win_size), cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW); 4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);
vector<cv::Point2f>::iterator px_ref_it = px_ref.begin(); vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
vector<cv::Point2f>::iterator px_cur_it = px_cur.begin(); vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
vector<Vector3d>::iterator f_ref_it = f_ref.begin(); vector<Vector3d>::iterator f_ref_it = f_ref.begin();
f_cur.clear(); f_cur.reserve(px_cur.size()); f_cur.clear(); f_cur.reserve(px_cur.size());
disparities.clear(); disparities.reserve(px_cur.size()); disparities.reserve(px_cur.size());
for(size_t i=0; px_ref_it != px_ref.end(); ++i) for(size_t i=0; px_ref_it != px_ref.end(); ++i)
{ {
if(!status[i]) if(!status[i])
GPU_version/vio/src/vo_node.cpp
+7
View File
@@ -90,6 +90,8 @@ private:
double* imu_; double* imu_;
double* cmd_; double* cmd_;
ros::Time imu_time_; ros::Time imu_time_;
int width;
int height;
boost::thread* imu_the_= nullptr; boost::thread* imu_the_= nullptr;
#if VIO_DEBUG #if VIO_DEBUG
vio::Visualizer visualizer_; vio::Visualizer visualizer_;
@@ -100,6 +102,8 @@ VioNode::VioNode() :
vo_(NULL), vo_(NULL),
cam_(NULL) cam_(NULL)
{ {
width=vk::getParam<int>("vio/cam_width", 640);
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)); 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_))
@@ -126,6 +130,9 @@ 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())
return;
if(cv::countNonZero(img) < 0.85*height*width)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());