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

change feature score in fast algorithm and add brightness

Browse Source
This commit is contained in:
gfuse
2021-09-16 19:12:24 +02:00
parent 887487b4fc
commit 1e685aa829
14 changed files with 3586 additions and 10380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+4 -8525
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+3515 -1807
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/feature_detection.h
+1 -1
View File
@@ -104,7 +104,7 @@ public:
std::shared_ptr<Frame> frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts,
list<shared_ptr<Feature>>& fts,
cv::Mat* descriptors= nullptr);
opencl* gpu_fast_;
};
GPU_version/vio/include/vio/ukf.h
+3 -3
View File
@@ -132,12 +132,12 @@ public:
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
while(lock)usleep(5);
lock=true;
filter_->correct(x,-1.0*z,pitch);
filter_->correct(10.0*x,-1.0*z,-1.0*pitch);
lock=false;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
};
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
}
bool start_=false;
private:
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/out.txt
BIN
View File
Binary file not shown.
GPU_version/vio/param/vo_fast.yaml
+5 -4
View File
@@ -10,7 +10,8 @@ svo:
kfselect_mindist: 0.01
poseoptim_thresh: 8.0
init_min_inliers: 30
ACC_noise: 100 #acc white noise in continuous in EKF
GYO_noise: 100 #gyro white noise in continuous in EKF
svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 100 #control vector noise in EKF
map_scale: 0.5
ACC_noise: 2000 #acc white noise in continuous in EKF
GYO_noise: 2000 #gyro white noise in continuous in EKF
svo_ekf: 0.0001 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 2000 #control vector noise in EKF
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -269,7 +269,7 @@ namespace vio {
#if VIO_DEBUG
fprintf(log_,"[%s] If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
#endif
if(xyz_f.z() > 0.0) {
if(xyz_f.z() < 0.0) {
++it; // behind the camera
continue;
}
GPU_version/vio/src/feature_detection.cpp
+18 -18
View File
@@ -69,51 +69,51 @@ void FastDetector::detect(
std::shared_ptr<Frame> frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts,
list<shared_ptr<Feature>>& fts,
cv::Mat* descriptors){
Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f));
std::vector<cv::KeyPoint> keypoints;
fts.clear();
for(int L=0; L<n_pyr_levels_; ++L)
{
const int scale = (1<<L);
cv::Mat img=img_pyr.at(L);
if(!gpu_fast_->reload_buf(0,0,img)){
std::cerr<<"Failed to write into GPU goodbye :)"<<'\n';
ROS_ERROR("Failed to write into GPU goodbye :)");
exit(0);
};
cl_int icorner[1]={0};
if(!gpu_fast_->reload_buf(0,2,icorner)){
std::cerr<<"Failed to write into GPU goodbye :)"<<'\n';
ROS_ERROR("Failed to write into GPU goodbye :)");
exit(0);
};
gpu_fast_->run(0,img.cols,img.rows);
int count[1]={0};
cl_int count[1]={0};
gpu_fast_->read(0,2,1,count);
cl_int2 fast_corners[350000]={0};
gpu_fast_->read(0,1,count[0],fast_corners);
if(count[0]<1 ){
ROS_ERROR("Can not communicate with GPU");
exit(0);
}
for(uint i=0;i<count[0];++i)
{
if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0)
if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0){
continue;
}
const int k = static_cast<int>((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_
+ static_cast<int>((fast_corners[i].x*scale)/cell_size_);
if(grid_occupancy_[k] || k > corners.size())
continue;
if(grid_occupancy_[k] || k > corners.size()){
continue;
}
const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
if(score > corners.at(k).score){
corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f);
// Create feature for every corner that has high enough corner score
if(corners.at(k).score > detection_threshold){
fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y)*scale, corners.at(k).level));
if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 1));
}
corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y)*scale, corners.at(k).level));
if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 1));
}
}
}
if(descriptors){
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
GPU_version/vio/src/frame_handler_mono.cpp
+6 -4
View File
@@ -131,8 +131,9 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{
ukfPtr_.start_=true;
if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE)
return RESULT_NO_KEYFRAME;
if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE){
return RESULT_NO_KEYFRAME;
}
new_frame_->setKeyframe();
map_.reset();
map_.addKeyframe(new_frame_);
@@ -224,7 +225,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2){
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2 ||
sfba_n_edges_final < Config::qualityMinFts()/*10*/){
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
@@ -242,7 +244,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
#endif
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
depth_filter_->addFrame(new_frame_);
if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()/*10*/){
if(sfba_n_edges_final < Config::qualityMinFts()){
return RESULT_FAILURE;
}
GPU_version/vio/src/initialization.cpp
+12
View File
@@ -28,9 +28,12 @@ namespace initialization {
InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
{
reset();
features_ref_.clear();
px_ref_.clear();
detectFeatures(frame_ref, px_ref_, features_ref_, gpu_fast_);
if(px_ref_.size() < 100)
{
ROS_WARN("Process first frame. Detected observations are px=%d, features=%d less than 100",px_ref_.size(),features_ref_.size());
return FAILURE;
}
#if VIO_DEBUG
@@ -80,6 +83,15 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
return NO_KEYFRAME;
}
frame_cur->T_f_w_ = T_cur_from_ref_;
// Rescale the map such that the mean scene depth is equal to the specified scale
vector<double> depth_vec;
for(size_t i=0; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
-frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()))));
// For each inlier create 3D point and add feature in both frames
for(auto&& id:inliers_){
Features::iterator first=features_ref_.begin();
GPU_version/vio/src/pose_optimizer.cpp
+2 -10
View File
@@ -102,10 +102,7 @@ void optimizeGaussNewton(
// check if error increased
if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
{
//#if VIO_DEBUG
// fprintf(log,"[%s] it:%d \t FAILURE \t new_chi2: %f\n",
// vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2);
//#endif
frame->T_f_w_ = T_old; // roll-back
break;
}
@@ -113,12 +110,7 @@ void optimizeGaussNewton(
// update the model
T_old = frame->T_f_w_;
frame->T_f_w_=SE2_5(T_old.se2().translation().x()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z());
chi2 = new_chi2;
//#if VIO_DEBUG
// fprintf(log,"[%s] it:%d \t Success \t new_chi2: %f \t dT = %f, %f, %f\n",
// vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2,dT.x(),dT.y(),dT.z());
//#endif
// stop when converged
if(vk::norm_max(dT) <= EPS)
@@ -148,7 +140,7 @@ void optimizeGaussNewton(
error_init /=chi2_vec_init.size();
if(!chi2_vec_final.empty())for(auto&& i:chi2_vec_final)error_final+=i;
error_final /= chi2_vec_final.size();
fprintf(log,"[%s] n deleted obs = %d \t n obs felt =%d \t error init =%f \t error end=%f\n",
fprintf(log,"[%s] n deleted obs = %d \t n obs with reprojection error less than 0.5 px =%d \t error init =%f \t error end=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,num_obs,error_init,error_final);
#endif
}
GPU_version/vio/src/reprojector.cpp
+2
View File
@@ -112,6 +112,7 @@ namespace vio {
const int k = static_cast<int>((*point)->px.y() / grid_.cell_size) *
grid_.grid_n_cols
+ static_cast<int>((*point)->px.x() / grid_.cell_size);
if(k>grid_.cells.size()-1)continue;
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) (*point)->px.x(),
(int) (*point)->px.y());
@@ -129,6 +130,7 @@ namespace vio {
const int k = static_cast<int>((*point)->px.y()/ grid_.cell_size) *
grid_.grid_n_cols
+ static_cast<int>((*point)->px.x() / grid_.cell_size);
if(k>grid_.cells.size()-1)continue;
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) (*point)->px.x(),
(int) (*point)->px.y());
GPU_version/vio/src/vo_node.cpp
+16 -6
View File
@@ -119,12 +119,22 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
try {
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
if(img.empty())return;
cv::Mat imgbul;//tem;
cv::Laplacian(img,imgbul,CV_64F);
cv::Mat imgbul,float_img,frame,show;//tem;
img.convertTo(float_img,CV_64F,1.f/255);
float_img*=2.0;
float_img+=0.2;
float_img.convertTo(frame,CV_8UC1,255);
cv::Laplacian(frame,imgbul,CV_64F);
cv::Scalar mean, stddev;
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);
cv::cvtColor(frame,show,CV_GRAY2RGB);
imshow("New Image", show);
cv::waitKey(10);
if(stddev.val[0] * stddev.val[0]< 30.0){
ROS_WARN("Frame is blur or too dark");
return;
}
vo_->addImage(frame, msg->header.stamp.toSec(),msg->header.stamp);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("vo exception: %s", e.what());
}
@@ -152,7 +162,7 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
auto odom=vo_->ukfPtr_.get_location();
fprintf(vo_->log_,"[%s] Odometry x=%f, y=%f, theta=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
odom.second.se2().translation()(0),odom.second.se2().translation()(1),
-odom.second.pitch());
odom.second.pitch());
visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
#endif
}
@@ -170,7 +180,7 @@ void VioNode::imu_th(){
while(start_ && !boost::this_thread::interruption_requested())
{
Q.callOne(ros::WallDuration(10,0.0));
usleep(20000);
usleep(10000);
}
}