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

change pointer into shared_ptr

Browse Source
This commit is contained in:
gfuse
2021-09-15 12:21:27 +02:00
parent 75a5555443
commit c7e98907d1
22 changed files with 30261 additions and 7280 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+28810 -6221
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+1313 -908
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/cl_class.h
+1
View File
@@ -51,6 +51,7 @@ public:
template<typename T>
int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
assert(buf);
for(auto&& i:_buffers)if(i.second==id){
cl_int error;
cl::Event event;
GPU_version/vio/include/vio/depth_filter.h
+7 -7
View File
@@ -39,14 +39,14 @@ struct Seed
static int seed_counter;
int batch_id; //!< Batch id is the id of the keyframe for which the seed was created.
int id; //!< Seed ID, only used for visualization.
Feature* ftr; //!< Feature in the keyframe for which the depth should be computed.
std::shared_ptr<Feature> ftr; //!< Feature in the keyframe for which the depth should be computed.
float a; //!< a of Beta distribution: When high, probability of inlier is large.
float b; //!< b of Beta distribution: When high, probability of outlier is large.
float mu; //!< Mean of normal distribution.
float z_range; //!< Max range of the possible depth.
float sigma2; //!< Variance of normal distribution.
Matrix2d patch_cov; //!< Patch covariance in reference image.
Seed(Feature* ftr, float depth_mean, float depth_min);
Seed(std::shared_ptr<Feature> ftr, float depth_mean, float depth_min);
};
/// Depth filter implements the Bayesian Update proposed in:
@@ -61,7 +61,7 @@ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef boost::unique_lock<boost::mutex> lock_t;
typedef boost::function<void ( Point*, double )> callback_t;
typedef boost::function<void ( std::shared_ptr<Point>, double )> callback_t;
/// Depth-filter config parameters
struct Options
@@ -115,17 +115,17 @@ public:
/// Can be used to compute the Next-Best-View in parallel.
/// IMPORTANT! Make sure you hold a valid reference counting pointer to frame
/// so it is not being deleted while you use it.
void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);
void getSeedsCopy(const FramePtr& frame, std::list<std::shared_ptr<Seed>>& seeds);
/// Return a reference to the seeds. This is NOT THREAD SAFE!
//std::list<Seed>& getSeeds() { return seeds_; } Bug 238
std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
std::list<std::shared_ptr<Seed>, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
/// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
static bool updateSeed(
const float x,
const float tau2,
Seed* seed);
std::shared_ptr<Seed> seed);
/// Compute the uncertainty of the measurement.
static double computeTau(
@@ -137,7 +137,7 @@ public:
protected:
feature_detection::DetectorPtr feature_detector_;
callback_t seed_converged_cb_;
std::list<Seed, Eigen::aligned_allocator<Seed>> seeds_;
std::list<std::shared_ptr<Seed>, Eigen::aligned_allocator<Seed>> seeds_;
boost::mutex seeds_mut_;
bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted.
boost::thread* thread_;
GPU_version/vio/include/vio/feature.h
+5 -7
View File
@@ -32,34 +32,32 @@ struct Feature
};
FeatureType type; //!< Type can be corner or edgelet.
Frame* frame; //!< Pointer to frame in which the feature was detected.
std::shared_ptr<Frame> frame; //!< Pointer to frame in which the feature was detected.
Vector2d px; //!< Coordinates in pixels on pyramid level 0.
Vector3d f; //!< Unit-bearing vector of the feature.
int level; //!< Image pyramid level where feature was extracted.
Point* point; //!< Pointer to 3D point which corresponds to the feature.
std::shared_ptr<Point> point; //!< Pointer to 3D point which corresponds to the feature.
Vector2d grad; //!< Dominant gradient direction for edglets, normalized.
Feature(Frame* _frame, const Vector2d& _px, int _level) :
Feature(std::shared_ptr<Frame> _frame, const Vector2d& _px, int _level) :
type(CORNER),
frame(_frame),
px(_px),
f(frame->cam_->cam2world(px)),
level(_level),
point(NULL),
grad(1.0,0.0)
{}
Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
Feature(std::shared_ptr<Frame> _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
type(CORNER),
frame(_frame),
px(_px),
f(_f),
level(_level),
point(NULL),
grad(1.0,0.0)
{}
Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
Feature(std::shared_ptr<Frame> _frame, std::shared_ptr<Point> _point, const Vector2d& _px, const Vector3d& _f, int _level) :
type(CORNER),
frame(_frame),
px(_px),
GPU_version/vio/include/vio/feature_detection.h
+2 -2
View File
@@ -57,7 +57,7 @@ public:
};
virtual void detect(
Frame* frame,
std::shared_ptr<Frame> frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts,
@@ -101,7 +101,7 @@ public:
virtual ~FastDetector() {}
virtual void detect(
Frame* frame,
std::shared_ptr<Frame> frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts,
GPU_version/vio/include/vio/frame.h
+5 -5
View File
@@ -33,7 +33,7 @@ namespace vio {
class Point;
struct Feature;
typedef list<Feature*> Features;
typedef list<std::shared_ptr<Feature>> Features;
typedef vector<cv::Mat> ImgPyr;
@@ -51,7 +51,7 @@ namespace vio {
Matrix<double, 3, 3> Cov_; //!< Covariance.
ImgPyr img_pyr_; //!< Image Pyramid.
Features fts_; //!< List of features in the image.
vector<Feature*> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
vector<std::shared_ptr<Feature>> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
bool is_keyframe_; //!< Was this frames selected as keyframe?
g2oFrameSE3* v_kf_; //!< Temporary pointer to the g2o node object of the keyframe.
int last_published_ts_; //!< Timestamp of last publishing.
@@ -66,7 +66,7 @@ namespace vio {
void setKeyframe();
/// Add a feature to the image
void addFeature(Feature* ftr);
void addFeature(std::shared_ptr<Feature> ftr);
/// The KeyPoints are those five features which are closest to the 4 image corners
/// and to the center and which have a 3D point assigned. These points are used
@@ -74,10 +74,10 @@ namespace vio {
void setKeyPoints();
/// Check if we can select five better key-points.
void checkKeyPoints(Feature* ftr);
void checkKeyPoints(std::shared_ptr<Feature> ftr);
/// If a point is deleted, we must remove the corresponding key-point.
void removeKeyPoint(Feature* ftr);
void removeKeyPoint(std::shared_ptr<Feature> ftr);
/// Return number of point observations.
inline size_t nObs() const { return fts_.size(); }
GPU_version/vio/include/vio/global.h
+2 -3
View File
@@ -45,6 +45,7 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
namespace vio
{
using namespace Eigen;
using namespace Sophus;
@@ -128,8 +129,6 @@ namespace vio
SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
return tem;
}
~SE2_5(){
}
bool empty() const{
return T2_ == nullptr ? true : false;
}
@@ -139,7 +138,7 @@ namespace vio
};
class Frame;
typedef boost::shared_ptr<Frame> FramePtr;
typedef std::shared_ptr<Frame> FramePtr;
} // namespace vio
#endif // SVO_GLOBAL_H_
GPU_version/vio/include/vio/map.h
+10 -10
View File
@@ -32,7 +32,7 @@ class Seed;
class MapPointCandidates
{
public:
typedef pair<Point*, Feature*> PointCandidate;
typedef pair<std::shared_ptr<Point>, std::shared_ptr<Feature>> PointCandidate;
typedef list<PointCandidate> PointCandidateList;
/// The depth-filter is running in a parallel thread and fills the canidate list.
@@ -42,19 +42,19 @@ public:
/// Candidate points are created from converged seeds.
/// Until the next keyframe, these points can be used for reprojection and pose optimization.
PointCandidateList candidates_;
list< Point* > trash_points_;
list< std::shared_ptr<Point> > trash_points_;
MapPointCandidates();
~MapPointCandidates();
/// Add a candidate point.
void newCandidatePoint(Point* point, double depth_sigma2);
void newCandidatePoint(std::shared_ptr<Point> point, double depth_sigma2);
/// Adds the feature to the frame and deletes candidate from list.
void addCandidatePointToFrame(FramePtr frame);
/// Remove a candidate point from the list of candidates.
bool deleteCandidatePoint(Point* point);
bool deleteCandidatePoint(std::shared_ptr<Point> point);
/// Remove all candidates that belong to a frame.
void removeFrameCandidates(FramePtr frame);
@@ -72,7 +72,7 @@ class Map : boost::noncopyable
{
public:
list< FramePtr > keyframes_; //!< List of keyframes in the map.
list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
list< std::shared_ptr<Point> > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
MapPointCandidates point_candidates_;
@@ -83,16 +83,16 @@ public:
void reset();
/// Delete a point in the map and remove all references in keyframes to it.
void safeDeletePoint(Point* pt);
void safeDeletePoint(std::shared_ptr<Point> pt);
/// Moves the point to the trash queue which is cleaned now and then.
void deletePoint(Point* pt);
void deletePoint(std::shared_ptr<Point> pt);
/// Moves the frame to the trash queue which is cleaned now and then.
bool safeDeleteFrame(FramePtr frame);
/// Remove the references between a point and a frame.
void removePtFrameRef(Frame* frame, Feature* ftr);
void removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr);
/// Add a new keyframe to the map.
void addKeyframe(FramePtr new_keyframe);
@@ -128,8 +128,8 @@ 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);
void frameValidation(FramePtr frame, int id);
void pointValidation(std::shared_ptr<Point> point, int id);
} // namespace map_debug
} // namespace vio
GPU_version/vio/include/vio/matcher.h
+1 -1
View File
@@ -99,7 +99,7 @@ public:
double h_inv_; //!< hessian of 1d image alignment along epipolar line
int search_level_;
bool reject_;
Feature* ref_ftr_;
std::shared_ptr<Feature> ref_ftr_;
Vector2d px_cur_;
Matcher() = default;
GPU_version/vio/include/vio/point.h
+6 -6
View File
@@ -53,7 +53,7 @@ public:
Vector3d normal_; //!< Surface normal at point.
Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation.
bool normal_set_; //!< Flag whether the surface normal was estimated or not.
list<Feature*> obs_; //!< References to keyframes which observe the point.
list<std::shared_ptr<Feature>> obs_; //!< References to keyframes which observe the point.
size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
int last_published_ts_; //!< Timestamp of last publishing.
@@ -64,23 +64,23 @@ public:
int last_structure_optim_; //!< Timestamp of last point optimization
Point(const Vector3d& pos);
Point(const Vector3d& pos, Feature* ftr);
Point(const Vector3d& pos, std::shared_ptr<Feature> ftr);
~Point();
/// Add a reference to a frame.
void addFrameRef(Feature* ftr);
void addFrameRef(std::shared_ptr<Feature> ftr);
/// Remove reference to a frame.
bool deleteFrameRef(Frame* frame);
bool deleteFrameRef(FramePtr frame);
/// Initialize point normal. The inital estimate will point towards the frame.
void initNormal();
/// Check whether mappoint has reference to a frame.
Feature* findFrameRef(Frame* frame);
std::shared_ptr<Feature> findFrameRef(FramePtr frame);
/// Get Frame with similar viewpoint.
bool getCloseViewObs(const Vector2d& pos, Feature*& obs, int id=0) const;
bool getCloseViewObs(const Vector2d& pos, std::shared_ptr<Feature>& obs, int id=0) const;
/// Get number of observations.
inline size_t nRefs() const { return obs_.size(); }
GPU_version/vio/include/vio/reprojector.h
+3 -3
View File
@@ -79,9 +79,9 @@ private:
/// will search a maching feature in the image.
struct Candidate {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Point* pt; //!< 3D point.
std::shared_ptr<Point> pt; //!< 3D point.
Vector2d px; //!< projected 2D pixel location.
Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
Candidate(std::shared_ptr<Point> pt, Vector2d& px) : pt(pt), px(px) {}
};
typedef std::list<Candidate > Cell;
typedef std::vector<Cell*> CandidateGrid;
@@ -104,7 +104,7 @@ private:
void initializeGrid(vk::AbstractCamera* cam);
void resetGrid();
bool reprojectCell(Cell& cell, FramePtr frame);
bool reprojectPoint(FramePtr frame, Point* point);
bool reprojectPoint(FramePtr frame, std::shared_ptr<Point> point);
};
} // namespace vio
GPU_version/vio/include/vio/visualizer.h
+1 -1
View File
@@ -35,7 +35,7 @@ class Point;
class Map;
class FrameHandlerMono;
typedef boost::shared_ptr<Frame> FramePtr;
typedef std::shared_ptr<Frame> FramePtr;
/// This class bundles all functions to publish visualisation messages.
class Visualizer
GPU_version/vio/src/depth_filter.cpp
+25 -27
View File
@@ -37,7 +37,7 @@ namespace vio {
int Seed::batch_counter = 0;
int Seed::seed_counter = 0;
Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
Seed::Seed(std::shared_ptr<Feature> ftr, float depth_mean, float depth_min) :
batch_id(batch_counter),
id(seed_counter++),
ftr(ftr),
@@ -148,16 +148,14 @@ namespace vio {
fprintf(log_,"[%s] init seeds\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
feature_detector_->setExistingFeatures(frame->fts_);
feature_detector_->detect(frame.get(), frame->img_pyr_,
feature_detector_->detect(frame, frame->img_pyr_,
Config::triangMinCornerScore(), new_features);
// initialize a seed for every new feature
seeds_updating_halt_ = true;
seeds_mut_.lock();
++Seed::batch_counter;
std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
});
for(auto&& ftr:new_features)seeds_.push_back(make_shared<Seed>(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
#if VIO_DEBUG
fprintf(log_,"[%s] DepthFilter: Initialized %d new seeds\n",vio::time_in_HH_MM_SS_MMM().c_str(),new_features.size());
@@ -173,11 +171,11 @@ namespace vio {
fprintf(log_,"[%s] remove key frame\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
seeds_mut_.lock();
std::list<Seed>::iterator it=seeds_.begin();
std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin();
size_t n_removed = 0;
while(it!=seeds_.end())
{
if(it->ftr->frame->id_ == frame->id_)
if((*it)->ftr->frame->id_ == frame->id_)
{
it = seeds_.erase(it);
++n_removed;
@@ -240,7 +238,7 @@ namespace vio {
// for all the seeds in every frame!
size_t n_updates=0, n_failed_matches=0;
lock_t lock(seeds_mut_);
std::list<Seed>::iterator it=seeds_.begin();
std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin();
#if VIO_DEBUG
fprintf(log_,"[%s] update seed\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
@@ -253,19 +251,19 @@ namespace vio {
if(seeds_updating_halt_)
return;
if(it->ftr->frame == NULL){
if((*it)->ftr->frame == NULL){
it = seeds_.erase(it);
continue;
}
// check if point is visible in the current image
SE2 T(frame->T_f_w_.pitch()-it->ftr->frame->T_f_w_.pitch(),frame->T_f_w_.se2().translation()+it->ftr->frame->T_f_w_.inverse().translation());
SE2 T(frame->T_f_w_.pitch()-(*it)->ftr->frame->T_f_w_.pitch(),frame->T_f_w_.se2().translation()+(*it)->ftr->frame->T_f_w_.inverse().translation());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(0.0, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_cur_ref(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
const Vector3d xyz_f(T_cur_ref*(1.0/it->mu * it->ftr->f) );
const Vector3d xyz_f(T_cur_ref*(1.0/(*it)->mu * (*it)->ftr->f) );
#if VIO_DEBUG
fprintf(log_,"[%s] If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
#endif
@@ -278,8 +276,8 @@ namespace vio {
continue;
}
float z_min = it->mu + sqrt(it->sigma2);
float z_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
float z_min = (*it)->mu + sqrt((*it)->sigma2);
float z_max = max((*it)->mu - sqrt((*it)->sigma2), 0.00000001f);
if(z_min < 0){
++it;
@@ -288,18 +286,18 @@ namespace vio {
double z;
if(!matcher_.findEpipolarMatchDirect(
*it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_min, 1.0/z_max, z))
*(*it)->ftr->frame, *frame, *(*it)->ftr, 1.0/(*it)->mu, 1.0/z_min, 1.0/z_max, z))
{
it->b++; // increase outlier probability when no match was found
(*it)->b++; // increase outlier probability when no match was found
++it;
++n_failed_matches;
continue;
}
double tau = computeTau(T_cur_ref.inverse(), it->ftr->f, z, px_error_angle);
double tau = computeTau(T_cur_ref.inverse(), (*it)->ftr->f, z, px_error_angle);
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
// update the estimate
if(!updateSeed(1.0/z, tau_inverse*tau_inverse, &*it)){
if(!updateSeed(1.0/z, tau_inverse*tau_inverse, *it)){
++it;
++n_failed_matches;
continue;
@@ -313,13 +311,13 @@ namespace vio {
}
// if the seed has converged, we initialize a new candidate point and remove the seed
if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
if(sqrt((*it)->sigma2) < (*it)->z_range/options_.seed_convergence_sigma2_thresh)
{
assert(it->ftr->point == NULL); // TODO this should not happen anymore
Vector3d xyz_world(it->ftr->frame->getSE3Inv() * (it->ftr->f/it->mu));
Point* point = new Point(xyz_world, it->ftr);
it->ftr->point = point;
seed_converged_cb_(point, it->sigma2); // put in candidate list
assert((*it)->ftr->point == NULL); // TODO this should not happen anymore
Vector3d xyz_world((*it)->ftr->frame->getSE3Inv() * ((*it)->ftr->f/(*it)->mu));
std::shared_ptr<Point> point = std::make_shared<Point>(xyz_world, (*it)->ftr);
(*it)->ftr->point = point;
seed_converged_cb_(point, (*it)->sigma2); // put in candidate list
it = seeds_.erase(it);
}
else if(isnan(z_min))
@@ -337,16 +335,16 @@ namespace vio {
frame_queue_.pop();
}
void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds)
void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<std::shared_ptr<Seed>>& seeds)
{
lock_t lock(seeds_mut_);
for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
for(std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
{
if (it->ftr->frame->id_ == frame->id_)
if ((*it)->ftr->frame->id_ == frame->id_)
seeds.push_back(*it);
}
}
bool DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
bool DepthFilter::updateSeed(const float x, const float tau2, std::shared_ptr<Seed> seed)
{
float norm_scale = sqrt(seed->sigma2 + tau2);
if(std::isnan(norm_scale))
GPU_version/vio/src/feature_detection.cpp
+4 -4
View File
@@ -41,11 +41,11 @@ void AbstractDetector::resetGrid()
void AbstractDetector::setExistingFeatures(const Features& fts)
{
std::for_each(fts.begin(), fts.end(), [&](Feature* i){
for(auto&& i:fts){
grid_occupancy_.at(
static_cast<int>(i->px[1]/cell_size_)*grid_n_cols_
+ static_cast<int>(i->px[0]/cell_size_)) = true;
});
};
}
void AbstractDetector::setGridOccpuancy(const Vector2d& px)
@@ -66,7 +66,7 @@ FastDetector::FastDetector(
}
void FastDetector::detect(
Frame* frame,
std::shared_ptr<Frame> frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts,
@@ -102,7 +102,7 @@ void FastDetector::detect(
corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
// Create feature for every corner that has high enough corner score
if(corners.at(k).score > detection_threshold){
fts.push_back(new Feature(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x, fast_corners[i].y, 1));
}
}
GPU_version/vio/src/frame.cpp
+6 -7
View File
@@ -40,7 +40,7 @@ Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) :
Frame::~Frame()
{
std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;});
for(auto&& f:fts_)f.reset();
}
void Frame::initFrame(const cv::Mat& img)
@@ -50,7 +50,7 @@ void Frame::initFrame(const cv::Mat& img)
throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");
// Set keypoints to NULL
std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; });
for(auto&& ftr:key_pts_)ftr.reset();
// Build Image Pyramid
frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
@@ -62,7 +62,7 @@ void Frame::setKeyframe()
setKeyPoints();
}
void Frame::addFeature(Feature* ftr)
void Frame::addFeature(std::shared_ptr<Feature> ftr)
{
fts_.push_back(ftr);
}
@@ -73,11 +73,10 @@ void Frame::setKeyPoints()
if(key_pts_[i] != NULL)
if(key_pts_[i]->point == NULL)
key_pts_[i] = NULL;
std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); });
for(auto&& ftr:fts_)if(ftr->point != NULL) checkKeyPoints(ftr);
}
void Frame::checkKeyPoints(Feature* ftr)
void Frame::checkKeyPoints(std::shared_ptr<Feature> ftr)
{
const int cu = cam_->width()/2;
const int cv = cam_->height()/2;
@@ -123,7 +122,7 @@ void Frame::checkKeyPoints(Feature* ftr)
}
}
void Frame::removeKeyPoint(Feature* ftr)
void Frame::removeKeyPoint(std::shared_ptr<Feature> ftr)
{
bool found = false;
if(key_pts_.size()!=5 ){
GPU_version/vio/src/frame_handler_base.cpp
+4 -5
View File
@@ -123,16 +123,15 @@ void FrameHandlerBase::optimizeStructure(
size_t max_n_pts,
int max_iter)
{
deque<Point*> pts;
deque<std::shared_ptr<Point>> pts;
for(auto&& it:frame->fts_){
if(it->point!=NULL)pts.push_back(it->point);
}
//max_n_pts = min(max_n_pts, pts.size());
//nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
for(deque<Point*>::iterator it=pts.begin(); it!=pts.begin(); ++it)
{
(*it)->optimize(max_iter);
(*it)->last_structure_optim_ = frame->id_;
for(auto&& pt:pts){
pt->optimize(max_iter);
pt->last_frame_overlap_id_= frame->id_;
}
}
GPU_version/vio/src/frame_handler_mono.cpp
+2 -3
View File
@@ -81,8 +81,7 @@ void FrameHandlerMono::initialize()
feature_detection::DetectorPtr feature_detector(
new feature_detection::FastDetector(
cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels()));
DepthFilter::callback_t depth_filter_cb = boost::bind(
&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
DepthFilter::callback_t depth_filter_cb = boost::bind(&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
depth_filter_->startThread();
}
@@ -103,7 +102,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
overlap_kfs_.clear();
// create new frame
new_frame_=boost::make_shared<Frame>(cam_, img.clone(), timestamp);
new_frame_=std::make_shared<Frame>(cam_, img.clone(), timestamp);
time_=time;
// process frame
UpdateResult res = RESULT_FAILURE;
GPU_version/vio/src/initialization.cpp
+5 -5
View File
@@ -89,13 +89,13 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
if(frame_cur->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10))
{
Vector3d pos = frame_cur->getSE3Inv() * (xyz_in_cur_[*it]/* *scale */);
Point* new_point = new Point(pos);
std::shared_ptr<Point> new_point = std::make_shared<Point>(pos);
Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, px_cur, f_cur_[*it], 0);
frame_cur->addFeature(ftr_cur);
new_point->addFrameRef(ftr_cur);
Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
std::shared_ptr<Feature> ftr_ref=std::make_shared<Feature>(frame_ref_, new_point, px_ref, f_ref_[*it], 0);
frame_ref_->addFeature(ftr_ref);
new_point->addFrameRef(ftr_ref);
}
@@ -127,14 +127,14 @@ void detectFeatures(
Features new_features;
feature_detection::FastDetector detector(
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
// now for all maximum corners, initialize a new seed
px_vec.clear();
f_vec.clear();
for(auto&& ftr:new_features){
px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
f_vec.push_back(ftr->f);
delete ftr;
ftr.reset();
}
}
void trackKlt(
GPU_version/vio/src/map.cpp
+35 -42
View File
@@ -20,6 +20,7 @@
#include <vio/frame.h>
#include <vio/feature.h>
#include <boost/bind.hpp>
#include <vio/for_it.hpp>
namespace vio {
@@ -28,7 +29,6 @@ Map::Map() {}
Map::~Map()
{
reset();
SVO_INFO_STREAM("Map destructed");
}
void Map::reset()
@@ -42,19 +42,18 @@ bool Map::safeDeleteFrame(FramePtr frame)
{
if(syn_==true)return false;
bool found = false;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
{
if(it->get()->id_ == frame->id_)
{
std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){
removePtFrameRef(it->get(), ftr);
});
keyframes_.erase(it);
found = true;
break;
}
size_t position;
for(auto&& keyframe:_for(keyframes_)){
if(keyframe.item->id_==frame->id_){
for(auto&& fts:keyframe.item->fts_)removePtFrameRef(keyframe.item, fts);
found = true;
position=keyframe.index;
break;
}
}
std::list<std::shared_ptr<Frame>>::iterator left=keyframes_.begin();
std::advance(left,position);
keyframes_.erase(left);
point_candidates_.removeFrameCandidates(frame);
if(found)
@@ -64,40 +63,37 @@ bool Map::safeDeleteFrame(FramePtr frame)
return false;
}
void Map::removePtFrameRef(Frame* frame, Feature* ftr)
void Map::removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr)
{
if(ftr->point == NULL)
return; // mappoint may have been deleted in a previous ref. removal
Point* pt = ftr->point;
delete ftr->point;
if(pt->obs_.size() <= 2)
if(ftr->point->obs_.size() <= 2)
{
// If the references list of mappoint has only size=2, delete mappoint
safeDeletePoint(pt);
safeDeletePoint(ftr->point);
return;
}
pt->deleteFrameRef(frame); // Remove reference from map_point
ftr->point->deleteFrameRef(frame); // Remove reference from map_point
frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe
}
void Map::safeDeletePoint(Point* pt)
void Map::safeDeletePoint(std::shared_ptr<Point> pt)
{
// Delete references to mappoints in all keyframes
if(pt->obs_.size()>1){
for(auto&& ftr:pt->obs_){
ftr->frame->removeKeyPoint(ftr);
delete ftr->point;
if(ftr->frame)ftr->frame->removeKeyPoint(ftr);
ftr->point.reset();
}
}else{
delete pt->obs_.back()->point;
pt->obs_.back()->point.reset();
}
pt->obs_.clear();
// Delete mappoint
deletePoint(pt);
}
void Map::deletePoint(Point* pt)
void Map::deletePoint(std::shared_ptr<Point> pt)
{
pt->type_ = Point::TYPE_DELETED;
trash_points_.push_back(pt);
@@ -210,9 +206,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s)
void Map::emptyTrash()
{
std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){
delete pt;
});
for(auto&& t:trash_points_)t.reset();
trash_points_.clear();
point_candidates_.emptyTrash();
}
@@ -225,7 +219,7 @@ MapPointCandidates::~MapPointCandidates()
reset();
}
void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2)
void MapPointCandidates::newCandidatePoint(std::shared_ptr<Point> point, double depth_sigma2)
{
point->type_ = Point::TYPE_CANDIDATE;
boost::unique_lock<boost::mutex> lock(mut_);
@@ -238,7 +232,7 @@ void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
PointCandidateList::iterator it=candidates_.begin();
while(it != candidates_.end())
{
if(it->first->obs_.front()->frame == frame.get())
if(it->first->obs_.front()->frame == frame)
{
// insert feature in the frame
it->first->type_ = Point::TYPE_UNKNOWN;
@@ -251,7 +245,7 @@ void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
}
}
bool MapPointCandidates::deleteCandidatePoint(Point* point)
bool MapPointCandidates::deleteCandidatePoint(std::shared_ptr<Point> point)
{
boost::unique_lock<boost::mutex> lock(mut_);
for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it)
@@ -286,8 +280,8 @@ void MapPointCandidates::reset()
{
boost::unique_lock<boost::mutex> lock(mut_);
std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){
delete c.first;
delete c.second;
c.first.reset();
c.second.reset();
});
candidates_.clear();
}
@@ -296,16 +290,14 @@ void MapPointCandidates::deleteCandidate(PointCandidate& c)
{
// camera-rig: another frame might still be pointing to the candidate point
// therefore, we can't delete it right now.
delete c.second;
c.second.reset();
c.first->type_ = Point::TYPE_DELETED;
trash_points_.push_back(c.first);
}
void MapPointCandidates::emptyTrash()
{
std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){
delete p;
});
for(auto&& point:trash_points_)point.reset();
trash_points_.clear();
}
@@ -313,11 +305,12 @@ namespace map_debug {
void mapValidation(Map* map, int id)
{
for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
frameValidation(it->get(), id);
for(auto&& it:map->keyframes_)
frameValidation(it, id);
}
void frameValidation(Frame* frame, int id)
void frameValidation(FramePtr frame, int id)
{
for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
@@ -338,7 +331,7 @@ void frameValidation(Frame* frame, int id)
printf("ERROR DataValidation %i: KeyPoints not correct!\n", id);
}
void pointValidation(Point* point, int id)
void pointValidation(std::shared_ptr<Point> point, int id)
{
for(auto it=point->obs_.begin(); it!=point->obs_.end(); ++it)
{
@@ -363,7 +356,7 @@ void mapStatistics(Map* map)
// compute average number of observations that each point has
size_t n_frame_obs(0);
size_t n_pts(0);
std::set<Point*> points;
std::set<std::shared_ptr<Point>> points;
for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
{
for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
GPU_version/vio/src/point.cpp
+6 -6
View File
@@ -38,7 +38,7 @@ Point::Point(const Vector3d& pos) :
last_structure_optim_(0)
{}
Point::Point(const Vector3d& pos, Feature* ftr) :
Point::Point(const Vector3d& pos, std::shared_ptr<Feature> ftr) :
id_(point_counter_++),
pos_(pos),
normal_set_(false),
@@ -57,14 +57,14 @@ Point::Point(const Vector3d& pos, Feature* ftr) :
Point::~Point()
{}
void Point::addFrameRef(Feature* ftr)
void Point::addFrameRef(std::shared_ptr<Feature> ftr)
{
obs_.push_front(ftr);
++n_obs_;
}
Feature* Point::findFrameRef(Frame* frame)
std::shared_ptr<Feature> Point::findFrameRef(FramePtr frame)
{
boost::unique_lock<boost::mutex> lock(point_mut_);
for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
@@ -73,7 +73,7 @@ Feature* Point::findFrameRef(Frame* frame)
return NULL; // no keyframe found
}
bool Point::deleteFrameRef(Frame* frame)
bool Point::deleteFrameRef(FramePtr frame)
{
boost::unique_lock<boost::mutex> lock(point_mut_);
for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
@@ -94,7 +94,7 @@ bool Point::deleteFrameRef(Frame* frame)
void Point::initNormal()
{
assert(!obs_.empty());
const Feature* ftr = obs_.back();
const std::shared_ptr<Feature> ftr = obs_.back();
assert(ftr->frame != NULL);
normal_ = ftr->frame->se3().rotation_matrix().transpose()*(-ftr->f);
double x=pow(20/(pos_-Vector3d(ftr->frame->pos()(0),0.0,ftr->frame->pos()(1))).norm(),2);
@@ -102,7 +102,7 @@ void Point::initNormal()
normal_set_ = true;
}
bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) const
bool Point::getCloseViewObs(const Vector2d& framepos, std::shared_ptr<Feature>& ftr,int id) const
{
boost::unique_lock<boost::mutex> lock(point_mut_);
// TODO: get frame with same point of view AND same pyramid level!
GPU_version/vio/src/reprojector.cpp
+8 -7
View File
@@ -78,7 +78,7 @@ namespace vio {
cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
feature_detection::FastDetector detector(
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
list<pair<FramePtr, double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
if (!last_frame->fts_.empty())
@@ -103,7 +103,7 @@ namespace vio {
for (auto &&match:matches) {
if (match.queryIdx != f.index)continue;
if(keypoints_cur.size() < match.trainIdx)continue;
list<Feature*>::iterator point=keypoints_cur.begin();
list<std::shared_ptr<Feature>>::iterator point=keypoints_cur.begin();
std::advance(point,match.trainIdx);
if(!(*point))continue;
if (f.item->point == NULL){
@@ -113,9 +113,10 @@ namespace vio {
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) (*point)->px.x(),
(int) (*point)->px.y());
frame->fts_.push_back(new Feature(it_frame.item.first.get(),
new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
px,it_frame.item.first->cam_->cam2world(f.item->px),(*point)->level));
frame->fts_.push_back(std::make_shared<Feature>(it_frame.item.first,
std::make_shared<Point>(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
px,
it_frame.item.first->cam_->cam2world(f.item->px),(*point)->level));
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++;
@@ -187,7 +188,7 @@ namespace vio {
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::TYPE_GOOD;
Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
std::shared_ptr<Feature> new_feature = std::make_shared<Feature>(frame, it->px, matcher_.search_level_);
frame->addFeature(new_feature);
// Here we add a reference in the feature to the 3D point, the other way
// round is only done if this frame is selected as keyframe.
@@ -215,7 +216,7 @@ namespace vio {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO original implementation
bool Reprojector::reprojectPoint(FramePtr frame, Point *point) {
bool Reprojector::reprojectPoint(FramePtr frame, std::shared_ptr<Point> point) {
Vector2d px(frame->w2c(point->pos_));
if (frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{