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

add M_PI_2 to go from 2d to 3d

Browse Source
This commit is contained in:
gfuse
2021-09-04 15:10:44 +02:00
parent e754978e3e
commit 99a5d45a6f
10 changed files with 65 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+2
View File
@@ -74,5 +74,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17)
set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for)
ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")
ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")
ADD_DEFINITIONS(-DVIO_DEBUG=true)
GPU_version/vio/include/vio/depth_filter.h
+3 -2
View File
@@ -20,7 +20,6 @@
#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
//#include <vikit/performance_monitor.h>
#include <vio/global.h>
#include <vio/feature_detection.h>
#include <vio/matcher.h>
@@ -150,7 +149,9 @@ 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_;
#if VIO_DEBUG
FILE* log_=nullptr;
#endif
//uint seeds_cont_=0;
/// Initialize new seeds from a frame.
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -132,7 +132,7 @@ namespace vio {
SE2 inv=T_f_w_.inverse();
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real())-M_PI_2, Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1)));
return out;
GPU_version/vio/include/vio/frame_handler_mono.h
+4 -3
View File
@@ -47,7 +47,7 @@ public:
FramePtr lastFrame() { return last_frame_; }
/// Get the set of spatially closest keyframes of the last frame.
const set<FramePtr>& coreKeyframes() { return core_kfs_; }
const set<FramePtr>& coreKeyframes() { /*return core_kfs_;*/ }
/// Return the feature track to visualize the KLT tracking during initialization.
const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; }
@@ -62,7 +62,6 @@ public:
const SE3& T_kf_f,
const cv::Mat& img,
const double timestamp);
//std::shared_ptr<Imu_Integration> imu_integPtr_; //Integration of IMU
void UpdateIMU(double* value,const ros::Time& time);
void UpdateCmd(double* value,const ros::Time& time);
UKF ukfPtr_;
@@ -72,12 +71,14 @@ protected:
Reprojector reprojector_; //!< Projects points from other keyframes into the current frame
FramePtr new_frame_; //!< Current frame.
FramePtr last_frame_; //!< Last frame, not necessarily a keyframe.
set<FramePtr> core_kfs_; //!< Keyframes in the closer neighbourhood.
vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
opencl* gpu_fast_;
ros::Time time_;
#if VIO_DEBUG
FILE* log_=nullptr;
#endif
/// Initialize the visual odometry algorithm.
GPU_version/vio/include/vio/global.h
+2 -1
View File
@@ -68,6 +68,7 @@ namespace vio
}
SE2_5(SE3& se3){
Quaterniond q=se3.unit_quaternion().normalized();
//Camera frame z front, x right, y down -> right hands
auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
T2_=new SE2(SO2(euler(1)),Vector2d(se3.translation().x(),se3.translation().z()));
assert(T2_!= nullptr);
@@ -98,7 +99,7 @@ namespace vio
//Camera frame z front, x right, y down -> right hands
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX()) // roll
* AngleAxisd(pitch(), Vector3d::UnitY())// pitch
* AngleAxisd(pitch()-M_PI_2, 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
+3 -3
View File
@@ -126,16 +126,16 @@ public:
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)
//Camera frame z front, x right, y down -> right hands (pitch counts from x)
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_(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),filter_->state_(1),-1.0*(filter_->state_(2)+M_PI_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),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),filter_->state_(1),-1.0*(filter_->state_(2)+M_PI_2)));
}
private:
Base* filter_= nullptr;
GPU_version/vio/kernel/compute-residual.cl
+1 -1
View File
@@ -25,7 +25,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
float3 error=ref+cur;
float yaw=0.0;
float pitch=error.z;
float pitch=error.z-1.57079632679489661923;
float roll=0.26;
float R00=cos(yaw)*cos(pitch);//cos(pitch)
float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
GPU_version/vio/src/depth_filter.cpp
+35 -14
View File
@@ -28,9 +28,10 @@
#include <vio/matcher.h>
#include <vio/config.h>
#include <vio/feature_detection.h>
#if VIO_DEBUG
#include <sys/types.h>
#include <sys/stat.h>
#endif
namespace vio {
int Seed::batch_counter = 0;
@@ -56,41 +57,55 @@ namespace vio {
new_keyframe_min_depth_(0.0),
new_keyframe_mean_depth_(0.0)
{
#if VIO_DEBUG
log_ =fopen("/root/Project/log.txt","w+");
assert(log_);
chmod("/root/Project/log.txt", ACCESSPERMS);
#endif
}
DepthFilter::~DepthFilter()
{
stopThread();
SVO_INFO_STREAM("DepthFilter destructed.");
#if VIO_DEBUG
fprintf(log_,"DepthFilter destructed.\n");
#endif
}
void DepthFilter::startThread()
{
thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this);
fprintf(log_,"init\n");
#if VIO_DEBUG
fprintf(log_,"init depth filter\n");
#endif
}
void DepthFilter::stopThread()
{
SVO_INFO_STREAM("DepthFilter stop thread invoked.");
#if VIO_DEBUG
fprintf(log_,"DepthFilter stop thread invoked.");
#endif
if(thread_ != NULL)
{
SVO_INFO_STREAM("DepthFilter interrupt and join thread... ");
#if VIO_DEBUG
fprintf(log_,"DepthFilter interrupt and join thread ");
#endif
seeds_updating_halt_ = true;
thread_->interrupt();
usleep(5000);
thread_->join();
thread_ = NULL;
}
#if VIO_DEBUG
fclose(log_);
#endif
}
void DepthFilter::addFrame(FramePtr frame)
{
#if VIO_DEBUG
fprintf(log_,"add new frame\n");
#endif
if(thread_ != NULL)
{
{
@@ -111,7 +126,9 @@ namespace vio {
{
new_keyframe_min_depth_ = depth_min;
new_keyframe_mean_depth_ = depth_mean;
#if VIO_DEBUG
fprintf(log_,"add key frame\n");
#endif
if(thread_ != NULL)
{
@@ -126,7 +143,9 @@ namespace vio {
void DepthFilter::initializeSeeds(FramePtr frame)
{
Features new_features;
#if VIO_DEBUG
fprintf(log_,"init seeds\n");
#endif
feature_detector_->setExistingFeatures(frame->fts_);
feature_detector_->detect(frame.get(), frame->img_pyr_,
Config::triangMinCornerScore(), new_features);
@@ -139,8 +158,9 @@ namespace vio {
seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
});
if(options_.verbose)
SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
#if VIO_DEBUG
fprintf(log_,"DepthFilter: Initialized %d new seeds\n",new_features.size());
#endif
seeds_updating_halt_ = false;
seeds_mut_.unlock();
}
@@ -148,7 +168,9 @@ namespace vio {
void DepthFilter::removeKeyframe(FramePtr frame)
{
seeds_updating_halt_ = true;
#if VIO_DEBUG
fprintf(log_,"remove key frame\n");
#endif
seeds_mut_.lock();
std::list<Seed>::iterator it=seeds_.begin();
size_t n_removed = 0;
@@ -178,16 +200,15 @@ namespace vio {
frame_queue_.pop();
seeds_updating_halt_ = false;
if(options_.verbose)
SVO_INFO_STREAM("DepthFilter: RESET.");
#if VIO_DEBUG
fprintf(log_,"DepthFilter: RESET.\n");
#endif
}
void DepthFilter::updateSeedsLoop()
{
while(!boost::this_thread::interruption_requested())
{
fprintf(log_,"loop 182\n");
FramePtr frame;
{
lock_t lock(frame_queue_mut_);
@@ -219,8 +240,9 @@ namespace vio {
size_t n_updates=0, n_failed_matches=0;
lock_t lock(seeds_mut_);
std::list<Seed>::iterator it=seeds_.begin();
#if VIO_DEBUG
fprintf(log_,"update seed\n");
#endif
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)
@@ -239,7 +261,7 @@ namespace vio {
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real())-M_PI_2, Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
@@ -298,7 +320,6 @@ namespace vio {
}
else if(isnan(z_min))
{
SVO_WARN_STREAM("z_min is NaN");
it = seeds_.erase(it);
}
else
GPU_version/vio/src/frame_handler_mono.cpp
+12 -14
View File
@@ -90,7 +90,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
}
// some cleanup from last iteration, can't do before because of visualization
core_kfs_.clear();
//core_kfs_.clear();
overlap_kfs_.clear();
// create new frame
@@ -170,13 +170,13 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
if(!img_align.run(last_frame_, new_frame_))new_frame_->T_f_w_=init_f.second;
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
//assert (false);
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
std::cerr<<"Reprojection Map nPoint: "<<n_point
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<
"\t angle between two frames:"<<last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch()<<'\n';
if( reprojector_.n_trials_ < 10)return RESULT_FAILURE;
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
@@ -184,33 +184,32 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){
fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI_4){
return RESULT_FAILURE;
}
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
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;
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
depth_filter_->addFrame(new_frame_);
if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()/*10*/){
return RESULT_FAILURE;
}
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
// select keyframe
core_kfs_.insert(new_frame_);
//core_kfs_.insert(new_frame_);
setTrackingQuality(sfba_n_edges_final);
if(tracking_quality_ == TRACKING_INSUFFICIENT)
if(tracking_quality_ == TRACKING_INSUFFICIENT || tracking_quality_ == TRACKING_BAD)
{
return RESULT_FAILURE;
return RESULT_NO_KEYFRAME;
}
double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited
if(!needNewKf(depth_mean))//edited
{
//depth_filter_->addFrame(new_frame_);
return RESULT_NO_KEYFRAME;
}
new_frame_->setKeyframe();
@@ -234,7 +233,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// add keyframe to map
map_.addKeyframe(new_frame_);
//std::cout<<"Svo pipline was finished\n";
return RESULT_IS_KEYFRAME;
}
@@ -268,7 +266,7 @@ void FrameHandlerMono::resetAll()
resetCommon();
last_frame_.reset();
new_frame_.reset();
core_kfs_.clear();
//core_kfs_.clear();
overlap_kfs_.clear();
depth_filter_->reset();
}
@@ -300,7 +298,7 @@ void FrameHandlerMono::setCoreKfs(size_t n_closest)
std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(),
boost::bind(&pair<FramePtr, size_t>::second, _1) >
boost::bind(&pair<FramePtr, size_t>::second, _2));
std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair<FramePtr,size_t>& i){ core_kfs_.insert(i.first); });
//std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair<FramePtr,size_t>& i){ core_kfs_.insert(i.first); });
}
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){
if(value== nullptr)return;
GPU_version/vio/src/vo_node.cpp
+2 -16
View File
@@ -1,19 +1,5 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#define VIO_DEBUG true
// 0.1 m/s linear velocity
// 0.2 radian/s angular velocity
#include <ros/ros.h>
#include <string>
#include <chrono>