SVO-6-map–2021.1.29
map.h
#ifndef SVO_MAP_H_
#define SVO_MAP_H_
#include <queue>
#include <boost/noncopyable.hpp>
#include <boost/thread.hpp>
#include <svo/global.h>
namespace svo{
class Point ;
class Feature ;
class Seed ;
class MapPointCandidates {
public :
typedef pair< Point* , Feature* > PointCandidate;
typedef pair< PointCandidate> PointCandidateList;
boost:: mutex mut_;
PointCandidateList camdidates_;
list< Point* > trash_points_;
MapPointCandidates ( ) ;
~ MapPointCandidates ( ) ;
void newCandidatePoint ( Point* point, double depth_sigma2) ;
void addCandidatePointToFrame ( FramePtr frame) ;
bool deleteCandidatePoint ( Point* point) ;
void removeFrameCandidates ( FramePtr frame) ;
void reset ( ) ;
void deleteCandidate ( PointCandidate& c) ;
void emptyTrash ( ) ;
} ;
class Map : boost:: noncopyable{
public :
list< FramePtr > keyframes_;
list< Point* > trash_points_;
MapPointCandidates point_candidates_;
Map ( ) ;
~ Map ( ) ;
void reset ( ) ;
void safeDeletePoint ( Point* pt) ;
void deletePoint ( Point* pt) ;
bool safeDeleteFrame ( FramePtr frame) ;
void removePtFrameRef ( Frame* frame, Feature* ftr) ;
void addKeyframe ( FramePtr new_keyframe) ;
void getCloseKeyframes ( const FramePtr& frame, list< pair< FramePtr, double >> & close_kfs) const ;
FramePtr getClosestKeyframes ( const FramePtr& frame) const ;
FramePtr getFurthestKeyframe ( const Vector3d& pos) const ;
bool getKeyframeById ( const int id, FramePtr& frame) const ;
void transform ( const Matrix3d& R, const Vector3d* t, const double & s) ;
void emptyTrash ( ) ;
inline FramePtr lastKeyframe ( ) { return keyframes_. back ( ) ; }
inline size_t size ( ) const { return keyframes_. size ( ) ; }
} ;
namespace map_debug{
void mapStatistics ( Map* map) ;
void mapValidation ( Map* map, int id) ;
void frameValidation ( Frame* frame, int id) ;
void pointValidation ( Point* point, int id) ;
}
}
#endif
map.cpp
#include <set>
#include <svo_practice/map.h>
#include <svo_practice/point.h>
#include <svo_practice/frame.h>
#include <svo_practice/feature.h>
#include <boost/bind.hpp>
namespace svo{
svo:: Map:: Map ( ) { }
svo:: Map:: ~ Map ( ) {
reset ( ) ;
SVO_INFO_STREAM ( "map destructed" ) ;
}
void svo:: Map:: reset ( )
{
keyframes_. clear ( ) ;
point_candidates_. reset ( ) ;
emptyTrash ( ) ;
}
bool svo:: Map:: safeDeleteFrame ( FramePtr frame)
{
bool found = false ;
for ( auto it = keyframes_. begin ( ) , ite = keyframes_. end ( ) ; it != ite, ++ it) {
if ( * it == frame) {
std:: for_each ( ( * it) - > fts_. begin ( ) , ( * it) - > fts_. end ( ) , [ & ] ( Feature* ftr) {
removePtFrameRef ( it - > get ( ) , ftr) ;
} ) ;
keyframes_. erase ( it) ;
found = true ;
break ;
}
}
point_candidates_. removeFrameCandidates ( frame) ;
if ( found) return true ;
SVO_ERROR_STREAM ( "Tried to delete Keyframe in map which was not there." ) ;
return false ;
}
void svo:: Map:: removePtFrameRef ( Frame* frame, svo:: Feature* ftr)
{
if ( ftr - > point == NULL ) return ;
Point* pt = ftr - > point;
ftr - > point = NULL ;
if ( pt - > obs_. size ( ) <= 2 ) {
safeDeletePoint ( pt) ;
return ;
}
pt - > delateFrameRef ( frame) ;
frame - > removeKeyPoint ( ftr) ;
}
void svo:: Map:: safeDeletePoint ( svo:: Point* pt)
{
std:: for_each ( pt - > obs_. begin ( ) , pt - > obs_. end ( ) , [ & ] ( Feature* ftr) {
ftr - > point = NULL ;
ftr - > frame - > removeKeyPoint ( ftr) ;
} ) ;
pt - > obs_. clear ( ) ;
deletePoint ( pt) ;
}
void svo:: Map:: deletePoint ( svo:: Point* pt)
{
pt - > type_ = Point:: TYPE_DELETED;
trash_points_. push_back ( pt) ;
}
void svo:: Map:: getCloseKeyframes ( const FramePtr& frame,
list< pair< FramePtr, double > > & close_kfs) const
{
for ( auto kf : keyframes_) {
for ( auto keypoint : kf - > key_pts_) {
if ( keypoint == nullptr ) continue ;
if ( frame - > isVisible ( keypoint - > point - > pos_) ) {
close_kfs. push_back ( std:: make_pair ( kf,
frame - > T_f_w_. translation ( ) -
kf - > T_f_w_. translation ( ) ) . norm ( ) ) ) ;
break ;
}
}
}
}
FramePtr svo:: Map:: getClosestKeyframes ( const FramePtr& frame) const
{
list< pair< FramePtr, double > > close_kfs;
getCloseKeyframes ( frame, close_kfs) ;
if ( close_kfs. empty ( ) ) { return nullptr ; }
close_kfs. sort ( boost:: bind ( & std:: pair< FramePtr, double > :: second, _1) <
boost:: bind ( & std:: pair< FramePtr, double > :: second, _2) ) ;
if ( close_kfs. front ( ) . first != frame) return close_kfs. front ( ) . first;
close_kfs. pop_front ( ) ;
return close_kfs. front ( ) . first;
}
FramePtr svo:: Map:: getFurthestKeyframe ( const Vector3d& pos) const
{
FramePtr furthest_kf;
double maxdist = 0.0 ;
for ( auto it = keyframes_. begin ( ) , ite = keyframes_. end ( ) ; it != ite; ++ it) {
double dist = ( ( * it) - > pos ( ) - pos) . norm ( ) ;
if ( dist > maxdist) {
maxdist = dist;
furthest_kf = * it;
}
}
return furthest_kf;
}
bool svo:: Map:: getKeyframeById ( const int id, FramePtr& frame) const
{
bool found = false ;
for ( auto it = keyframes_. begin ( ) , ite = keyframes_. end ( ) ; it != ite; ++ it) {
if ( ( * it) - > id_ == id) {
found = true ;
frame = * it;
break ;
}
}
return found;
}
void svo:: Map:: transform ( const Matrix3d& R, const Vector3d* t, const double & s)
{
for ( auto it = keyframes_. begin ( ) , ite = keyframes_. end ( ) ; it != ite; ++ it) {
Vector3d pos = s * R * ( * it) - > pos ( ) + t;
Matrix3d rot = R * ( * it) - > T_f_w_. rotation_matrix ( ) . inverse ( ) ;
( * it) - > T_f_w_ = SE3 ( rot, pos) . inverse ( ) ;
for ( auto ftr = ( * it) - > fts_. begin ( ) ; ftr != ( * it) - > fts_. end ( ) ; ++ ftr) {
if ( ( * ftr) - > point == NULL ) continue ;
if ( ( * ftr) - > point - > last_publish_ts_ = - 1000 ) continue ;
( * ftr) - > point - > last_publish_ts_ = - 1000 ;
( * ftr) - > point - > pos_ = s * R * ( * ftr) - > point - > pos_ + t;
}
}
}
void svo:: Map:: emptyTrash ( )
{
std:: for_each ( trash_points_. begin ( ) , trash_points_. end ( ) , [ & ] ( Point* pt) {
delete pt;
pt = NULL ;
} ) ;
trash_points_. clear ( ) ;
point_candidates_. emptyTrash ( ) ;
}
svo:: MapPointCandidates:: MapPointCandidates ( ) { }
svo:: MapPointCandidates:: ~ MapPointCandidates ( ) { }
void svo:: MapPointCandidates:: newCandidatePoint ( svo:: Point* point, double depth_sigma2)
{
point - > type_ = Point:: TYPE_CANDIDATE;
boost:: unique_lock< boost:: mutex> lock ( mut_) ;
candidates_. push_back ( PointCandidate ( point, point - > obs_. front ( ) ) ) ;
}
void svo:: MapPointCandidates:: addCandidatePointToFrame ( FramePtr frame)
{
boost:: unique_lock< boost:: mutex> lock ( mut_) ;
PointCandidateList:: iterator it = candidates_. begin ( ) ;
while ( it != candidates_. end ( ) ) {
if ( it - > first - > obs_. front ( ) - > frame == frame. get ( ) ) {
it - > first - > type_ = Point:: TYPE_UNKNOWN;
it - > first - > n_failed_reproj_ = 0 ;
it - > second - > frame - > addFeature ( it - > second) ;
it = candidates_. erase ( it) ;
} else {
++ it;
}
}
}
bool svo:: MapPointCandidates:: deleteCandidatePoint ( svo:: Point* point)
{
boost:: unique_lock< boost:: mutex> lock ( mut_) ;
for ( auto it = candidates_. begin ( ) , ite = candidates_. end ( ) ; it != ite; ++ it) {
if ( it - > first == point) {
deleteCandidate ( * it) ;
candidates_. erase ( it) ;
return true ;
}
}
return false ;
}
void svo:: MapPointCandidates:: removeFrameCandidates ( FramePtr frame)
{
boost:: unique_lock< boost:: mutex> lock ( mut_) ;
auto it = candidates_. begin ( ) ;
while ( it != candidates_. end ( ) ) {
if ( it - > second - > frame == frame. get ( ) ) {
deleteCandidate ( * it) ;
it = candidates_. earse ( it) ;
}
else {
++ it;
}
}
}
void svo:: MapPointCandidates:: reset ( )
{
boost:: unique_lock< boost:: mutex> lock ( mut_) ;
std:: for_each ( candidates_. begin ( ) , candidates_. end ( ) , [ & ] ( PointCandidate& c) {
delete c. first;
delete c. second;
} ) ;
candidates_. clear ( ) ;
}
void svo:: MapPointCandidates:: deleteCandidate ( PointCandidate& c)
{
delete c. second; c. second = NULL ;
c. first - > type_ = Point:: TYPE_DELETED;
trash_points_. push_back ( c. first) ;
}
void svo:: MapPointCandidates:: emptyTrash ( )
{
std:: for_each ( trash_points_. begin ( ) , trash_points_. end ( ) , [ & ] ( Point* & p) {
delete p; p = NULL ; } ) ;
trash_points_. clear ( ) ;
}
namespace map_debug{
void mapValidation ( Map* map, int id)
{
for ( auto it= map- > keyframes_. begin ( ) ; it!= map- > keyframes_. end ( ) ; ++ it)
frameValidation ( it- > get ( ) , id) ;
}
void frameValidation ( Frame* frame, int id)
{
for ( auto it = frame- > fts_. begin ( ) ; it!= frame- > fts_. end ( ) ; ++ it)
{
if ( ( * it) - > point== NULL )
continue ;
if ( ( * it) - > point- > type_ == Point:: TYPE_DELETED)
printf ( "ERROR DataValidation %i: Referenced point was deleted.\n" , id) ;
if ( ! ( * it) - > point- > findFrameRef ( frame) )
printf ( "ERROR DataValidation %i: Frame has reference but point does not have a reference back.\n" , id) ;
pointValidation ( ( * it) - > point, id) ;
}
for ( auto it= frame- > key_pts_. begin ( ) ; it!= frame- > key_pts_. end ( ) ; ++ it)
if ( * it != NULL )
if ( ( * it) - > point == NULL )
printf ( "ERROR DataValidation %i: KeyPoints not correct!\n" , id) ;
}
void pointValidation ( Point* point, int id) {
for ( auto it = point - > obs_. begin ( ) ; it != point - > obs_. end ( ) ; ++ it) {
bool found = false ;
for ( auto it_ftr= ( * it) - > frame- > fts_. begin ( ) ; it_ftr!= ( * it) - > frame- > fts_. end ( ) ; ++ it_ftr) {
if ( ( * it_ftr) - > point == point) {
found == true ; break ;
}
if ( ! found)
printf ( "ERROR DataValidation %i: Point %i has inconsistent reference in frame %i, is candidate = %i\n" ,
id, point- > id_, ( * it) - > frame- > id_, ( int ) point- > type_) ;
}
}
}
void mapStatistics ( Map* map) {
size_t n_pt_obs ( 0 ) ;
for ( auto it= map- > keyframes_. begin ( ) ; it!= map- > keyframes_. end ( ) ; ++ it)
n_pt_obs + = ( * it) - > nObs ( ) ;
printf ( "\n\nMap Statistics: Frame avg. point obs = %f\n" ,
( float ) n_pt_obs/ map- > size ( ) ) ;
size_t n_frame_obs ( 0 ) ;
size_t n_pts ( 0 ) ;
std:: set< Point* > points;
for ( auto it = map- > keyframes_. begin ( ) ; it!= map- > keyframes_. end ( ) ; ++ it) {
for ( auto ftr= ( * it) - > fts_. begin ( ) ; ftr!= ( * it) - > fts_. end ( ) ; ++ ftr) {
if ( ( * ftr) - > point == NULL ) continue ;
if ( points. insert ( ( * ftr) - > point) . second) {
++ n_pts;
n_frame_obs + = ( * ftr) - > point - > nRefs ( ) ;
}
}
}
printf ( "Map Statistics: Point avg. frame obs = %f\n\n" , ( float ) n_frame_obs/ n_pts) ;
}
}
}