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).
inline Vector2d w2c(const Vector3d& xyz_w) const {
Eigen::Matrix<double,3,3> R;
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);
return cam_->world2cam( T_f_w_.se3()*xyz_w);
}
/// 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 ); }
@@ -110,35 +106,22 @@ public:
/// Transforms point coordinates in world-frame (w) to camera-frams (f).
inline Vector3d w2f(const Vector3d& xyz_w) const {
Eigen::Matrix<double,3,3> R;
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);
return Vector3d(T_f_w_.se3()*xyz_w);
}
/// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
inline Vector3d f2w(const Vector3d& f) const {
Eigen::Matrix<double,3,3> R;
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;
return getSE3Inv() * f;
}
/// Projects Point from unit sphere (f) in camera pixels (c).
inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
/// 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{
Eigen::Matrix<double,3,3> R;
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;
return T_f_w_.se3();
}
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
void UpdateIMU(double* value,const ros::Time& time);
void UpdateCmd(double* value,const ros::Time& time);
UKF ukfPtr_;
protected:
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.
opencl* gpu_fast_;
ros::Time time_;
UKF ukfPtr_;
/// Initialize the visual odometry algorithm.
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;
class SE2_5: public SE2{
public:
SE2 T2_;
SE2_5(SE2&& se2):T2_(se2){
}
SE2_5(SE2& se2):T2_(se2){
@@ -87,17 +86,31 @@ namespace svo
-sin(pitch),cos(pitch);
T2_=SE2(R,Vector2d(x,z));
};
SE2 getSE2() const{
SE2 se2() const{
return T2_;
}
SE2 inverse() const{
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{
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(){}
private:
SE2 T2_;
};
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 int max_level);
void warpAffine(
bool warpAffine(
const Matrix2d& A_cur_ref,
const cv::Mat& img_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);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
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_;
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)));
@@ -90,8 +88,6 @@ public:
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();
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_;
yaw_t_1_=state_(2);
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) {
while(lock)usleep(5);
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;
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:
Base* filter_= nullptr;
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
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 y = floor(v);
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];
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];
}
GPU_version/vio/include/gpu_svo/visualizer.h
+2 -10
View File
@@ -26,6 +26,7 @@
#include <image_transport/image_transport.h>
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <gpu_svo/ukf.h>
namespace svo {
@@ -54,18 +55,9 @@ public:
~Visualizer() {};
void publishMinimal(
const cv::Mat& img,
const FramePtr& frame,
const FrameHandlerMono& slam,
UKF& ukf,
const double timestamp);
void visualizeMarkers(
const FramePtr& frame,
const set<FramePtr>& core_kfs,
const Map& map);
void removeDeletedPts(const Map& map);
};
} // 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_disparity: 30
structureoptim_max_pts: 50
kfselect_mindist: 0.001
kfselect_mindist: 0.01
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
svo_ekf: 100 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.001 #control vector noise in EKF
svo_ekf: 1.5 #Svo noise that will be devided by number of Reprojected points after opmization 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;
}
// 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;
R<<T.rotation_matrix()(0,0),0.0,T.rotation_matrix()(0,1),
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 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_){
std::cout<<"not in bach size\n";
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_));
// set last frame
last_frame_ = new_frame_;
if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_;
new_frame_.reset();
// finish processing
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
@@ -103,16 +103,18 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
new_frame_->setKeyframe();
map_.addKeyframe(new_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.");
return RESULT_IS_KEYFRAME;
}
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_);
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_->Cov_ = result.first;
if(res == initialization::NO_KEYFRAME)
@@ -133,28 +135,31 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
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
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
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_);
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
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_.getSE2().translation()-new_frame_->T_f_w_.getSE2().translation()).norm()>0.1){
new_frame_->T_f_w_ = last_frame_->T_f_w_;
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_=last_frame_;
return RESULT_NO_KEYFRAME;
}
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(),sfba_n_edges_final,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(),sfba_n_edges_final,time_);
new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first;
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\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()){
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());
double scene_depth_median = vk::getMedian(depth_vec);
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_.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_.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(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
-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
SE3 T_world_cur = frame_cur->getSE3Inv();
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(
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
}
}
GPU_version/vio/src/matcher.cpp
+35 -37
View File
@@ -69,7 +69,7 @@ int getBestSearchLevel(
return search_level;
}
void warpAffine(
bool warpAffine(
const Matrix2d& A_cur_ref,
const cv::Mat& img_ref,
const Vector2d& px_ref,
@@ -78,14 +78,14 @@ void warpAffine(
const int halfpatch_size,
uint8_t* patch)
{
if(patch==NULL || img_ref.empty())return false;
const int patch_size = halfpatch_size*2 ;
const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
if(isnan(A_ref_cur(0,0)))
{
printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
return;
return false;
}
// Perform the warp on a larger patch.
uint8_t* patch_ptr = patch;
const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
@@ -96,12 +96,15 @@ void warpAffine(
Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
px_patch *= (1<<search_level);
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;
else
}else{
*patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
}
}
}
return true;
}
} // namespace warp
@@ -117,7 +120,7 @@ bool depthFromTriangulation(
if(AtA.determinant() < 0.000001)
return false;
const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
depth = fabs(depth2[0]);
depth = fabs(depth2[0]/depth2[1]);
return true;
}
@@ -142,35 +145,36 @@ bool Matcher::findMatchDirect(
if(!ref_ftr_->frame->cam_->isInFrame(
ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
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::getWarpMatrixAffine(
*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(),
cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_);
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,
ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
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_))return false;
createPatchFromPatchWithBorder();
// px_cur should be set
Vector2d px_scaled(px_cur/(1<<search_level_));
bool success = false;
if(ref_ftr_->type == Feature::EDGELET)
{
Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
dir_cur.normalize();
success = feature_alignment::align1D(
cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
}
else
{
// if(ref_ftr_->type == Feature::EDGELET)
//{
// Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
// dir_cur.normalize();
// success = feature_alignment::align1D(
// cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
// patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
//}
//else
//{
success = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
}
// }
px_cur = px_scaled * (1<<search_level_);
return success;
}
@@ -185,24 +189,19 @@ bool Matcher::findEpipolarMatchDirect(
double& depth)
{
if(isnan(d_min) || isnan(d_max))return false;
SE2 T(cur_frame.T_f_w_.getSE2() * 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)));
SE2_5 T_cur_ref(cur_frame.T_f_w_.se2() * ref_frame.T_f_w_.inverse());
int zmssd_best = PatchScore::threshold();
Vector2d uv_best;
// 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 B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
Vector2d A = vk::project2d(T_cur_ref.se3() * (ref_ftr.f*d_min));
Vector2d B = vk::project2d(T_cur_ref.se3() * (ref_ftr.f*d_max));
epi_dir_ = A - B;
// Compute affine warp matrix
warp::getWarpMatrixAffine(
*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
reject_ = false;
@@ -225,13 +224,12 @@ bool Matcher::findEpipolarMatchDirect(
// Warp reference patch at ref_level
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_);
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_))return false;
createPatchFromPatchWithBorder();
if(epi_length_ < 2.0)
if(epi_length_ < 5.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_));
bool res;
if(options_.align_1d)
@@ -245,7 +243,7 @@ bool Matcher::findEpipolarMatchDirect(
if(res)
{
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 false;
@@ -314,13 +312,13 @@ bool Matcher::findEpipolarMatchDirect(
if(res)
{
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 false;
}
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 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!
Vector3d obs_dir(Vector3d(framepos(0),0.0,framepos(1)) - pos_); obs_dir.normalize();
auto min_it=obs_.begin();
double min_cos_angle = 0;
auto max_it=obs_.begin();
double max_cos_angle = 1;
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();
double cos_angle = obs_dir.dot(dir);
if(cos_angle > min_cos_angle)
if(cos_angle < max_cos_angle)
{
min_cos_angle = cos_angle;
min_it = it;
max_cos_angle = cos_angle;
max_it = it;
}
}
ftr = *min_it;
if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless
return false;
ftr = *max_it;
return true;
}
GPU_version/vio/src/pose_optimizer.cpp
+2 -2
View File
@@ -121,9 +121,9 @@ void optimizeGaussNewton(
// update the model
T_old = frame->T_f_w_;
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{
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;
if(verbose)
GPU_version/vio/src/reprojector.cpp
-1
View File
@@ -72,7 +72,6 @@ void Reprojector::reprojectMap(
list< pair<FramePtr,double> > 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
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
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())
{
SVO_WARN_STREAM("SparseImgAlign: no features to track!");
return 0;
}
@@ -54,8 +53,7 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
cur_frame_ = cur_frame;
ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
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_.getSE2() * ref_frame_->T_f_w_.inverse());
SE2 T_cur_from_ref(cur_frame_->T_f_w_.se2() * ref_frame_->T_f_w_.inverse());
for(level_=max_level_; level_>=min_level_; --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_);
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_;
}
@@ -251,7 +249,7 @@ void SparseImgAlign::update(
const ModelType& T_curold_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()
GPU_version/vio/src/visualizer.cpp
+11 -42
View File
@@ -24,6 +24,7 @@
#include <gpu_svo/timer.h>
#include <gpu_svo/output_helper.h>
#include <deque>
#include <gpu_svo/ukf.h>
namespace svo {
@@ -40,9 +41,7 @@ Visualizer() :
}
void Visualizer::publishMinimal(
const cv::Mat& img,
const FramePtr& frame,
const FrameHandlerMono& slam,
UKF& ukf,
const double timestamp)
{
++trace_id_;
@@ -50,63 +49,33 @@ void Visualizer::publishMinimal(
header_msg.frame_id = "/world";
header_msg.seq = trace_id_;
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)
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);
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.y = frame->se3().translation().z();
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.z = 0.0;
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.z = q.z();
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,
frame->Cov_(1,0),frame->Cov_(1,1),0.0,0.0,frame->Cov_(1,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,
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.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};
pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
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));
}
}
}
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
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();
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
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
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),
@@ -158,6 +155,7 @@ void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
#endif
vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp;
visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
}
void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};