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

correct frame and add log file for depth-filter

Browse Source
This commit is contained in:
gfuse
2021-09-04 11:32:59 +02:00
parent ea18c52f05
commit e754978e3e
9 changed files with 73 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/depth_filter.h
+1
View File
@@ -150,6 +150,7 @@ protected:
double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds.
double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds.
Matcher matcher_;
FILE* log_;
//uint seeds_cont_=0;
/// Initialize new seeds from a frame.
GPU_version/vio/include/vio/global.h
+4 -3
View File
@@ -95,10 +95,11 @@ namespace vio
SE3 se3() const{
assert(T2_!= nullptr);
//Todo add 15 roll orientation
//Camera frame z front, x right, y down -> right hands
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
* AngleAxisd(pitch(), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
q = AngleAxisd(0.261799, Vector3d::UnitX()) // roll
* AngleAxisd(pitch(), Vector3d::UnitY())// pitch
* AngleAxisd(0.0, Vector3d::UnitZ()); //yaw
SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
return tem;
}
GPU_version/vio/include/vio/ukf.h
+8 -6
View File
@@ -111,29 +111,31 @@ public:
virtual ~UKF() {
delete filter_;
};
void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,const ros::Time& time) {
void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
while(lock)usleep(5);
lock=true;
if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(y)<1.0)y=pow(y,3);//picked up from your code
if(filter_->predict_up)filter_->correct(x,y,pitch,1e-9*time.toNSec());
if(filter_->predict_up)filter_->correct(x,y,theta,1e-9*time.toNSec());
lock=false;
};
void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,const ros::Time& time) {
//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) {
while(lock)usleep(5);
lock=true;
filter_->predict(x,y,pitch,1e-9*time.toNSec());
filter_->predict(x,y,theta,1e-9*time.toNSec());
lock=false;
};
//Camera frame z front, x right, y down -> right hands (pitch counts from z)
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*/,size_t match,ros::Time& time) {
while(lock)usleep(5);
lock=true;
//if(filter_->predict_up)filter_->correct(z,x,pitch,match,1e-9*time.toNSec());
lock=false;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),M_PI-filter_->state_(2)));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-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_(1),-filter_->state_(0),M_PI-filter_->state_(2)));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-filter_->state_(2)));
}
private:
Base* filter_= nullptr;
GPU_version/vio/launch/debug.launch
+21
View File
@@ -0,0 +1,21 @@
<launch>
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" launch-prefix="xterm -e gdb --args"/>
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
<param name="vio/imu_topic" value="/SensorNode/imu" type="str" />
<param name="vio/cmd_topic" value="/cmd_vel" type="str" />
<param name="vio/start" value="/vio/start" type="str" />
<param name="vio/stop" value="/vio/stop" type="str" />
<param name="vio/odom" value="/vio/getOdom" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio)/param/px30.yaml" />
<!-- /node -->
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio)/param/vo_fast.yaml" />
</launch>
GPU_version/vio/launch/live.launch
-16
View File
@@ -1,16 +0,0 @@
<launch>
<node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/camera/image_raw" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find svo_ros)/param/camera_atan.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
</node>
</launch>
GPU_version/vio/src/depth_filter.cpp
+26 -8
View File
@@ -28,6 +28,8 @@
#include <vio/matcher.h>
#include <vio/config.h>
#include <vio/feature_detection.h>
#include <sys/types.h>
#include <sys/stat.h>
namespace vio {
@@ -53,7 +55,11 @@ namespace vio {
new_keyframe_set_(false),
new_keyframe_min_depth_(0.0),
new_keyframe_mean_depth_(0.0)
{}
{
log_ =fopen("/root/Project/log.txt","w+");
assert(log_);
chmod("/root/Project/log.txt", ACCESSPERMS);
}
DepthFilter::~DepthFilter()
{
@@ -64,6 +70,7 @@ namespace vio {
void DepthFilter::startThread()
{
thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this);
fprintf(log_,"init\n");
}
void DepthFilter::stopThread()
@@ -78,15 +85,17 @@ namespace vio {
thread_->join();
thread_ = NULL;
}
fclose(log_);
}
void DepthFilter::addFrame(FramePtr frame)
{
fprintf(log_,"add new frame\n");
if(thread_ != NULL)
{
{
lock_t lock(frame_queue_mut_);
if(frame_queue_.size() > 2)
if(frame_queue_.empty())lock_t lock(frame_queue_mut_);
if(frame_queue_.size() > 10)
frame_queue_.pop();
frame_queue_.push(frame);
}
@@ -102,6 +111,7 @@ namespace vio {
{
new_keyframe_min_depth_ = depth_min;
new_keyframe_mean_depth_ = depth_mean;
fprintf(log_,"add key frame\n");
if(thread_ != NULL)
{
@@ -116,13 +126,14 @@ namespace vio {
void DepthFilter::initializeSeeds(FramePtr frame)
{
Features new_features;
fprintf(log_,"init seeds\n");
feature_detector_->setExistingFeatures(frame->fts_);
feature_detector_->detect(frame.get(), frame->img_pyr_,
Config::triangMinCornerScore(), new_features);
// initialize a seed for every new feature
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
seeds_mut_.lock();
++Seed::batch_counter;
std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
@@ -131,12 +142,14 @@ namespace vio {
if(options_.verbose)
SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
seeds_updating_halt_ = false;
seeds_mut_.unlock();
}
void DepthFilter::removeKeyframe(FramePtr frame)
{
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_);
fprintf(log_,"remove key frame\n");
seeds_mut_.lock();
std::list<Seed>::iterator it=seeds_.begin();
size_t n_removed = 0;
while(it!=seeds_.end())
@@ -150,16 +163,17 @@ namespace vio {
++it;
}
seeds_updating_halt_ = false;
seeds_mut_.unlock();
}
void DepthFilter::reset()
{
seeds_updating_halt_ = true;
{
lock_t lock(seeds_mut_);
seeds_mut_.lock();
seeds_.clear();
seeds_mut_.unlock();
}
lock_t lock();
while(!frame_queue_.empty())
frame_queue_.pop();
seeds_updating_halt_ = false;
@@ -172,11 +186,14 @@ namespace vio {
{
while(!boost::this_thread::interruption_requested())
{
fprintf(log_,"loop 182\n");
FramePtr frame;
{
lock_t lock(frame_queue_mut_);
while(frame_queue_.empty() && new_keyframe_set_ == false)
frame_queue_cond_.wait(lock);
lock.unlock();
if(new_keyframe_set_)
{
new_keyframe_set_ = false;
@@ -185,7 +202,7 @@ namespace vio {
}
else
{
frame = frame_queue_.front();
frame = frame_queue_.back();
frame_queue_.pop();
}
}
@@ -203,6 +220,7 @@ namespace vio {
lock_t lock(seeds_mut_);
std::list<Seed>::iterator it=seeds_.begin();
fprintf(log_,"update seed\n");
const double focal_length = frame->cam_->errorMultiplier2();
double px_noise = 1.0;
double px_error_angle = atan2(px_noise,(2.0*focal_length))*2.0; // law of chord (sehnensatz)
GPU_version/vio/src/frame_handler_mono.cpp
+2 -1
View File
@@ -191,6 +191,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_);
new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first;
depth_filter_->addFrame(new_frame_);
if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
return RESULT_FAILURE;
}
@@ -209,7 +210,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited
{
depth_filter_->addFrame(new_frame_);
//depth_filter_->addFrame(new_frame_);
return RESULT_NO_KEYFRAME;
}
new_frame_->setKeyframe();
GPU_version/vio/src/visualizer.cpp
+7 -4
View File
@@ -52,11 +52,14 @@ void Visualizer::publishMinimal(
{
// publish cam in world frame (Estimated odometry in the worls frame)
auto odom=ukf.get_location();
Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ()));
//Rviz frame z up, x forward, y left -> right hands (yaw counts from x)
//Camera frame z front, x right, y down -> right hands (pitch counts from z)
//IMU frame y front, x right, z up -> left hands (theta counts from y)
Quaterniond q(AngleAxisd(-odom.second.pitch(), Vector3d::UnitZ()));
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose_with_cov->header = header_msg;
msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.y = -odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.z = 0.0;
msg_pose_with_cov->pose.pose.orientation.x = q.x();
msg_pose_with_cov->pose.pose.orientation.y = q.y();
@@ -71,7 +74,7 @@ void Visualizer::publishMinimal(
pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
publishPointMarker(
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(0),odom.second.se2().translation()(1),0.0), "trajectory",
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),-odom.second.se2().translation()(0),0.0), "trajectory",
ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
}
}
GPU_version/vio/src/vo_node.cpp
+4 -1
View File
@@ -115,6 +115,7 @@ VioNode::VioNode() :
VioNode::~VioNode()
{
vo_->depthFilter()->stopThread();
start_=false;
usleep(5000);
imu_the_->join();
@@ -187,9 +188,11 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) {
res.header.frame_id = "/world";
res.header.seq = trace_id_;
auto odom=vo_->ukfPtr_.get_location();
//Camera frame z front, x right, y down -> right hands (pitch counts from z)
//IMU frame y front, x right, z up -> left hands (theta counts from y)
res.x=odom.second.se2().translation()(0);
res.y=odom.second.se2().translation()(1);
res.yaw=odom.second.pitch();
res.yaw=-odom.second.pitch();
res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2),
odom.first(1,0),odom.first(1,1),odom.first(1,2),
odom.first(2,0),odom.first(2,1),odom.first(2,2)};