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

has error but feature is tracking

Browse Source
This commit is contained in:
gfuse
2021-09-16 01:13:28 +02:00
parent 76d5c7b926
commit 887487b4fc
6 changed files with 10344 additions and 38144 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+8525 -37554
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+1807 -582
View File
File diff suppressed because it is too large Load Diff
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*/) { 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); while(lock)usleep(5);
lock=true; lock=true;
filter_->correct(-1.0*x,-1.0*z,pitch); filter_->correct(x,-1.0*z,pitch);
lock=false; lock=false;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-1.0*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(filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
}; };
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){ 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(-1.0*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(filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
} }
bool start_=false; bool start_=false;
private: private:
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch> <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 --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -10,7 +10,7 @@ svo:
kfselect_mindist: 0.01 kfselect_mindist: 0.01
poseoptim_thresh: 8.0 poseoptim_thresh: 8.0
init_min_inliers: 30 init_min_inliers: 30
ACC_noise: 0.5 #acc white noise in continuous in EKF ACC_noise: 100 #acc white noise in continuous in EKF
GYO_noise: 0.5 #gyro 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 svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.1 #control vector noise in EKF Cmd_Cov: 100 #control vector noise in EKF
GPU_version/vio/src/vo_node.cpp
+5 -1
View File
@@ -149,6 +149,10 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
vo_->UpdateIMU(imu_in,imu->header.stamp); vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp; imu_time_=imu->header.stamp;
#if VIO_DEBUG #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()); visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
#endif #endif
} }
@@ -166,7 +170,7 @@ void VioNode::imu_th(){
while(start_ && !boost::this_thread::interruption_requested()) while(start_ && !boost::this_thread::interruption_requested())
{ {
Q.callOne(ros::WallDuration(10,0.0)); Q.callOne(ros::WallDuration(10,0.0));
usleep(1000); usleep(20000);
} }
} }