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

Edit EKF and VO pipline to publish the odometry as soon as algorithm initialized. And VO will not save those frames that do not have features.

Browse Source
This commit is contained in:
gfuse
2021-07-10 13:35:58 +02:00
parent 3b110b299b
commit 99fa000ad5
20 changed files with 134 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/gpu_svo/frame.h
+5 -22
View File
@@ -93,11 +93,7 @@ public:
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2c(const Vector3d& xyz_w) const { inline Vector2d w2c(const Vector3d& xyz_w) const {
Eigen::Matrix<double,3,3> R; return cam_->world2cam( T_f_w_.se3()*xyz_w);
R<<T_f_w_.T2_.rotation_matrix()(0,0),1e-19,T_f_w_.T2_.rotation_matrix()(0,1),
1e-19,1.0,1e-19,
T_f_w_.T2_.rotation_matrix()(1,0),1e-19,T_f_w_.T2_.rotation_matrix()(1,1);
return cam_->world2cam( SE3(R,Vector3d(T_f_w_.T2_.translation()(0),1e-19,T_f_w_.T2_.translation()(1)))*xyz_w);
} }
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); } inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); }
@@ -110,35 +106,22 @@ public:
/// Transforms point coordinates in world-frame (w) to camera-frams (f). /// Transforms point coordinates in world-frame (w) to camera-frams (f).
inline Vector3d w2f(const Vector3d& xyz_w) const { inline Vector3d w2f(const Vector3d& xyz_w) const {
Eigen::Matrix<double,3,3> R; return Vector3d(T_f_w_.se3()*xyz_w);
R<<T_f_w_.T2_.rotation_matrix()(0,0),1e-19,T_f_w_.T2_.rotation_matrix()(0,1),
1e-19,1.0,1e-19,
T_f_w_.T2_.rotation_matrix()(1,0),1e-19,T_f_w_.T2_.rotation_matrix()(1,1);
return Vector3d(SE3(R,Vector3d(T_f_w_.T2_.translation()(0),1e-19,T_f_w_.T2_.translation()(1)))*xyz_w);
} }
/// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
inline Vector3d f2w(const Vector3d& f) const { inline Vector3d f2w(const Vector3d& f) const {
Eigen::Matrix<double,3,3> R; return getSE3Inv() * f;
R<<T_f_w_.T2_.rotation_matrix()(0,0),1e-19,T_f_w_.T2_.rotation_matrix()(0,1),
1e-19,1.0,1e-19,
T_f_w_.T2_.rotation_matrix()(1,0),1e-19,T_f_w_.T2_.rotation_matrix()(1,1);
return SE3(R,Vector3d(T_f_w_.T2_.translation()(0),1e-19,T_f_w_.T2_.translation()(1))).inverse() * f;
} }
/// Projects Point from unit sphere (f) in camera pixels (c). /// Projects Point from unit sphere (f) in camera pixels (c).
inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); } inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
/// Return the pose of the frame in the (w)orld coordinate frame. /// Return the pose of the frame in the (w)orld coordinate frame.
inline Vector2d pos() const { return T_f_w_.T2_.translation(); } inline Vector2d pos() const { return T_f_w_.se2().translation(); }
inline SE3 se3() const{ inline SE3 se3() const{
Eigen::Matrix<double,3,3> R; return T_f_w_.se3();
R<<T_f_w_.getSE2().rotation_matrix()(0,0),1e-19,T_f_w_.getSE2().rotation_matrix()(0,1),
1e-19,1.0,1e-19,
T_f_w_.getSE2().rotation_matrix()(1,0),1e-19,T_f_w_.getSE2().rotation_matrix()(1,1);
SE3 out(R,Vector3d(T_f_w_.getSE2().translation()(0),1e-19,T_f_w_.getSE2().translation()(1)));
return out;
} }
inline SE3 getSE3Inv() const{ inline SE3 getSE3Inv() const{
GPU_version/vio/include/gpu_svo/frame_handler_mono.h
+1 -1
View File
@@ -64,6 +64,7 @@ public:
//std::shared_ptr<Imu_Integration> imu_integPtr_; //Integration of IMU //std::shared_ptr<Imu_Integration> imu_integPtr_; //Integration of IMU
void UpdateIMU(double* value,const ros::Time& time); void UpdateIMU(double* value,const ros::Time& time);
void UpdateCmd(double* value,const ros::Time& time); void UpdateCmd(double* value,const ros::Time& time);
UKF ukfPtr_;
protected: protected:
vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
@@ -76,7 +77,6 @@ protected:
DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
opencl* gpu_fast_; opencl* gpu_fast_;
ros::Time time_; ros::Time time_;
UKF ukfPtr_;
/// Initialize the visual odometry algorithm. /// Initialize the visual odometry algorithm.
virtual void initialize(); virtual void initialize();
GPU_version/vio/include/gpu_svo/global.h
+16 -3
View File
@@ -52,7 +52,6 @@ namespace svo
const double PI = 3.14159265; const double PI = 3.14159265;
class SE2_5: public SE2{ class SE2_5: public SE2{
public: public:
SE2 T2_;
SE2_5(SE2&& se2):T2_(se2){ SE2_5(SE2&& se2):T2_(se2){
} }
SE2_5(SE2& se2):T2_(se2){ SE2_5(SE2& se2):T2_(se2){
@@ -87,17 +86,31 @@ namespace svo
-sin(pitch),cos(pitch); -sin(pitch),cos(pitch);
T2_=SE2(R,Vector2d(x,z)); T2_=SE2(R,Vector2d(x,z));
}; };
SE2 getSE2() const{ SE2 se2() const{
return T2_; return T2_;
} }
SE2 inverse() const{ SE2 inverse() const{
double yaw=atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0)); double yaw=atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0));
return SE2(yaw+PI,T2_.translation()); if(yaw<0.0)
yaw=M_PI-yaw;
else
yaw=yaw-M_PI;
return SE2(yaw,-1.0*T2_.translation());
} }
double pitch()const{ double pitch()const{
return atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0)); return atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0));
} }
SE3 se3() const{
Eigen::Matrix<double,3,3> R;
R<<T2_.rotation_matrix()(0,0),0.0,T2_.rotation_matrix()(0,1),
0.0,1.0,0.0,
T2_.rotation_matrix()(1,0),0.0,T2_.rotation_matrix()(1,1);
return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
}
~SE2_5(){} ~SE2_5(){}
private:
SE2 T2_;
}; };
class Frame; class Frame;
GPU_version/vio/include/gpu_svo/matcher.h
+1 -1
View File
@@ -49,7 +49,7 @@ int getBestSearchLevel(
const Matrix2d& A_cur_ref, const Matrix2d& A_cur_ref,
const int max_level); const int max_level);
void warpAffine( bool warpAffine(
const Matrix2d& A_cur_ref, const Matrix2d& A_cur_ref,
const cv::Mat& img_ref, const cv::Mat& img_ref,
const Vector2d& px_ref, const Vector2d& px_ref,
GPU_version/vio/include/gpu_svo/ukf.h
+19 -6
View File
@@ -56,8 +56,6 @@ public:
E(2)=state_(2)+dt*dpitch-state_h_(2); E(2)=state_(2)+dt*dpitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E; state_=state_h_+k*E;
if(fabs(state_(2)-yaw_t_1_)>M_PI_4)state_(2)-=M_PI;
if(fabs(state_(2))>2*M_PI)state_(2)-=2.0*int(0.5*state_(2)/M_PI)*M_PI;
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*dpitch; state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))); state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
@@ -90,8 +88,6 @@ public:
E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2); E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+(k*E); state_=state_h_+(k*E);
if(fabs(state_(2)-yaw_t_1_)>M_PI_4)state_(2)-=M_PI;
if(fabs(state_(2))>2*M_PI)state_(2)-=2.0*int(0.5*state_(2)/M_PI)*M_PI;
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
yaw_t_1_=state_(2); yaw_t_1_=state_(2);
predict_up=false; predict_up=false;
@@ -131,10 +127,27 @@ public:
std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
while(lock)usleep(5); while(lock)usleep(5);
lock=true; lock=true;
if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec()); if(pitch<0.0)
pitch=M_PI-pitch;
else
pitch=pitch-M_PI;
if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec());
lock=false; lock=false;
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5>(filter_->cov_,svo::SE2_5(filter_->state_(0),filter_->state_(1),filter_->state_(2))); double pitch_f;
if(filter_->state_(0)<0.0)
pitch_f=M_PI-filter_->state_(2);
else
pitch_f=filter_->state_(2)-M_PI;
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
}; };
std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> get_location(){
double pitch_f;
if(filter_->state_(0)<0.0)
pitch_f=M_PI-filter_->state_(2);
else
pitch_f=filter_->state_(2)-M_PI;
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
}
private: private:
Base* filter_= nullptr; Base* filter_= nullptr;
bool lock=false; bool lock=false;
GPU_version/vio/include/gpu_svo/vision.h
+4 -1
View File
@@ -42,7 +42,8 @@ interpolateMat_32f(const cv::Mat& mat, float u, float v)
inline float inline float
interpolateMat_8u(const cv::Mat& mat, float u, float v) interpolateMat_8u(const cv::Mat& mat, float u, float v)
{ {
assert(mat.type()==CV_8U); if(mat.empty())return 0.0;
if(mat.type()!=CV_8U)return 0.0;
int x = floor(u); int x = floor(u);
int y = floor(v); int y = floor(v);
float subpix_x = u-x; float subpix_x = u-x;
@@ -55,6 +56,8 @@ interpolateMat_8u(const cv::Mat& mat, float u, float v)
const int stride = mat.step.p[0]; const int stride = mat.step.p[0];
unsigned char* ptr = mat.data + y*stride + x; unsigned char* ptr = mat.data + y*stride + x;
if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0;
if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0;
return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1];
} }
GPU_version/vio/include/gpu_svo/visualizer.h
+2 -10
View File
@@ -26,6 +26,7 @@
#include <image_transport/image_transport.h> #include <image_transport/image_transport.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gpu_svo/ukf.h>
namespace svo { namespace svo {
@@ -54,18 +55,9 @@ public:
~Visualizer() {}; ~Visualizer() {};
void publishMinimal( void publishMinimal(
const cv::Mat& img, UKF& ukf,
const FramePtr& frame,
const FrameHandlerMono& slam,
const double timestamp); const double timestamp);
void visualizeMarkers(
const FramePtr& frame,
const set<FramePtr>& core_kfs,
const Map& map);
void removeDeletedPts(const Map& map);
}; };
} // end namespace svo } // end namespace svo
GPU_version/vio/param/vo_fast.yaml
+4 -4
View File
@@ -7,9 +7,9 @@ svo:
init_min_tracked: 100 init_min_tracked: 100
init_min_disparity: 30 init_min_disparity: 30
structureoptim_max_pts: 50 structureoptim_max_pts: 50
kfselect_mindist: 0.001 kfselect_mindist: 0.01
poseoptim_thresh: 4.0 poseoptim_thresh: 4.0
ACC_noise: 0.001 #acc white noise in continuous in EKF ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.001 #gyro white noise in continuous in EKF GYO_noise: 0.001 #gyro white noise in continuous in EKF
svo_ekf: 100 #Svo noise that will be devided by number of Reprojected points after opmization in EKF svo_ekf: 1.5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.001 #control vector noise in EKF Cmd_Cov: 1.0 #control vector noise in EKF
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -233,7 +233,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
continue; continue;
} }
// check if point is visible in the current image // check if point is visible in the current image
SE2 T(it->ftr->frame->T_f_w_.getSE2() * frame->T_f_w_.inverse()); SE2 T(it->ftr->frame->T_f_w_.se2() * frame->T_f_w_.inverse());
Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,3> R;
R<<T.rotation_matrix()(0,0),0.0,T.rotation_matrix()(0,1), R<<T.rotation_matrix()(0,0),0.0,T.rotation_matrix()(0,1),
0.0,1.0,0.0, 0.0,1.0,0.0,
GPU_version/vio/src/feature_alignment.cpp
-1
View File
@@ -78,7 +78,6 @@ bool align1D(
int u_r = floor(u); int u_r = floor(u);
int v_r = floor(v); int v_r = floor(v);
if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_){ if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_){
std::cout<<"not in bach size\n";
break; break;
} }
GPU_version/vio/src/frame_handler_mono.cpp
+16 -11
View File
@@ -89,7 +89,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
map_.getClosestKeyframe(last_frame_)); map_.getClosestKeyframe(last_frame_));
// set last frame // set last frame
last_frame_ = new_frame_; if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_;
new_frame_.reset(); new_frame_.reset();
// finish processing // finish processing
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
@@ -103,16 +103,18 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
new_frame_->setKeyframe(); new_frame_->setKeyframe();
map_.addKeyframe(new_frame_); map_.addKeyframe(new_frame_);
stage_ = STAGE_SECOND_FRAME; stage_ = STAGE_SECOND_FRAME;
ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0),new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_);
SVO_INFO_STREAM("Init: Selected first frame."); SVO_INFO_STREAM("Init: Selected first frame.");
return RESULT_IS_KEYFRAME; return RESULT_IS_KEYFRAME;
} }
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{ {
new_frame_->T_f_w_ = last_frame_->T_f_w_; auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_); initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_);
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0),new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); 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(),1000,time_);
new_frame_->T_f_w_ = result.second; new_frame_->T_f_w_ = result.second;
new_frame_->Cov_ = result.first; new_frame_->Cov_ = result.first;
if(res == initialization::NO_KEYFRAME) if(res == initialization::NO_KEYFRAME)
@@ -133,28 +135,31 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
{ {
new_frame_->T_f_w_ = last_frame_->T_f_w_; auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
// sparse image align // sparse image align
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::LevenbergMarquardt, false, false); 30, SparseImgAlign::LevenbergMarquardt, false, false);
img_align.run(last_frame_, new_frame_); if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
reprojector_.reprojectMap(new_frame_, overlap_kfs_); reprojector_.reprojectMap(new_frame_, overlap_kfs_);
size_t sfba_n_edges_final=0; size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final; double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton( pose_optimizer::optimizeGaussNewton(
Config::poseOptimThresh(), Config::poseOptimNumIter(), false, Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
if((last_frame_->T_f_w_.getSE2().translation()-new_frame_->T_f_w_.getSE2().translation()).norm()>0.1){ if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 || fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){
new_frame_->T_f_w_ = last_frame_->T_f_w_; new_frame_=last_frame_;
return RESULT_NO_KEYFRAME;
} }
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0), auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_); 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_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first; new_frame_->Cov_ = result.first;
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_ <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\tReprojected points after opmization: "<<sfba_n_edges_final<<"\t distance between two frames: "<< <<"\tReprojected points after opmization: "<<sfba_n_edges_final<<"\t distance between two frames: "<<
(last_frame_->T_f_w_.getSE2().translation()-new_frame_->T_f_w_.getSE2().translation()).norm()<<'\n'; (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
return RESULT_NO_KEYFRAME; return RESULT_NO_KEYFRAME;
} }
GPU_version/vio/src/initialization.cpp
+3 -3
View File
@@ -69,9 +69,9 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
depth_vec.push_back((xyz_in_cur_[i]).z()); depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec); double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median; double scale = Config::mapScale()/scene_depth_median;
frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.getSE2() * frame_ref_->T_f_w_.getSE2()); frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
frame_cur->T_f_w_.T2_.translation() = -frame_cur->T_f_w_.getSE2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
//frame_cur->T_f_w_.T2_.translation() = frame_cur->T_f_w_.getSE2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); -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 each inlier create 3D point and add feature in both frames
SE3 T_world_cur = frame_cur->getSE3Inv(); SE3 T_world_cur = frame_cur->getSE3Inv();
for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
GPU_version/vio/src/map.cpp
+1 -1
View File
@@ -118,7 +118,7 @@ void Map::getCloseKeyframes(
{ {
close_kfs.push_back( close_kfs.push_back(
std::make_pair( std::make_pair(
kf, (frame->T_f_w_.getSE2().translation()-kf->T_f_w_.getSE2().translation()).norm())); kf, (frame->T_f_w_.se2().translation()-kf->T_f_w_.se2().translation()).norm()));
break; // this keyframe has an overlapping field of view -> add to close_kfs break; // this keyframe has an overlapping field of view -> add to close_kfs
} }
} }
GPU_version/vio/src/matcher.cpp
+35 -37
View File
@@ -69,7 +69,7 @@ int getBestSearchLevel(
return search_level; return search_level;
} }
void warpAffine( bool warpAffine(
const Matrix2d& A_cur_ref, const Matrix2d& A_cur_ref,
const cv::Mat& img_ref, const cv::Mat& img_ref,
const Vector2d& px_ref, const Vector2d& px_ref,
@@ -78,14 +78,14 @@ void warpAffine(
const int halfpatch_size, const int halfpatch_size,
uint8_t* patch) uint8_t* patch)
{ {
if(patch==NULL || img_ref.empty())return false;
const int patch_size = halfpatch_size*2 ; const int patch_size = halfpatch_size*2 ;
const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>(); const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
if(isnan(A_ref_cur(0,0))) if(isnan(A_ref_cur(0,0)))
{ {
printf("Affine warp is NaN, probably camera has no translation\n"); // TODO printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
return; return false;
} }
// Perform the warp on a larger patch. // Perform the warp on a larger patch.
uint8_t* patch_ptr = patch; uint8_t* patch_ptr = patch;
const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref); const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
@@ -96,13 +96,16 @@ void warpAffine(
Vector2f px_patch(x-halfpatch_size, y-halfpatch_size); Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
px_patch *= (1<<search_level); px_patch *= (1<<search_level);
const Vector2f px(A_ref_cur*px_patch + px_ref_pyr); const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1) if(patch_ptr == nullptr)continue;
if (px[0]<1 || px[1]<1 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1){
*patch_ptr = 0; *patch_ptr = 0;
else }else{
*patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]); *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
} }
} }
} }
return true;
}
} // namespace warp } // namespace warp
@@ -117,7 +120,7 @@ bool depthFromTriangulation(
if(AtA.determinant() < 0.000001) if(AtA.determinant() < 0.000001)
return false; return false;
const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation(); const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
depth = fabs(depth2[0]); depth = fabs(depth2[0]/depth2[1]);
return true; return true;
} }
@@ -142,35 +145,36 @@ bool Matcher::findMatchDirect(
if(!ref_ftr_->frame->cam_->isInFrame( if(!ref_ftr_->frame->cam_->isInFrame(
ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level)) ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
return false; return false;
if(ref_ftr_->frame->img_pyr_.empty())return false;
if(ref_ftr_->frame->img_pyr_[ref_ftr_->level].empty())return false;
// warp affine // warp affine
warp::getWarpMatrixAffine( warp::getWarpMatrixAffine(
*ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f, *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
(Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(), (Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(),
cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_); cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_);
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_); ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_))return false;
createPatchFromPatchWithBorder(); createPatchFromPatchWithBorder();
// px_cur should be set // px_cur should be set
Vector2d px_scaled(px_cur/(1<<search_level_)); Vector2d px_scaled(px_cur/(1<<search_level_));
bool success = false; bool success = false;
if(ref_ftr_->type == Feature::EDGELET) // if(ref_ftr_->type == Feature::EDGELET)
{ //{
Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); // Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
dir_cur.normalize(); // dir_cur.normalize();
success = feature_alignment::align1D( // success = feature_alignment::align1D(
cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(), // cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); // patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
} //}
else //else
{ //{
success = feature_alignment::align2D( success = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled); options_.align_max_iter, px_scaled);
} // }
px_cur = px_scaled * (1<<search_level_); px_cur = px_scaled * (1<<search_level_);
return success; return success;
} }
@@ -185,24 +189,19 @@ bool Matcher::findEpipolarMatchDirect(
double& depth) double& depth)
{ {
if(isnan(d_min) || isnan(d_max))return false; if(isnan(d_min) || isnan(d_max))return false;
SE2 T(cur_frame.T_f_w_.getSE2() * ref_frame.T_f_w_.inverse()); SE2_5 T_cur_ref(cur_frame.T_f_w_.se2() * ref_frame.T_f_w_.inverse());
Eigen::Matrix<double,3,3> R;
R<<T.rotation_matrix()(0,0),0.0,T.rotation_matrix()(0,1),
0.0,1.0,0.0,
T.rotation_matrix()(1,0),0.0,T.rotation_matrix()(1,1);
SE3 T_cur_ref(R,Vector3d(T.translation()(0),0.0,T.translation()(1)));
int zmssd_best = PatchScore::threshold(); int zmssd_best = PatchScore::threshold();
Vector2d uv_best; Vector2d uv_best;
// Compute start and end of epipolar line in old_kf for match search, on unit plane! // Compute start and end of epipolar line in old_kf for match search, on unit plane!
Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min)); Vector2d A = vk::project2d(T_cur_ref.se3() * (ref_ftr.f*d_min));
Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max)); Vector2d B = vk::project2d(T_cur_ref.se3() * (ref_ftr.f*d_max));
epi_dir_ = A - B; epi_dir_ = A - B;
// Compute affine warp matrix // Compute affine warp matrix
warp::getWarpMatrixAffine( warp::getWarpMatrixAffine(
*ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f, *ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_); d_estimate, T_cur_ref.se3(), ref_ftr.level, A_cur_ref_);
// feature pre-selection // feature pre-selection
reject_ = false; reject_ = false;
@@ -225,13 +224,12 @@ bool Matcher::findEpipolarMatchDirect(
// Warp reference patch at ref_level // Warp reference patch at ref_level
warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px, if(!warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_); ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_))return false;
createPatchFromPatchWithBorder(); createPatchFromPatchWithBorder();
if(epi_length_ < 5.0)
if(epi_length_ < 2.0)
{ {
px_cur_ = (px_A+px_B)/2.0; px_cur_ = (px_A+px_B)/5.0;
Vector2d px_scaled(px_cur_/(1<<search_level_)); Vector2d px_scaled(px_cur_/(1<<search_level_));
bool res; bool res;
if(options_.align_1d) if(options_.align_1d)
@@ -245,7 +243,7 @@ bool Matcher::findEpipolarMatchDirect(
if(res) if(res)
{ {
px_cur_ = px_scaled*(1<<search_level_); px_cur_ = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth)) if(depthFromTriangulation(T_cur_ref.se3(), ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
return true; return true;
} }
return false; return false;
@@ -314,13 +312,13 @@ bool Matcher::findEpipolarMatchDirect(
if(res) if(res)
{ {
px_cur_ = px_scaled*(1<<search_level_); px_cur_ = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth)) if(depthFromTriangulation(T_cur_ref.se3(), ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
return true; return true;
} }
return false; return false;
} }
px_cur_ = cur_frame.cam_->world2cam(uv_best); px_cur_ = cur_frame.cam_->world2cam(uv_best);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth)) if(depthFromTriangulation(T_cur_ref.se3(), ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
return true; return true;
} }
return false; return false;
GPU_version/vio/src/point.cpp
+6 -8
View File
@@ -99,21 +99,19 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
{ {
// TODO: get frame with same point of view AND same pyramid level! // TODO: get frame with same point of view AND same pyramid level!
Vector3d obs_dir(Vector3d(framepos(0),0.0,framepos(1)) - pos_); obs_dir.normalize(); Vector3d obs_dir(Vector3d(framepos(0),0.0,framepos(1)) - pos_); obs_dir.normalize();
auto min_it=obs_.begin(); auto max_it=obs_.begin();
double min_cos_angle = 0; double max_cos_angle = 1;
for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
{ {
Vector3d dir(Vector3d((*it)->frame->pos()(0),0.0,(*it)->frame->pos()(1)) - pos_); dir.normalize(); Vector3d dir(Vector3d((*it)->frame->pos()(0),0.0,(*it)->frame->pos()(1)) - pos_); dir.normalize();
double cos_angle = obs_dir.dot(dir); double cos_angle = obs_dir.dot(dir);
if(cos_angle > min_cos_angle) if(cos_angle < max_cos_angle)
{ {
min_cos_angle = cos_angle; max_cos_angle = cos_angle;
min_it = it; max_it = it;
} }
} }
ftr = *min_it; ftr = *max_it;
if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless
return false;
return true; return true;
} }
GPU_version/vio/src/pose_optimizer.cpp
+2 -2
View File
@@ -121,9 +121,9 @@ void optimizeGaussNewton(
// update the model // update the model
T_old = frame->T_f_w_; T_old = frame->T_f_w_;
if(direction){ if(direction){
frame->T_f_w_=SE2_5(SE2::exp(dT)*(T_old.getSE2())); frame->T_f_w_=SE2_5(SE2::exp(dT)*(T_old.se2()));
}else{ }else{
frame->T_f_w_=SE2_5(SE2::exp(-1.0*dT)*(T_old.getSE2())); frame->T_f_w_=SE2_5(SE2::exp(-1.0*dT)*(T_old.se2()));
} }
chi2 = new_chi2; chi2 = new_chi2;
if(verbose) if(verbose)
GPU_version/vio/src/reprojector.cpp
-1
View File
@@ -72,7 +72,6 @@ void Reprojector::reprojectMap(
list< pair<FramePtr,double> > close_kfs; list< pair<FramePtr,double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs); map_.getCloseKeyframes(frame, close_kfs);
std::cout<<"The number of closed frames: "<<std::to_string(close_kfs.size())<<'\n';
// Sort KFs with overlap according to their closeness // Sort KFs with overlap according to their closeness
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) < close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2)); boost::bind(&std::pair<FramePtr, double>::second, _2));
GPU_version/vio/src/sparse_img_align.cpp
+3 -5
View File
@@ -46,7 +46,6 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
if(ref_frame->fts_.empty()) if(ref_frame->fts_.empty())
{ {
SVO_WARN_STREAM("SparseImgAlign: no features to track!");
return 0; return 0;
} }
@@ -54,8 +53,7 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
cur_frame_ = cur_frame; cur_frame_ = cur_frame;
ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F); ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_); jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
//visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? SE2 T_cur_from_ref(cur_frame_->T_f_w_.se2() * ref_frame_->T_f_w_.inverse());
SE2 T_cur_from_ref(cur_frame_->T_f_w_.getSE2() * ref_frame_->T_f_w_.inverse());
for(level_=max_level_; level_>=min_level_; --level_) for(level_=max_level_; level_>=min_level_; --level_)
{ {
visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
@@ -66,7 +64,7 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
printf("\nPYRAMID LEVEL %i\n---------------\n", level_); printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
optimize(T_cur_from_ref); optimize(T_cur_from_ref);
} }
cur_frame_->T_f_w_ = SE2_5(T_cur_from_ref * ref_frame_->T_f_w_.getSE2()); cur_frame_->T_f_w_ = SE2_5(T_cur_from_ref * ref_frame_->T_f_w_.se2());
return n_meas_/patch_area_; return n_meas_/patch_area_;
} }
@@ -251,7 +249,7 @@ void SparseImgAlign::update(
const ModelType& T_curold_from_ref, const ModelType& T_curold_from_ref,
ModelType& T_curnew_from_ref) ModelType& T_curnew_from_ref)
{ {
T_curnew_from_ref = T_curold_from_ref * SE2::exp(-1.0*x_); T_curnew_from_ref = T_curold_from_ref * SE2::exp(-0.25*x_);
} }
void SparseImgAlign::startIteration() void SparseImgAlign::startIteration()
GPU_version/vio/src/visualizer.cpp
+11 -42
View File
@@ -24,6 +24,7 @@
#include <gpu_svo/timer.h> #include <gpu_svo/timer.h>
#include <gpu_svo/output_helper.h> #include <gpu_svo/output_helper.h>
#include <deque> #include <deque>
#include <gpu_svo/ukf.h>
namespace svo { namespace svo {
@@ -40,9 +41,7 @@ Visualizer() :
} }
void Visualizer::publishMinimal( void Visualizer::publishMinimal(
const cv::Mat& img, UKF& ukf,
const FramePtr& frame,
const FrameHandlerMono& slam,
const double timestamp) const double timestamp)
{ {
++trace_id_; ++trace_id_;
@@ -50,63 +49,33 @@ void Visualizer::publishMinimal(
header_msg.frame_id = "/world"; header_msg.frame_id = "/world";
header_msg.seq = trace_id_; header_msg.seq = trace_id_;
header_msg.stamp = ros::Time(timestamp); header_msg.stamp = ros::Time(timestamp);
if(pub_pose_with_cov_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME) if(pub_pose_with_cov_.getNumSubscribers() > 0)
{ {
// publish cam in world frame (Estimated odometry in the worls frame) // publish cam in world frame (Estimated odometry in the worls frame)
Quaterniond q(AngleAxisd(frame->T_f_w_.pitch(), Vector3d::UnitZ())); auto odom=ukf.get_location();
Quaterniond q(AngleAxisd(odom.second.pitch()+M_PI_2, Vector3d::UnitZ()));
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose_with_cov->header = header_msg; msg_pose_with_cov->header = header_msg;
msg_pose_with_cov->pose.pose.position.x = frame->se3().translation().x(); msg_pose_with_cov->pose.pose.position.x = -odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.y = frame->se3().translation().z(); msg_pose_with_cov->pose.pose.position.y = -odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.z = 0.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.x = q.x();
msg_pose_with_cov->pose.pose.orientation.y = q.y(); msg_pose_with_cov->pose.pose.orientation.y = q.y();
msg_pose_with_cov->pose.pose.orientation.z = q.z(); msg_pose_with_cov->pose.pose.orientation.z = q.z();
msg_pose_with_cov->pose.pose.orientation.w = q.w(); msg_pose_with_cov->pose.pose.orientation.w = q.w();
msg_pose_with_cov->pose.covariance = {frame->Cov_(0,0),frame->Cov_(0,1),0.0,0.0,frame->Cov_(0,2),0.0, msg_pose_with_cov->pose.covariance = {odom.first(0,0),odom.first(0,1),0.0,0.0,odom.first(0,2),0.0,
frame->Cov_(1,0),frame->Cov_(1,1),0.0,0.0,frame->Cov_(1,2),0.0, odom.first(1,0),odom.first(1,1),0.0,0.0,odom.first(1,2),0.0,
0.0,0.0,0.0000001,0.0,0.0,0.0, 0.0,0.0,0.0000001,0.0,0.0,0.0,
0.0,0.0,0.0,0.0000001,0.0,0.0, 0.0,0.0,0.0,0.0000001,0.0,0.0,
frame->Cov_(2,0),frame->Cov_(2,1),0.0,0.0,frame->Cov_(2,2),0.0, odom.first(2,0),odom.first(2,1),0.0,0.0,odom.first(2,2),0.0,
0.0,0.0,0.0,0.0,0.0,0.0000001}; 0.0,0.0,0.0,0.0,0.0,0.0000001};
pub_pose_with_cov_.publish(msg_pose_with_cov); pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
publishPointMarker( publishPointMarker(
pub_points_, Eigen::Vector3d(frame->se3().translation().x(),frame->se3().translation().z(),0.0), "trajectory", pub_points_, Eigen::Vector3d(-odom.second.se2().translation()(0),-odom.second.se2().translation()(1),0.0), "trajectory",
ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5)); ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
} }
} }
} }
void Visualizer::visualizeMarkers(
const FramePtr& frame,
const set<FramePtr>& core_kfs,
const Map& map)
{
if(frame == NULL)
return;
publishTfTransform(
frame->se3()*T_world_from_vision_.inverse(),
ros::Time(frame->timestamp_), "cam_pos", "world", br_);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
{
publishPointMarker(
pub_points_, T_world_from_vision_*Vector3d(frame->pos()(0),0.0,frame->pos()(1)), "trajectory",
ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
removeDeletedPts(map);
}
}
void Visualizer::removeDeletedPts(const Map& map)
{
if(pub_points_.getNumSubscribers() > 0)
{
for(list<Point*>::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it)
publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d());
}
}
} // end namespace gpu_svo } // end namespace gpu_svo
GPU_version/vio/src/vo_node.cpp
+1 -3
View File
@@ -108,9 +108,6 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
auto start=std::chrono::steady_clock::now(); auto start=std::chrono::steady_clock::now();
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp); vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp);
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
if(vo_->stage() == svo::FrameHandlerMono::STAGE_PAUSED)
usleep(100000);
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_, ",,,,,,,%s,%f,%f,%f\n",std::to_string(std::chrono::duration<double>(std::chrono::steady_clock::now()-start).count()).c_str(),vo_->lastFrame()->T_f_w_.translation()(0), fprintf(log_, ",,,,,,,%s,%f,%f,%f\n",std::to_string(std::chrono::duration<double>(std::chrono::steady_clock::now()-start).count()).c_str(),vo_->lastFrame()->T_f_w_.translation()(0),
vo_->lastFrame()->T_f_w_.translation()(1), vo_->lastFrame()->T_f_w_.translation()(1),
@@ -158,6 +155,7 @@ void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
#endif #endif
vo_->UpdateIMU(imu_in,imu->header.stamp); vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp; imu_time_=imu->header.stamp;
visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
} }
void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) { void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z}; double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};