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

track features in all direction but acuraccy is not good

Browse Source
This commit is contained in:
gfuse
2021-09-17 20:31:13 +02:00
parent c374f21714
commit 082e971d80
11 changed files with 8900 additions and 3879 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+7526 -400
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+1313 -3427
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/math_utils.h
+14
View File
@@ -129,6 +129,20 @@ T getMedian(vector<T>& data_vec)
nth_element(data_vec.begin(), it, data_vec.end());
return *it;
}
template<class T>
T getMean(vector<T>& data_vec,T& min=0,T& max=0)
{
assert(!data_vec.empty());
T sum=0;
min=std::numeric_limits<T>::max();
max=std::numeric_limits<T>::min();
for(auto&& i:data_vec){
sum+=i;
if(i<min)min=i;
if(i>max)max=i;
}
return sum/data_vec.size();
}
inline double pyrFromZero_d(double x_0, int level)
{
GPU_version/vio/include/vio/ukf.h
+14 -17
View File
@@ -6,8 +6,10 @@
#include <iostream>
#include <fstream>
#include "vio/config.h"
#include <vio/config.h>
#include <Eigen/Dense>
#include <boost/thread.hpp>
#include <boost/function.hpp>
class Base{
public:
Base(const Eigen::Matrix<double, 3, 1> &init) {
@@ -112,36 +114,31 @@ public:
};
void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
if(!start_)return;
while(lock)usleep(5);
lock=true;
x=pow(x,3);//picked up from your code
y=pow(y,3);//picked up from your code
/*x=pow(x,3);//picked up from your code
y=pow(y,3);//picked up from your code*/
boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(x,y,theta,1e-9*time.toNSec());
lock=false;
};
//IMU frame y front, x right, z up -> left hands (theta counts from y)
void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
if(!start_)return;
while(lock)usleep(5);
lock=true;
boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->predict(x,y,theta,1e-9*time.toNSec());
lock=false;
};
//Camera frame z front, x right, y down -> right hands (pitch counts from x)correct
//Camera frame z back, x right, y down -> right hands (pitch counts from x)correct
//Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
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(10.0*x,-10.0*z,pitch);
lock=false;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),(filter_->state_(2))));
boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(10.0*x,-10.0*z,-1.0*pitch);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*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(0.1*filter_->state_(0),-0.1*filter_->state_(1),(filter_->state_(2))));
boost::unique_lock<boost::mutex> lock(ekf_mut_);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
}
bool start_=false;
private:
Base* filter_= nullptr;
bool lock=false;
boost::mutex ekf_mut_;
};
#endif //VIO_UKF_H
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -10,7 +10,7 @@ svo:
kfselect_mindist: 0.01
poseoptim_thresh: 8.0
init_min_inliers: 30
ACC_noise: 200000 #0.5 #acc white noise in continuous in EKF
GYO_noise: 200000 #0.5 #gyro white noise in continuous in EKF
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.5 #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: 2000000 #0.5 #control vector noise in EKF
Cmd_Cov: 0.5 #control vector noise in EKF
GPU_version/vio/src/depth_filter.cpp
+3 -3
View File
@@ -281,12 +281,12 @@ namespace vio {
float z_min = (*it)->mu + sqrt((*it)->sigma2);
float z_max = max((*it)->mu - sqrt((*it)->sigma2), 0.00000001f);
#if VIO_DEBUG
fprintf(log_,"[%s] Z min: %f, Z max: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max);
fprintf(log_,"[%s] Z inverse min: %f, Z inverse max: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max);
#endif
if(z_min < 0){
/* if(z_min < 0){
++it;
continue;
}
}*/
double z;
if(!matcher_.findEpipolarMatchDirect(
GPU_version/vio/src/frame.cpp
+9 -5
View File
@@ -180,17 +180,21 @@ bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
{
if((*it)->point != NULL)
{
const double z = frame.w2f((*it)->point->pos_).z();
depth_vec.push_back(z);
depth_min = fmin(z, depth_min);
double z = frame.w2f((*it)->point->pos_).z();
if(z>0 && z<20.0){
depth_vec.push_back(z);
depth_min = fmin(z, depth_min);
}else{
(*it)->point.reset();
}
}
}
if(depth_vec.empty())
{
//SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
return false;
}
depth_mean = vk::getMedian(depth_vec);
double min,max;
depth_mean = vk::getMean(depth_vec,min,max);
return true;
}
GPU_version/vio/src/frame_handler_mono.cpp
+3 -3
View File
@@ -225,7 +225,7 @@ 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()){
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
@@ -243,9 +243,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
#endif
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
depth_filter_->addFrame(new_frame_);
if(sfba_n_edges_final < Config::qualityMinFts()){
/* if(sfba_n_edges_final < Config::qualityMinFts()){
return RESULT_FAILURE;
}
}*/
// select keyframe
//core_kfs_.insert(new_frame_);
GPU_version/vio/src/initialization.cpp
+2 -1
View File
@@ -54,7 +54,8 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
ukf_->UpdateSvo(0.0,0.0,0.0);
return NO_KEYFRAME;
}
double disparity = vk::getMedian(disparities_);
double min,max;
double disparity = vk::getMean(disparities_,min,max);
if(disparity < Config::initMinDisparity()){
auto result=ukf_->get_location();
if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0);
GPU_version/vio/src/robust_cost.cpp
+2 -1
View File
@@ -70,7 +70,8 @@ float MADScaleEstimator::
compute(std::vector<float>& errors) const
{
// error must be in absolute values!
return NORMALIZER * vk::getMedian(errors);
float min,max;
return NORMALIZER * vk::getMean(errors,min,max);
}
float NormalDistributionScaleEstimator::
GPU_version/vio/src/vo_node.cpp
+11 -19
View File
@@ -142,27 +142,13 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
if(!start_)return;
double imu_in[3];
if(abs(imu->linear_acceleration.x)>5.0){
return;
}else{
imu_in[0]=imu->linear_acceleration.x;
};
if(abs(imu->linear_acceleration.y)>5.0){
return;
}else{
imu_in[1]=imu->linear_acceleration.y;
};
imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x;
imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y;
imu_in[0] = imu->linear_acceleration.x;
imu_in[1] = imu->linear_acceleration.y;
imu_in[2] = imu->angular_velocity.z;
memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp;
#if VIO_DEBUG
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());
visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
#endif
}
@@ -170,17 +156,23 @@ void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
if(!start_)return;
double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};
vo_->UpdateCmd(_cmd,imu_time_);
#if VIO_DEBUG
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(),
10.0*odom.second.se2().translation()(0),10.0*odom.second.se2().translation()(1),
odom.second.pitch());
#endif
}
void VioNode::imu_th(){
ros::NodeHandle nh;
ros::CallbackQueue Q;
nh.setCallbackQueue(&Q);
ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this,ros::TransportHints().tcpNoDelay());
ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),10,&VioNode::cmdCb, this,ros::TransportHints().tcpNoDelay());
ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),1,&VioNode::imuCb, this,ros::TransportHints().tcpNoDelay());
ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),1,&VioNode::cmdCb, this,ros::TransportHints().tcpNoDelay());
while(start_ && !boost::this_thread::interruption_requested())
{
Q.callOne(ros::WallDuration(10,0.0));
usleep(10000);
usleep(5000);
}
}