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

debug close obsview

Browse Source
This commit is contained in:
gfuse
2021-09-01 22:49:21 +02:00
parent fb23169978
commit 4d857d790c
10 changed files with 48 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+1 -1
View File
@@ -39,7 +39,7 @@ add_service_files(FILES
generate_messages(DEPENDENCIES std_msgs)
catkin_package(
DEPENDS Eigen OpenCV Sophus Boost
DEPENDS Eigen OpenCV Boost
CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime
INCLUDE_DIRS include
)
GPU_version/vio/include/sophus/se2.cpp
+7 -7
View File
@@ -62,15 +62,15 @@ SE2 SE2
{
SE2 result(*this);
result.translation_ += so2_*(other.translation_);
result.so2_*=other.so2_;
result.so2_ *= other.so2_;
return result;
}
SE2& SE2
::operator *= (const SE2 & other)
{
translation_+= so2_*(other.translation_);
so2_*=other.so2_;
translation_ += so2_ * (other.translation_);
so2_ *= other.so2_;
return *this;
}
@@ -79,7 +79,7 @@ SE2 SE2
{
SE2 ret;
ret.so2_= so2_.inverse();
ret.translation_ = ret.so2_*(translation_*-1.);
ret.translation_ = ret.so2_ * (translation_ *-1.);
return ret;
}
@@ -113,8 +113,8 @@ Matrix<double, 3, 3> SE2
Matrix3d res;
res.setIdentity();
res.topLeftCorner<2,2>() = R;
res(0,2) = translation_[1];
res(1,2) = -translation_[0];
res(0,2) = translation_.y();
res(1,2) = -translation_.x();
return res;
}
@@ -220,7 +220,7 @@ Vector3d SE2
Matrix2d V_inv;
V_inv(0,0) = halftheta_by_tan_of_halftheta; V_inv(1,0) = -halftheta;
V_inv(0,1) = halftheta; V_inv(1,1) = halftheta_by_tan_of_halftheta;
upsilon_theta.head<2>() = V_inv*se2.translation_;
upsilon_theta.head<2>() = V_inv* se2.translation_;
return upsilon_theta;
}
GPU_version/vio/include/sophus/se2.h
+3 -3
View File
@@ -44,7 +44,7 @@ public:
const Vector2d & translation);
SE2 (double theta,
const Vector2d & translation_);
const Vector2d & translation);
SE2 (const SE2 & other);
@@ -95,11 +95,11 @@ public:
{
return translation_;
}
/*
Vector2d& translation()
{
return translation_;
}
}*/
Matrix2d rotation_matrix() const
{
GPU_version/vio/include/vio/frame.h
+2 -2
View File
@@ -119,8 +119,8 @@ namespace vio {
/// Return the pose of the frame in the (w)orld coordinate frame.
inline Vector2d pos() const {
Vector2d tem=T_f_w_.se2().translation();
return tem;
SE2 tem=T_f_w_.se2();
return tem.translation();
}
inline SE3 se3() const{
GPU_version/vio/include/vio/global.h
+14 -16
View File
@@ -52,31 +52,29 @@ namespace vio
const double PI = 3.14159265;
class SE2_5: public SE2{
public:
SE2_5(SE2&& se2):T2_(new SE2(se2)){
SE2_5(SE2&& se2){
T2_=new SE2(se2);
assert(T2_!= nullptr);
}
SE2_5(SE2& se2):T2_(new SE2(se2)){
SE2_5(SE2& se2){
T2_=new SE2(se2);
assert(T2_!= nullptr);
}
SE2_5(SE3&& se3){
Quaterniond q=se3.unit_quaternion().normalized();
auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
Eigen::Matrix<double, 2,2> R;
R<<cos(euler(0)),sin(euler(0)),
-sin(euler(0)),cos(euler(0));
T2_ = new SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
T2_ = new SE2(SO2(euler(1)),Vector2d(se3.translation().x(),se3.translation().z()));
assert(T2_!= nullptr);
}
SE2_5(SE3& se3){
Quaterniond q=se3.unit_quaternion().normalized();
auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
Eigen::Matrix<double, 2,2> R;
R<<cos(euler(0)),sin(euler(0)),
-sin(euler(0)),cos(euler(0));
T2_=new SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
T2_=new SE2(SO2(euler(1)),Vector2d(se3.translation().x(),se3.translation().z()));
assert(T2_!= nullptr);
}
SE2_5(double y,double z,double roll){
Eigen::Matrix<double, 2,2> R;
R<<cos(roll),sin(roll),
-sin(roll),cos(roll);
T2_=new SE2(R,Vector2d(y,z));
SE2_5(double y,double z,double pitch){
T2_=new SE2(SO2(pitch),Vector2d(y,z));
assert(T2_!= nullptr);
};
SE2 se2() const{
assert(T2_!= nullptr);
@@ -100,7 +98,7 @@ namespace vio
}
~SE2_5(){
}
bool empty(){
bool empty() const{
return T2_ == nullptr ? true : false;
}
GPU_version/vio/include/vio/point.h
+1
View File
@@ -59,6 +59,7 @@ public:
int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point.
int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point.
int last_structure_optim_; //!< Timestamp of last point optimization
bool syn_on=false;
Point(const Vector3d& pos);
Point(const Vector3d& pos, Feature* ftr);
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -141,7 +141,7 @@ namespace vio {
size_t n_removed = 0;
while(it!=seeds_.end())
{
if(it->ftr->frame == frame.get())
if(it->ftr->frame->id_ == frame->id_)
{
it = seeds_.erase(it);
++n_removed;
GPU_version/vio/src/matcher.cpp
+2 -3
View File
@@ -141,10 +141,9 @@ bool Matcher::findMatchDirect(
Vector2d& px_cur)
{
if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;
//if(ref_ftr_->frame->cam_== nullptr)return false;
assert(ref_ftr_->frame!= nullptr);
if(ref_ftr_== nullptr)return false;
if(ref_ftr_->frame== nullptr)return false;
if(ref_ftr_->frame->cam_== nullptr)return false;
assert(ref_ftr_->frame->cam_!= nullptr);
Vector2i pxi=ref_ftr_->px.cast<int>();
if(ref_ftr_->level==NULL){
if(!ref_ftr_->frame->cam_->isInFrame(pxi, 6))return false;
GPU_version/vio/src/point.cpp
+4 -2
View File
@@ -75,7 +75,7 @@ bool Point::deleteFrameRef(Frame* frame)
{
for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
{
if((*it)->frame == frame)
if((*it)->frame->id_ == frame->id_)
{
obs_.erase(it);
return true;
@@ -98,11 +98,12 @@ void Point::initNormal()
bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
{
// TODO: get frame with same point of view AND same pyramid level!
ftr= nullptr;
Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize();
double max_cos_angle = 1.0;
if(obs_.empty())return false;
for(auto&& ob:obs_){
if(ob->frame->T_f_w_.empty())continue;
if(ob == nullptr)continue;
Vector2d t=ob->frame->pos();
Vector3d dir(Vector3d(t.x(),1e-9,t.y()) + pos_);
double cos_angle = obs_dir.dot(dir.normalized());
@@ -112,6 +113,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
ftr = ob;
}
}
if(ftr== nullptr)return false;
return true;
}
GPU_version/vio/src/reprojector.cpp
+13 -9
View File
@@ -139,6 +139,7 @@ namespace vio {
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
overlap_kfs.reserve(options_.max_n_kfs);
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
for (auto &&it_frame:_for(close_kfs)) {
if (it_frame.index > options_.max_n_kfs)continue;
std::vector<cv::KeyPoint> keypoints_kfs;
@@ -151,11 +152,14 @@ namespace vio {
extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref);
matcher->match(descriptors_ref, descriptors_cur, matches);
for (auto &&f: _for(it_frame.item.first->fts_)) {
if (f.item->point == NULL)f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item);
if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
f.item->point->last_frame_overlap_id_ = frame->id_;
for (auto &&match:matches) {
if (match.queryIdx != f.index)continue;
if (f.item->point == NULL){
f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()));
// assert(!f.item->point->obs_.empty());
}
if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
f.item->point->last_frame_overlap_id_ = frame->id_;
assert(keypoints_cur.size() > match.trainIdx);
const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
grid_.grid_n_cols
@@ -163,6 +167,8 @@ namespace vio {
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
(int) keypoints_cur.at(match.trainIdx).pt.y);
f.item->point->addFrameRef(new Feature(it_frame.item.first.get(),f.item->px,0));
//f.item->point->addFrameRef(f.item);
grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
overlap_kfs.back().second++;
break;
@@ -170,7 +176,6 @@ namespace vio {
}
}
{
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
for (auto&& cell : grid_.cells) {
if (reprojectCell(*cell, frame))
++n_matches_;
@@ -193,16 +198,14 @@ namespace vio {
while(it!=cell.end())
{
++n_trials_;
if(it->pt->type_ == Point::TYPE_DELETED)
{
it = cell.erase(it);
continue;
}
bool found_match = true;
if(options_.find_match_direct)
found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
if(!found_match)
syn_=true;
if(!matcher_.findMatchDirect(*it->pt, *frame, it->px))
{
it->pt->n_failed_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
@@ -212,6 +215,7 @@ namespace vio {
it = cell.erase(it);
continue;
}
syn_=false;
it->pt->n_succeeded_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::TYPE_GOOD;